From e803df0cd9c20db24fd2e70cb5b67b618dc038a0 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Wed, 20 Dec 2023 20:00:10 -0600 Subject: [PATCH] RedCrab: 'Finished' adapting code to use velocity instead of raw power, verified distance math :smile:, claw arm still needs work, misc. --- .../minibots/red_crab/RedCrabMinibot.java | 215 +++++++++++------- .../minibots/red_crab/cyberarm_RedCrab.json | 2 +- .../minibots/red_crab/states/Move.java | 109 +++++---- .../minibots/red_crab/states/Pilot.java | 14 +- .../minibots/red_crab/states/Rotate.java | 39 ++-- 5 files changed, 237 insertions(+), 142 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 d22734a..36fe364 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 @@ -10,6 +10,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; @@ -55,7 +56,7 @@ public class RedCrabMinibot { public static double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0; public static double CLAW_ARM_COLLECT_ANGLE = 200.0; - public static final double WINCH_MAX_SPEED = 0.5; + public static double WINCH_MAX_SPEED = 0.5; public static double CLAW_WRIST_STOW_POSITION = 0.7; public static double CLAW_WRIST_DEPOSIT_POSITION = 0.64; @@ -80,12 +81,10 @@ public class RedCrabMinibot { /// HARDWARE /// public final IMU imu; - public final MotorEx frontLeft, frontRight, backLeft, backRight, winch; + public final DcMotorEx frontLeft, frontRight, backLeft, backRight, winch; public final DcMotorEx clawArm; public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; - public final MotorGroup left, right; - final CyberarmEngine engine; public TimeCraftersConfiguration config; @@ -119,7 +118,7 @@ public class RedCrabMinibot { /// IMU /// /// ------------------------------------------------------------------------------------ /// - imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0 + imu = engine.hardwareMap.get(IMU.class, "imu"); // | Control Hub, I2C Port: 0 IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, @@ -132,33 +131,29 @@ public class RedCrabMinibot { /// DRIVE TRAIN /// /// ------------------------------------------------------------------------------------ /// - frontLeft = new MotorEx(engine.hardwareMap, "frontLeft"); // | Ctrl|Ex Hub, Port: ?? - frontRight = new MotorEx(engine.hardwareMap, "frontRight"); // | Ctrl|Ex Hub, Port: ?? - backLeft = new MotorEx(engine.hardwareMap, "backLeft"); // | Ctrl|Ex Hub, Port: ?? - backRight = new MotorEx(engine.hardwareMap, "backRight"); // | Ctrl|Ex Hub, Port: ?? + frontLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontLeft"); // | Expansion Hub, Motor Port: 2 + frontRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontRight"); // | Expansion Hub, Motor Port: 3 + backLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("backLeft"); // | Expansion Hub, Motor Port: 3 + backRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("backRight"); // | Expansion Hub, Motor Port: 2 - /// --- (SOFT) RESET MOTOR ENCODERS - frontLeft.resetEncoder(); - frontRight.resetEncoder(); - backLeft.resetEncoder(); - backRight.resetEncoder(); + /// --- RESET MOTOR ENCODERS + frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); /// --- MOTOR DIRECTIONS - frontLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); - frontRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); - backLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); - backRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + frontRight.setDirection(DcMotorSimple.Direction.FORWARD); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + backRight.setDirection(DcMotorSimple.Direction.FORWARD); /// --- MOTOR BRAKING MODE /// --- NOTE: Having BRAKE mode set for drivetrain helps with consistently of control - frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - backRight.setZeroPowerBehavior((Motor.ZeroPowerBehavior.BRAKE)); - - /// --- MOTOR GROUPS - left = new MotorGroup(frontLeft, backLeft); - right = new MotorGroup(frontRight, backRight); + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); /// --- MOTOR DISTANCE PER TICK double distancePerTick = Utilities.ticksToUnit( @@ -168,35 +163,33 @@ public class RedCrabMinibot { DistanceUnit.MM, 1); - frontLeft.setDistancePerPulse(distancePerTick); - frontRight.setDistancePerPulse(distancePerTick); - 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); + frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); /// WINCH /// /// ------------------------------------------------------------------------------------ /// - winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ?? + winch = (DcMotorEx) engine.hardwareMap.dcMotor.get("winch"); // | Expansion Hub, Motor Port: 0 - /// --- (SOFT) MOTOR ENCODER RESET - winch.resetEncoder(); + /// --- MOTOR ENCODER RESET + winch.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); /// --- MOTOR DIRECTION /// --- NOTE: Unknown if FORWARD or REVERSE is correct - winch.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + winch.setDirection(DcMotorSimple.Direction.FORWARD); + + /// --- RUN MODE + winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); /// CLAW and Co. /// /// ------------------------------------------------------------------------------------ /// clawArmPIDFController = new PIDFController(0.4, 0.01, 0.1, 0.0); - clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm"); // | Ctrl|Ex Hub, Port: ?? - clawWrist = engine.hardwareMap.servo.get("clawWrist"); // | Ctrl|Ex Hub, Port: ?? - leftClaw = engine.hardwareMap.servo.get("leftClaw"); // | Ctrl|Ex Hub, Port: ?? - rightClaw = engine.hardwareMap.servo.get("rightClaw"); // | Ctrl|Ex Hub, Port: ?? + clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm"); // | Expansion Hub, Motor Port: 1 + clawWrist = engine.hardwareMap.servo.get("clawWrist"); // | Control Hub, Servo Port: 0 + leftClaw = engine.hardwareMap.servo.get("leftClaw"); // | Control Hub, Servo Port: 2 + rightClaw = engine.hardwareMap.servo.get("rightClaw"); // | Control Hub, Servo Port: 1 /// --- Claw Arm Motor /// --- --- (SOFT) RESET MOTOR ENCODER @@ -207,9 +200,9 @@ public class RedCrabMinibot { /// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃 clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); /// --- --- Run Mode - clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); clawArm.setTargetPosition(0); + clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); + clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); /// --- Claws /// --- --- Wrist @@ -223,12 +216,12 @@ public class RedCrabMinibot { rightClaw.setPosition((CLAW_RIGHT_CLOSED_POSITION)); /// DRONE LATCH /// - droneLatch = engine.hardwareMap.servo.get("droneLatch"); // | Ctrl|Ex Hub, Port: ?? + droneLatch = engine.hardwareMap.servo.get("droneLatch"); // | Expansion Hub, Servo Port: 0 droneLatch.setDirection(Servo.Direction.FORWARD); droneLatch.setPosition(DRONE_LATCH_INITIAL_POSITION); /// HOOK ARM /// - hookArm = engine.hardwareMap.servo.get("hookArm"); // | Ctrl|Ex Hub, Port: ?? + hookArm = engine.hardwareMap.servo.get("hookArm"); // | Control Hub, Servo Port: 3 hookArm.setDirection(Servo.Direction.FORWARD); // hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF: @@ -263,6 +256,10 @@ public class RedCrabMinibot { RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value(); RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value(); + /// WINCH + + RedCrabMinibot.WINCH_MAX_SPEED = config.variable("Robot", "Winch_Tuning", "max_speed").value(); + /// CLAW WRIST RedCrabMinibot.CLAW_WRIST_STOW_POSITION = config.variable("Robot", "ClawWrist_Tuning", "stow_position").value(); RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "deposit_position").value(); @@ -280,8 +277,8 @@ public class RedCrabMinibot { RedCrabMinibot.HOOK_ARM_UP_POSITION = config.variable("Robot", "HookArm_Tuning", "up_position").value(); /// DRONE LATCH - RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value(); - RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value(); + RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value(); + RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value(); RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value(); } @@ -291,56 +288,96 @@ public class RedCrabMinibot { engine.telemetry.addLine("Motors"); engine.telemetry.addData( "Front Left", - "Power: %.2f, Current: %.2f mAmp, Position: %d", - frontLeft.motorEx.getPower(), - frontLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - frontLeft.motorEx.getCurrentPosition()); + "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)", + frontLeft.getPower(), + frontLeft.getCurrent(CurrentUnit.MILLIAMPS), + frontLeft.getCurrentPosition(), + frontLeft.getVelocity(), + Utilities.ticksToUnit( + DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + DRIVETRAIN_GEAR_RATIO, + DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + (int)frontLeft.getVelocity())); + engine.telemetry.addLine(); engine.telemetry.addData( "Front Right", - "Power: %.2f, Current: %.2f mAmp, Position: %d", - frontRight.motorEx.getPower(), - frontRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - frontRight.motorEx.getCurrentPosition()); + "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)", + frontRight.getPower(), + frontRight.getCurrent(CurrentUnit.MILLIAMPS), + frontRight.getCurrentPosition(), + frontRight.getVelocity(), + Utilities.ticksToUnit( + DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + DRIVETRAIN_GEAR_RATIO, + DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + (int)frontLeft.getVelocity())); + engine.telemetry.addLine(); engine.telemetry.addData( "Back Left", - "Power: %.2f, Current: %.2f mAmp, Position: %d", - backLeft.motorEx.getPower(), - backLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - backLeft.motorEx.getCurrentPosition()); + "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)", + backLeft.getPower(), + backLeft.getCurrent(CurrentUnit.MILLIAMPS), + backLeft.getCurrentPosition(), + backLeft.getVelocity(), + Utilities.ticksToUnit( + DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + DRIVETRAIN_GEAR_RATIO, + DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + (int)backLeft.getVelocity())); + engine.telemetry.addLine(); engine.telemetry.addData( "Back Right", - "Power: %.2f, Current: %.2f mAmp, Position: %d", - backRight.motorEx.getPower(), - backRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - backRight.motorEx.getCurrentPosition()); + "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)", + backRight.getPower(), + backRight.getCurrent(CurrentUnit.MILLIAMPS), + backRight.getCurrentPosition(), + backRight.getVelocity(), + Utilities.ticksToUnit( + DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + DRIVETRAIN_GEAR_RATIO, + DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + (int)backRight.getVelocity())); engine.telemetry.addLine(); + engine.telemetry.addLine(); + engine.telemetry.addData( "Winch", - "Power: %.2f, Current: %.2f mAmp, Position: %d", - winch.motorEx.getPower(), - winch.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - winch.motorEx.getCurrentPosition()); + "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)", + winch.getPower(), + winch.getCurrent(CurrentUnit.MILLIAMPS), + winch.getCurrentPosition(), + winch.getVelocity(), + Utilities.ticksToUnit( + DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + DRIVETRAIN_GEAR_RATIO, + DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + (int)winch.getVelocity())); engine.telemetry.addLine(); + PIDFCoefficients clawArmPIDFPosition = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION); + PIDFCoefficients clawArmPIDFEncoder = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER); engine.telemetry.addData( "Claw Arm", - "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f", + "Power: %.2f, Current: %.2f mAmp, Position: %d, Angle: %.2f, Velocity: %.2f (%.2f degrees/s)", clawArm.getPower(), clawArm.getCurrent(CurrentUnit.MILLIAMPS), clawArm.getCurrentPosition(), - clawArm.getVelocity()); + Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition()), + clawArm.getVelocity(), + Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, (int)clawArm.getVelocity())); engine.telemetry.addData( - " PIDF", "P: %.4f, I: %.4f, D: %.4f, F: %.4f", - clawArmPIDFController.getP(), - clawArmPIDFController.getI(), - clawArmPIDFController.getD(), - clawArmPIDFController.getF()); - engine.telemetry.addData( - " PIDF+", "PosError: %.4f, velError: %.4f, Period: %.4f", - clawArmPIDFController.getPositionError(), - clawArmPIDFController.getVelocityError(), - clawArmPIDFController.getPeriod()); + " PIDF", "P: %.4f, I: %.4f, D: %.4f, F: %.4f, PosP: %.4f", + clawArmPIDFEncoder.p, + clawArmPIDFEncoder.i, + clawArmPIDFEncoder.d, + clawArmPIDFEncoder.f, + clawArmPIDFPosition.p); engine.telemetry.addLine(); engine.telemetry.addLine("Servos"); @@ -399,4 +436,20 @@ public class RedCrabMinibot { clawArm.setVelocity(velocity); } + + public double distanceMM(DcMotorEx motor) { + return Utilities.ticksToUnit( + RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.DRIVETRAIN_GEAR_RATIO, + RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + motor.getCurrentPosition() + ); + } + + public boolean atTargetPosition(DcMotorEx motor, double travelledDistanceMM, double toleranceMM) { + double distanceMM = distanceMM(motor); + + return Utilities.isBetween(distanceMM, travelledDistanceMM - toleranceMM, travelledDistanceMM + toleranceMM); + } } 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 abfa660..d2d10a2 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 11:45:16 -0600","spec_version":2,"revision":419},"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":"KPosP","value":"Dx1.0"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx60.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 +{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-20 19:51:07 -0600","spec_version":2,"revision":566},"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":"Bxfalse"},{"name":"lerpMS","value":"Dx1"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"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":"Dx415"},{"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 to 0d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"04-00","comment":"@PathEnactor - Enact path; paths must return to \"origin\" when done","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"06-00","comment":"@Move - Move up to backdrop","enabled":true,"variables":[{"name":"distanceMM","value":"Dx800"},{"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":"07-00","comment":"@Rotate - Rotate robot to face backdrop","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"20-00","comment":"@Move - park in corner","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":"Bxtrue"},{"name":"timeoutMS","value":"Ix2000"},{"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":"Dx72.0"},{"name":"kD","value":"Dx0.0"},{"name":"kF","value":"Dx0.0"},{"name":"kI","value":"Dx6.0"},{"name":"kP","value":"Dx25"},{"name":"KPosP","value":"Dx25"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx60"},{"name":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix6"}]},{"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"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.4"}]},{"name":"Winch_Tuning","comment":"Winch","enabled":true,"variables":[{"name":"max_speed","value":"Dx1.0"}]}]},{"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":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.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 a06ca64..e144ae1 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 @@ -1,7 +1,11 @@ package dev.cyberarm.minibots.red_crab.states; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + import dev.cyberarm.engine.V2.CyberarmState; import dev.cyberarm.engine.V2.Utilities; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; @@ -50,30 +54,45 @@ public class Move extends CyberarmState { throw(new RuntimeException("INVALID Initial IMU value!")); } + int toleranceTicks = Utilities.unitToTicks( + RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.DRIVETRAIN_GEAR_RATIO, + RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + toleranceMM); + int distanceTicks = Utilities.unitToTicks( + RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.DRIVETRAIN_GEAR_RATIO, + RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + toleranceMM); + + robot.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.frontLeft.setTargetPositionTolerance(toleranceTicks); + robot.frontRight.setTargetPositionTolerance(toleranceTicks); + robot.backLeft.setTargetPositionTolerance(toleranceTicks); + robot.backRight.setTargetPositionTolerance(toleranceTicks); + if (strafe) { - robot.frontLeft.resetEncoder(); - robot.frontRight.resetEncoder(); - robot.backLeft.resetEncoder(); - robot.backRight.resetEncoder(); + robot.frontLeft.setTargetPosition(distanceTicks); + robot.frontRight.setTargetPosition(-distanceTicks); + robot.backLeft.setTargetPosition(-distanceTicks); + robot.backRight.setTargetPosition(distanceTicks); - robot.frontLeft.setPositionTolerance(toleranceMM); - robot.frontRight.setPositionTolerance(toleranceMM); - robot.backLeft.setPositionTolerance(toleranceMM); - robot.backRight.setPositionTolerance(toleranceMM); - - robot.frontLeft.setTargetDistance(distanceMM); - robot.frontRight.setTargetDistance(-distanceMM); - robot.backLeft.setTargetDistance(-distanceMM); - robot.backRight.setTargetDistance(distanceMM); } else { - robot.left.resetEncoder(); - robot.right.resetEncoder(); - - robot.left.setPositionTolerance(toleranceMM); - robot.right.setPositionTolerance(toleranceMM); - - robot.left.setTargetDistance(distanceMM); - robot.right.setTargetDistance(distanceMM); + robot.frontLeft.setTargetPosition(distanceTicks); + robot.frontRight.setTargetPosition(distanceTicks); + robot.backLeft.setTargetPosition(distanceTicks); + robot.backRight.setTargetPosition(distanceTicks); } } @@ -93,13 +112,13 @@ public class Move extends CyberarmState { engine.telemetry.addData("lerp MM UP", lerpMM_UP); engine.telemetry.addData("lerp MM DOWN", lerpMM_DOWN); engine.telemetry.addData("Distance MM", distanceMM); - engine.telemetry.addData("Distance Travelled MM", robot.frontLeft.getDistance()); + engine.telemetry.addData("Distance Travelled MM", robot.distanceMM(robot.frontLeft)); engine.telemetry.addData("Timeout MS", timeoutMS); progressBar(20, runTime() / timeoutMS); } private void tankMove(){ - double travelledDistance = Math.abs(robot.frontLeft.getDistance()); + double travelledDistance = Math.abs(robot.distanceMM(robot.frontLeft)); double velocity = lerpVelocity(travelledDistance); double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu)); @@ -114,21 +133,25 @@ public class Move extends CyberarmState { rightVelocity += correctiveVelocity; } - robot.left.set(leftVelocity); - robot.right.set(rightVelocity); + robot.frontLeft.setVelocity(leftVelocity); + robot.frontRight.setVelocity(rightVelocity); + robot.backLeft.setVelocity(leftVelocity); + robot.backRight.setVelocity(leftVelocity); if (runTime() >= timeoutMS || - (robot.frontLeft.atTargetPosition() || robot.frontRight.atTargetPosition()) || - Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM)) { - robot.left.set(0); - robot.right.set(0); + (atTargetPosition(robot.frontLeft) || atTargetPosition(robot.frontRight)) || + Math.abs(robot.distanceMM(robot.frontLeft)) >= Math.abs(distanceMM)) { + robot.frontLeft.setVelocity(0); + robot.frontRight.setVelocity(0); + robot.backLeft.setVelocity(0); + robot.backRight.setVelocity(0); this.finished(); } } private void strafeMove() { - double travelledDistance = Math.abs(robot.frontLeft.getDistance()); + double travelledDistance = Math.abs(robot.distanceMM(robot.frontLeft)); double velocity = lerpVelocity(travelledDistance); double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu)); @@ -143,17 +166,17 @@ public class Move extends CyberarmState { backVelocity += correctiveVelocity; } - robot.frontLeft.set(frontVelocity); - robot.frontRight.set(-frontVelocity); - robot.backLeft.set(-backVelocity); - robot.backRight.set(backVelocity); + robot.frontLeft.setVelocity(frontVelocity); + robot.frontRight.setVelocity(-frontVelocity); + robot.backLeft.setVelocity(-backVelocity); + robot.backRight.setVelocity(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)) { - robot.frontLeft.set(0); - robot.frontRight.set(0); - robot.backLeft.set(0); - robot.backRight.set(0); + if (runTime() >= timeoutMS || (atTargetPosition(robot.frontLeft) || atTargetPosition(robot.backRight)) || + Math.abs(robot.distanceMM(robot.frontLeft)) >= Math.abs(distanceMM) || Math.abs(robot.distanceMM(robot.backRight)) >= Math.abs(distanceMM)) { + robot.frontLeft.setVelocity(0); + robot.frontRight.setVelocity(0); + robot.backLeft.setVelocity(0); + robot.backRight.setVelocity(0); this.finished(); } @@ -175,4 +198,10 @@ public class Move extends CyberarmState { return lerpVelocity; } + + private boolean atTargetPosition(DcMotorEx motor) { + double travelledDistanceMM = robot.distanceMM(motor); + + return Utilities.isBetween(travelledDistanceMM, distanceMM - toleranceMM, distanceMM + toleranceMM); + } } 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 46318fb..a4f4762 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 @@ -133,10 +133,10 @@ public class Pilot extends CyberarmState { RedCrabMinibot.DRIVETRAIN_VELOCITY_SLOW_MM); double velocity = robotSlowMode ? slowVelocity : maxVelocity; - robot.frontLeft.set(frontLeftPower * velocity); - robot.backLeft.set(backLeftPower * velocity); - robot.frontRight.set(frontRightPower * velocity); - robot.backRight.set(backRightPower * velocity); + robot.frontLeft.setVelocity(frontLeftPower * velocity); + robot.backLeft.setVelocity(backLeftPower * velocity); + robot.frontRight.setVelocity(frontRightPower * velocity); + robot.backRight.setVelocity(backRightPower * velocity); } private void clawArmAndWristController() { @@ -207,11 +207,11 @@ public class Pilot extends CyberarmState { private void winchController() { if (engine.gamepad1.right_trigger > 0) { - robot.winch.motorEx.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED); + robot.winch.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED); } else if (engine.gamepad1.left_trigger > 0) { - robot.winch.motorEx.setPower(-engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED); + robot.winch.setPower(-engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED); } else { - robot.winch.motorEx.setPower(0); + robot.winch.setPower(0); } } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java index 454e4a0..0d60c45 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java @@ -2,16 +2,17 @@ package dev.cyberarm.minibots.red_crab.states; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + import dev.cyberarm.engine.V2.CyberarmState; import dev.cyberarm.engine.V2.Utilities; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; -import dev.cyberarm.minibots.yellow.YellowMinibot; public class Rotate extends CyberarmState { final private RedCrabMinibot robot; final private String groupName, actionName; - final private double maxPower, minPower, lerpDegrees, headingDegrees, toleranceDegrees; + final private double maxVelocityMM, minVelocityMM, lerpDegrees, headingDegrees, toleranceDegrees; final private int timeoutMS; private boolean commitToRotation = false; @@ -21,8 +22,8 @@ public class Rotate extends CyberarmState { this.groupName = groupName; this.actionName = actionName; - this.minPower = robot.config.variable(groupName, actionName, "minPower").value(); - this.maxPower = robot.config.variable(groupName, actionName, "maxPower").value(); + this.maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value(); + this.minVelocityMM = robot.config.variable(groupName, actionName, " minVelocityMM").value(); this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value(); this.headingDegrees = robot.config.variable(groupName, actionName, "headingDEGREES").value(); @@ -33,26 +34,38 @@ public class Rotate extends CyberarmState { @Override public void exec() { - double angleDiff = Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees); + double angleDiff = Utilities.angleDiff(Utilities.facing(robot.imu), headingDegrees); if (Math.abs(angleDiff) <= toleranceDegrees || runTime() >= timeoutMS) { - robot.left.set(0); - robot.right.set(0); + robot.frontLeft.setPower(0); + robot.frontRight.setPower(0); + robot.backLeft.setPower(0); + robot.backRight.setPower(0); this.finished(); return; } - double power = Utilities.lerp(minPower, maxPower, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0)); + double velocityMM = Utilities.lerp(minVelocityMM, maxVelocityMM, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0)); + double velocity = Utilities.unitToTicks( + RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.DRIVETRAIN_GEAR_RATIO, + RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + velocityMM); if (!commitToRotation) { if (angleDiff < 0) { - robot.left.set(-power); - robot.right.set(power); + robot.frontLeft.setVelocity(-velocity); + robot.frontRight.setVelocity(velocity); + robot.backLeft.setVelocity(-velocity); + robot.backRight.setVelocity(velocity); } else { - robot.left.set(power); - robot.right.set(-power); + robot.frontLeft.setVelocity(velocity); + robot.frontRight.setVelocity(-velocity); + robot.backLeft.setVelocity(velocity); + robot.backRight.setVelocity(-velocity); } } @@ -64,7 +77,7 @@ public class Rotate extends CyberarmState { engine.telemetry.addLine(); engine.telemetry.addData("Robot Heading", Utilities.facing(robot.imu)); engine.telemetry.addData("Robot Target Heading", headingDegrees); - engine.telemetry.addData("Robot Angle Diff", Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees)); + engine.telemetry.addData("Robot Angle Diff", Utilities.angleDiff(Utilities.facing(robot.imu), headingDegrees)); engine.telemetry.addData("Robot Turn Rate", Utilities.turnRate(robot.imu)); } }