From d2200fcc0a6f23e25baf1b7a5a624e51f86af4b3 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Wed, 20 Dec 2023 10:37:34 -0600 Subject: [PATCH] RedCrab: Hopefully fix ticks to distance calculations for drivetrain, fixed drivetrain gear ratio (see: https://docs.revrobotics.com/ultraplanetary/ultraplanetary-gearbox/cartridge-details#actual-cartridge-gear-ratios), changed claw arm to RUN_WITHOUT_ENCODERS to disable in-built pid controller (might be conflicting with FTCLib's that I'm using), using bulk caching of hub values, misc. changes --- .../minibots/red_crab/RedCrabMinibot.java | 84 ++++++++++++------- .../minibots/red_crab/cyberarm_RedCrab.json | 2 +- .../RedCrabAutonomousBlueAudienceEngine.java | 2 +- .../RedCrabAutonomousBlueBackstageEngine.java | 2 +- .../engines/RedCrabAutonomousEngine.java | 13 ++- .../RedCrabAutonomousRedAudienceEngine.java | 2 +- .../RedCrabAutonomousRedBackstageEngine.java | 2 +- .../red_crab/engines/RedCrabTeleOpEngine.java | 8 ++ .../minibots/red_crab/states/ClawArmMove.java | 12 +-- .../minibots/red_crab/states/Pilot.java | 10 +-- 10 files changed, 89 insertions(+), 48 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 0871bd3..48be6b4 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 @@ -1,10 +1,10 @@ package dev.cyberarm.minibots.red_crab; -import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.controller.PIDFController; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -15,7 +15,9 @@ import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.tfod.TfodProcessor; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; @@ -34,10 +36,16 @@ public class RedCrabMinibot { /// TUNING CONSTANTS /// public static double DRIVETRAIN_MAX_SPEED = 0.5; + 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; public static double CLAW_ARM_MAX_SPEED = 0.5; - public static double CLAW_ARM_VELOCITY_DEGREES = 10; - public static double CLAW_ARM_kP = 0.1; + public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10; + public static double CLAW_ARM_kP = 0.0; + public static double CLAW_ARM_kI = 0.0; + public static double CLAW_ARM_kD = 0.0; + public static double CLAW_ARM_kF = 0.0; public static int CLAW_ARM_POSITION_TOLERANCE = 1; public static double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 public static double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 @@ -95,6 +103,8 @@ public class RedCrabMinibot { public TeamPropVisionProcessor teamProp = null; /// Spike Mark detector: using OpenCV for full frame saturation threshold detection. public SpikeMarkDetectorVisionProcessor spikeMark = null; + /// April Tag detector: Untested + public AprilTagProcessor aprilTag = null; /// Doohickey public VisionPortal visionPortal = null; @@ -148,12 +158,12 @@ public class RedCrabMinibot { right = new MotorGroup(frontRight, backRight); /// --- MOTOR DISTANCE PER TICK - double gearRatio = config.variable("Robot", "Drivetrain_Tuning", "gear_ratio").value(); - double motorTicks = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value(); - double wheelDiameterMM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value(); - - double wheelCircumference = Math.PI * wheelDiameterMM; - double distancePerTick = (motorTicks * gearRatio) / wheelCircumference; // raw motor encoder * gear ratio + double distancePerTick = Utilities.ticksToUnit( + DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + DRIVETRAIN_GEAR_RATIO, + DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + 1); frontLeft.setDistancePerPulse(distancePerTick); frontRight.setDistancePerPulse(distancePerTick); @@ -188,7 +198,7 @@ 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_USING_ENCODER); + clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); clawArm.setTargetPosition(0); @@ -204,14 +214,17 @@ public class RedCrabMinibot { rightClaw.setPosition((CLAW_RIGHT_CLOSED_POSITION)); /// DRONE LATCH /// - droneLatch = engine.hardwareMap.servo.get("droneLatch"); + droneLatch = engine.hardwareMap.servo.get("droneLatch"); // | Ctrl|Ex Hub, Port: ?? droneLatch.setDirection(Servo.Direction.FORWARD); droneLatch.setPosition(DRONE_LATCH_INITIAL_POSITION); /// HOOK ARM /// - hookArm = engine.hardwareMap.servo.get("hookArm"); + hookArm = engine.hardwareMap.servo.get("hookArm"); // | Ctrl|Ex Hub, Port: ?? hookArm.setDirection(Servo.Direction.FORWARD); // hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF: + + // Bulk read from hubs + Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL); } public void reloadConfig() { @@ -223,10 +236,13 @@ public class RedCrabMinibot { private void loadConstants() { /// Drivetrain RedCrabMinibot.DRIVETRAIN_MAX_SPEED = config.variable("Robot", "Drivetrain_Tuning", "max_speed").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(); /// CLAW ARM RedCrabMinibot.CLAW_ARM_MAX_SPEED = config.variable("Robot", "ClawArm_Tuning", "max_speed").value(); - RedCrabMinibot.CLAW_ARM_VELOCITY_DEGREES = config.variable("Robot", "ClawArm_Tuning", "velocityDEGREES").value(); + RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES = config.variable("Robot", "ClawArm_Tuning", "max_velocityDEGREES").value(); RedCrabMinibot.CLAW_ARM_POSITION_TOLERANCE = config.variable("Robot", "ClawArm_Tuning", "tolerance").value(); RedCrabMinibot.CLAW_ARM_STOW_ANGLE = config.variable("Robot", "ClawArm_Tuning", "stow_angle").value(); RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_float_angle").value(); @@ -303,7 +319,17 @@ public class RedCrabMinibot { clawArm.getCurrent(CurrentUnit.MILLIAMPS), clawArm.getCurrentPosition(), clawArm.getVelocity()); - engine.telemetry.addData("PIDF", "P: %.8f, I: %.8f, D: %.8f, F: %.8f", clawArmPIDFController.getP(), clawArmPIDFController.getI(), clawArmPIDFController.getD(), clawArmPIDFController.getF()); + 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()); engine.telemetry.addLine(); engine.telemetry.addLine("Servos"); @@ -324,34 +350,34 @@ public class RedCrabMinibot { public void controlClawArm() { Action action = config.action("Robot", "ClawArm_Tuning"); - double p = 0.0, i = 0.0, d = 0.0, f = 0.0; - double ticksInDegree = Utilities.motorAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1); + double ticksInDegree = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1); for (Variable v : action.getVariables()) { switch (v.name.trim()) { - case "kP": - p = v.value(); + case "kP": // Proportional + CLAW_ARM_kP = v.value(); break; - case "kI": - i = v.value(); + case "kI": // Integral + CLAW_ARM_kI = v.value(); break; - case "kD": - d = v.value(); + case "kD": // Derivative (Dampener) + CLAW_ARM_kD = v.value(); break; - case "kF": // feedback - f = v.value(); + case "kF": // Feedforward + CLAW_ARM_kF = v.value(); break; } } - clawArmPIDFController.setPIDF(p, i, d, f); int armPos = clawArm.getCurrentPosition(); - double pid = clawArmPIDFController.calculate(armPos, clawArm.getTargetPosition()); + clawArmPIDFController.setTolerance(CLAW_ARM_POSITION_TOLERANCE); + clawArmPIDFController.setPIDF(CLAW_ARM_kP, CLAW_ARM_kI, CLAW_ARM_kD, CLAW_ARM_kF); + double pidf = clawArmPIDFController.calculate(armPos, clawArm.getTargetPosition()); // double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f; - double power = Range.clip(pid, -CLAW_ARM_MAX_SPEED, CLAW_ARM_MAX_SPEED); + // Limit pidf's value to max power range + double power = Range.clip(pidf, -CLAW_ARM_MAX_SPEED, CLAW_ARM_MAX_SPEED); - clawArm.setVelocityPIDFCoefficients(p, i, d, f); - clawArm.setVelocity(CLAW_ARM_VELOCITY_DEGREES, AngleUnit.DEGREES); + clawArm.setPower(power); } } 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 44f571b..0b16dfe 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-19 21:55:01 -0600","spec_version":2,"revision":403},"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":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix1"},{"name":"velocityDEGREES","value":"Dx180.0"}]},{"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":"Dx16.0"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Dx28"},{"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 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 diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java index 0843747..544d31f 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java @@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot; import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") -public class RedCrabAutonomousBlueAudienceEngine extends CyberarmEngine { +public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine { @Override public void setup() { RedCrabMinibot robot = new RedCrabMinibot(true); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java index bd646d8..9533993 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java @@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot; import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") -public class RedCrabAutonomousBlueBackstageEngine extends CyberarmEngine { +public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine { @Override public void setup() { RedCrabMinibot robot = new RedCrabMinibot(true); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java index 7638c80..57d932f 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java @@ -1,2 +1,13 @@ -package dev.cyberarm.minibots.red_crab.engines;public class RedCrabAutonomousEngine { +package dev.cyberarm.minibots.red_crab.engines; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.engine.V2.Utilities; + +public abstract class RedCrabAutonomousEngine extends CyberarmEngine { + @Override + public void loop() { + Utilities.hubsClearBulkReadCache(hardwareMap); + + super.loop(); + } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java index 56fe2bb..779359d 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java @@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot; import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") -public class RedCrabAutonomousRedAudienceEngine extends CyberarmEngine { +public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine { @Override public void setup() { RedCrabMinibot robot = new RedCrabMinibot(true); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java index 67cb487..9066d58 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java @@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot; import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") -public class RedCrabAutonomousRedBackstageEngine extends CyberarmEngine { +public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine { @Override public void setup() { RedCrabMinibot robot = new RedCrabMinibot(true); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java index 6e59e91..1162cb9 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java @@ -3,6 +3,7 @@ package dev.cyberarm.minibots.red_crab.engines; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.engine.V2.Utilities; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; import dev.cyberarm.minibots.red_crab.states.ClawArmTask; import dev.cyberarm.minibots.red_crab.states.Pilot; @@ -17,4 +18,11 @@ public class RedCrabTeleOpEngine extends CyberarmEngine { addState(new Pilot(robot)); } + + @Override + public void loop() { + Utilities.hubsClearBulkReadCache(hardwareMap); + + super.loop(); + } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java index 1246343..7930c43 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java @@ -1,9 +1,5 @@ package dev.cyberarm.minibots.red_crab.states; -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.util.Range; - import dev.cyberarm.engine.V2.CyberarmState; import dev.cyberarm.engine.V2.Utilities; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; @@ -26,8 +22,8 @@ public class ClawArmMove extends CyberarmState { @Override public void start() { - robot.clawArm.setTargetPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle)); - robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); + robot.clawArm.setTargetPositionTolerance(Utilities.motorAngleToTicks(motorTicks, gearRatio, toleranceAngle)); + robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks(motorTicks, gearRatio, targetAngle)); } @Override @@ -45,8 +41,8 @@ public class ClawArmMove extends CyberarmState { engine.telemetry.addLine(); engine.telemetry.addData("Motor Power", robot.clawArm.getPower()); engine.telemetry.addData("Motor Position", robot.clawArm.getCurrentPosition()); - engine.telemetry.addData("Motor Angle", Utilities.motorAngle(motorTicks, gearRatio, robot.clawArm.getCurrentPosition())); - engine.telemetry.addData("Motor Target Position", Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); + engine.telemetry.addData("Motor Angle", Utilities.motorAngleToTicks(motorTicks, gearRatio, robot.clawArm.getCurrentPosition())); + engine.telemetry.addData("Motor Target Position", Utilities.motorAngleToTicks(motorTicks, gearRatio, targetAngle)); engine.telemetry.addData("Motor Target Angle", targetAngle); engine.telemetry.addData("Timeout MS", timeoutMS); progressBar(20, runTime() / timeoutMS); 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 6ee06ee..776be3d 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 @@ -128,7 +128,7 @@ public class Pilot extends CyberarmState { private void clawArmAndWristController() { switch (clawArmPosition) { case RedCrabMinibot.ClawArm_STOW: - robot.clawArm.setTargetPosition(Utilities.motorAngle( + robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks( RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, RedCrabMinibot.CLAW_ARM_STOW_ANGLE)); @@ -136,7 +136,7 @@ public class Pilot extends CyberarmState { robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_STOW_POSITION); break; case RedCrabMinibot.ClawArm_DEPOSIT: - robot.clawArm.setTargetPosition(Utilities.motorAngle( + robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks( RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE)); @@ -144,7 +144,7 @@ public class Pilot extends CyberarmState { robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION); break; case RedCrabMinibot.ClawArm_COLLECT_FLOAT: - robot.clawArm.setTargetPosition(Utilities.motorAngle( + robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks( RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE)); @@ -152,7 +152,7 @@ public class Pilot extends CyberarmState { robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_FLOAT_POSITION); break; case RedCrabMinibot.ClawArm_COLLECT: - robot.clawArm.setTargetPosition(Utilities.motorAngle( + robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks( RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE)); @@ -163,7 +163,7 @@ public class Pilot extends CyberarmState { } if (clawArmPosition == RedCrabMinibot.ClawArm_COLLECT && - robot.clawArm.getCurrentPosition() >= Utilities.motorAngle( + robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks( RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 25.0) {