TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

RedCrab: Make drivetrain use velocity instead of raw power

Browse Source
This commit is contained in:
Cyberarm
2023-12-20 11:09:54 -06:00
parent d2200fcc0a
commit b83aec0a14
4 changed files with 57 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -36,6 +36,8 @@ public class RedCrabMinibot {
/// TUNING CONSTANTS ///
public static double DRIVETRAIN_MAX_SPEED = 0.5;
public static double DRIVETRAIN_VELOCITY_MAX_MM = 610;
public static double DRIVETRAIN_VELOCITY_SLOW_MM = 250;
public static double DRIVETRAIN_GEAR_RATIO = 13.0321;
public static int DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION = 28;
public static double DRIVETRAIN_WHEEL_DIAMETER_MM = 90.0;
@@ -170,6 +172,12 @@ public class RedCrabMinibot {
backLeft.setDistancePerPulse(distancePerTick);
backRight.setDistancePerPulse(distancePerTick);
/// --- RUN MODE
frontLeft.setRunMode(Motor.RunMode.VelocityControl);
frontRight.setRunMode(Motor.RunMode.VelocityControl);
backLeft.setRunMode(Motor.RunMode.VelocityControl);
backRight.setRunMode(Motor.RunMode.VelocityControl);
/// WINCH ///
/// ------------------------------------------------------------------------------------ ///
winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ??
@@ -236,6 +244,8 @@ public class RedCrabMinibot {
private void loadConstants() {
/// Drivetrain
RedCrabMinibot.DRIVETRAIN_MAX_SPEED = config.variable("Robot", "Drivetrain_Tuning", "max_speed").value();
RedCrabMinibot.DRIVETRAIN_VELOCITY_MAX_MM = config.variable("Robot", "Drivetrain_Tuning", "velocity_max_in_mm").value();
RedCrabMinibot.DRIVETRAIN_VELOCITY_SLOW_MM = config.variable("Robot", "Drivetrain_Tuning", "velocity_slow_in_mm").value();
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO = config.variable("Robot", "Drivetrain_Tuning", "gear_ratio").value();
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value();
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value();

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json
View File

File diff suppressed because one or more lines are too long

54
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java
View File

@@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class Move extends CyberarmState {
private final RedCrabMinibot robot;
private final String groupName, actionName;
private final double distanceMM, lerpMM_UP, lerpMM_DOWN, maxPower, minPower, toleranceMM;
private final double distanceMM, lerpMM_UP, lerpMM_DOWN, maxVelocityMM, minVelocityMM, toleranceMM;
private boolean strafe = false;
private final int timeoutMS;
private double initialHeadingDegrees = 1024.0;
@@ -23,8 +23,8 @@ public class Move extends CyberarmState {
this.lerpMM_DOWN = robot.config.variable(groupName, actionName, "lerpMM_DOWN").value();
this.toleranceMM = robot.config.variable(groupName, actionName, "toleranceMM").value();
this.maxPower = robot.config.variable(groupName, actionName, "maxPower").value();
this.minPower = robot.config.variable(groupName, actionName, "minPower").value();
this.maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
this.minVelocityMM = robot.config.variable(groupName, actionName, "minVelocityMM").value();
this.strafe = robot.config.variable(groupName, actionName, "strafe").value();
@@ -100,22 +100,22 @@ public class Move extends CyberarmState {
private void tankMove(){
double travelledDistance = Math.abs(robot.frontLeft.getDistance());
double power = lerpPower(travelledDistance);
double velocity = lerpVelocity(travelledDistance);
double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
double leftPower = power;
double rightPower = power;
double leftVelocity = velocity;
double rightVelocity = velocity;
// use +10% of power at 7 degrees of error to correct angle
double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power * 0.1);
double correctiveVelocity = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (velocity * 0.1);
if (angleDiff > -0.5) {
leftPower += correctivePower;
leftVelocity += correctiveVelocity;
} else if (angleDiff < 0.5) {
rightPower += correctivePower;
rightVelocity += correctiveVelocity;
}
robot.left.set(leftPower);
robot.right.set(rightPower);
robot.left.set(leftVelocity);
robot.right.set(rightVelocity);
if (runTime() >= timeoutMS ||
(robot.frontLeft.atTargetPosition() || robot.frontRight.atTargetPosition()) ||
@@ -129,24 +129,24 @@ public class Move extends CyberarmState {
private void strafeMove() {
double travelledDistance = Math.abs(robot.frontLeft.getDistance());
double power = lerpPower(travelledDistance);
double velocity = lerpVelocity(travelledDistance);
double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
double frontPower = power;
double backPower = power;
double frontVelocity = velocity;
double backVelocity = velocity;
// use +40% of power at 7 degrees of error to correct angle
double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power * 0.40);
double correctiveVelocity = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (velocity * 0.40);
if (angleDiff > -0.5) {
frontPower += correctivePower;
frontVelocity += correctiveVelocity;
} else if (angleDiff < 0.5) {
backPower += correctivePower;
backVelocity += correctiveVelocity;
}
robot.frontLeft.set(frontPower);
robot.frontRight.set(-frontPower);
robot.backLeft.set(-backPower);
robot.backRight.set(backPower);
robot.frontLeft.set(frontVelocity);
robot.frontRight.set(-frontVelocity);
robot.backLeft.set(-backVelocity);
robot.backRight.set(backVelocity);
if (runTime() >= timeoutMS || (robot.frontLeft.atTargetPosition() || robot.backRight.atTargetPosition()) ||
Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM) || Math.abs(robot.backRight.getDistance()) >= Math.abs(distanceMM)) {
@@ -159,20 +159,20 @@ public class Move extends CyberarmState {
}
}
private double lerpPower(double travelledDistance) {
double lerpPower = maxPower;
private double lerpVelocity(double travelledDistance) {
double lerpVelocity = maxVelocityMM;
// Ease power up
if (travelledDistance < lerpMM_UP) { // Not using <= to prevent divide by zero
lerpPower = Utilities.lerp(minPower, maxPower, Range.clip(travelledDistance / lerpMM_UP, 0.0, 1.0));
lerpVelocity = Utilities.lerp(minVelocityMM, maxVelocityMM, Range.clip(travelledDistance / lerpMM_UP, 0.0, 1.0));
// Cruising power
} else if (travelledDistance < Math.abs(distanceMM) - lerpMM_DOWN) {
lerpPower = maxPower;
lerpVelocity = maxVelocityMM;
// Ease power down
} else {
lerpPower = Utilities.lerp(minPower, maxPower, Range.clip( (Math.abs(distanceMM) - travelledDistance) / lerpMM_DOWN, 0.0, 1.0));
lerpVelocity = Utilities.lerp(minVelocityMM, maxVelocityMM, Range.clip( (Math.abs(distanceMM) - travelledDistance) / lerpMM_DOWN, 0.0, 1.0));
}
return lerpPower;
return lerpVelocity;
}
}

24
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java
View File

@@ -3,6 +3,7 @@ package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
@@ -18,6 +19,7 @@ public class Pilot extends CyberarmState {
private boolean droneLaunchAuthorized = false;
private boolean droneLaunchRequested = false;
private double droneLastLaunchRequestStartMS = 0;
private boolean robotSlowMode = false;
public Pilot(RedCrabMinibot robot) {
this.robot = robot;
@@ -117,12 +119,24 @@ public class Pilot extends CyberarmState {
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
double maxPower = RedCrabMinibot.DRIVETRAIN_MAX_SPEED;
double maxVelocity = Utilities.unitToTicks(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
RedCrabMinibot.DRIVETRAIN_VELOCITY_MAX_MM);
double slowVelocity = Utilities.unitToTicks(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
RedCrabMinibot.DRIVETRAIN_VELOCITY_SLOW_MM);
double velocity = robotSlowMode ? slowVelocity : maxVelocity;
robot.frontLeft.motorEx.setPower(frontLeftPower * maxPower);
robot.backLeft.motorEx.setPower(backLeftPower * maxPower);
robot.frontRight.motorEx.setPower(frontRightPower * maxPower);
robot.backRight.motorEx.setPower(backRightPower * maxPower);
robot.frontLeft.set(frontLeftPower * velocity);
robot.backLeft.set(backLeftPower * velocity);
robot.frontRight.set(frontRightPower * velocity);
robot.backRight.set(backRightPower * velocity);
}
private void clawArmAndWristController() {