From b83aec0a14a7cd33e30a326eb036b8666b2b45ab Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Wed, 20 Dec 2023 11:09:54 -0600 Subject: [PATCH] RedCrab: Make drivetrain use velocity instead of raw power --- .../minibots/red_crab/RedCrabMinibot.java | 10 ++++ .../minibots/red_crab/cyberarm_RedCrab.json | 2 +- .../minibots/red_crab/states/Move.java | 54 +++++++++---------- .../minibots/red_crab/states/Pilot.java | 24 +++++++-- 4 files changed, 57 insertions(+), 33 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 48be6b4..c58fc48 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -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(); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json index 0b16dfe..de76ea2 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json @@ -1 +1 @@ -{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-20 09:07:55 -0600","spec_version":2,"revision":406},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"00-01","comment":"@ServoMove - Move wrist to position camera","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx5000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix8000"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]},{"name":"02-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2000"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxPower","value":"Dx0.35"},{"name":"minPower","value":"Dx0.25"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"03-00","comment":"@Rotate - Rotate robot 0","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"04-00","comment":"@PathEnactor - Enact path","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90 degrees","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"06-00","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx3000"},{"name":"lerpMM_DOWN","value":"Dx1000"},{"name":"lerpMM_UP","value":"Dx1000"},{"name":"maxPower","value":"Dx0.5"},{"name":"minPower","value":"Dx0.15"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]}]},{"name":"Robot","actions":[{"name":"Claw_Tuning","comment":"Claws","enabled":true,"variables":[{"name":"claw_left_closed_position","value":"Dx0.2"},{"name":"claw_left_open_position","value":"Dx0.5"},{"name":"claw_right_closed_position","value":"Dx0.78"},{"name":"claw_right_open_position","value":"Dx0.5"}]},{"name":"ClawArm_Tuning","comment":"Claw Arm","enabled":true,"variables":[{"name":"collect_angle","value":"Dx200.0"},{"name":"collect_float_angle","value":"Dx180"},{"name":"deposit_angle","value":"Dx130.0"},{"name":"gear_ratio","value":"Dx80"},{"name":"kD","value":"Dx0.0"},{"name":"kF","value":"Dx0.1"},{"name":"kI","value":"Dx0.0"},{"name":"kP","value":"Dx0.5"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx180.0"},{"name":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix1"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.64"},{"name":"collect_position","value":"Dx0.64"},{"name":"deposit_position","value":"Dx0.64"},{"name":"stow_position","value":"Dx0.5"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx13.0321"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Ix28"},{"name":"wheel_diameter_mm","value":"Dx90"}]},{"name":"DroneLauncher_Tuning","comment":"Drone Launcher","enabled":true,"variables":[{"name":"initial_position","value":"Dx0.5"},{"name":"launch_confirmation_time_ms","value":"Ix1000"},{"name":"launch_position","value":"Dx0.7"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.4"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","actions":[]},{"name":"zAutonomous_RED_Audience","actions":[]}],"presets":{"groups":[],"actions":[{"name":"ClawArmMove","comment":"@ClawArmMove - Move claw arm","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"Move","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxPower","value":"Dx0.75"},{"name":"minPower","value":"Dx0.15"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"PathEnactor","comment":"@PathEnactor - Enact path","enabled":true,"variables":[]},{"name":"PathSelector","comment":"@PathSelector - Pick path","enabled":true,"variables":[{"name":"fallbackPath","value":"Ix0"},{"name":"minConfidence","value":"Dx0.7"},{"name":"timeoutMS","value":"Ix2000"}]},{"name":"Rotate","comment":"@Rotate - Rotate robot to face degree heading","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1.5"}]},{"name":"ServoMove","comment":"@ServoMove - Move servo to position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix2000"}]}]}}} \ No newline at end of file +{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-20 11:14:42 -0600","spec_version":2,"revision":417},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"00-01","comment":"@ServoMove - Move wrist to position camera","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx5000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix8000"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]},{"name":"02-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2000"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"03-00","comment":"@Rotate - Rotate robot 0","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"04-00","comment":"@PathEnactor - Enact path","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90 degrees","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"06-00","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx3000"},{"name":"lerpMM_DOWN","value":"Dx1000"},{"name":"lerpMM_UP","value":"Dx1000"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]}]},{"name":"Robot","actions":[{"name":"Claw_Tuning","comment":"Claws","enabled":true,"variables":[{"name":"claw_left_closed_position","value":"Dx0.2"},{"name":"claw_left_open_position","value":"Dx0.5"},{"name":"claw_right_closed_position","value":"Dx0.78"},{"name":"claw_right_open_position","value":"Dx0.5"}]},{"name":"ClawArm_Tuning","comment":"Claw Arm","enabled":true,"variables":[{"name":"collect_angle","value":"Dx200.0"},{"name":"collect_float_angle","value":"Dx180"},{"name":"deposit_angle","value":"Dx130.0"},{"name":"gear_ratio","value":"Dx80"},{"name":"kD","value":"Dx0.0"},{"name":"kF","value":"Dx0.1"},{"name":"kI","value":"Dx0.0"},{"name":"kP","value":"Dx0.5"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx180.0"},{"name":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix1"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.64"},{"name":"collect_position","value":"Dx0.64"},{"name":"deposit_position","value":"Dx0.64"},{"name":"stow_position","value":"Dx0.5"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx13.0321"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Ix28"},{"name":"velocity_max_in_mm","value":"Dx610"},{"name":"velocity_slow_in_mm","value":"Dx250"},{"name":"wheel_diameter_mm","value":"Dx90"}]},{"name":"DroneLauncher_Tuning","comment":"Drone Launcher","enabled":true,"variables":[{"name":"initial_position","value":"Dx0.5"},{"name":"launch_confirmation_time_ms","value":"Ix1000"},{"name":"launch_position","value":"Dx0.7"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.4"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","actions":[]},{"name":"zAutonomous_RED_Audience","actions":[]}],"presets":{"groups":[],"actions":[{"name":"ClawArmMove","comment":"@ClawArmMove - Move claw arm","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"Move","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"PathEnactor","comment":"@PathEnactor - Enact path","enabled":true,"variables":[]},{"name":"PathSelector","comment":"@PathSelector - Pick path","enabled":true,"variables":[{"name":"fallbackPath","value":"Ix0"},{"name":"minConfidence","value":"Dx0.7"},{"name":"timeoutMS","value":"Ix2000"}]},{"name":"Rotate","comment":"@Rotate - Rotate robot to face degree heading","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1.5"}]},{"name":"ServoMove","comment":"@ServoMove - Move servo to position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix2000"}]}]}}} \ No newline at end of file diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java index 6701fc6..a06ca64 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java @@ -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; } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java index 776be3d..46318fb 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -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() {