From a4ed6101c47c17cffd036774e7247134f746c4b1 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 16 Dec 2023 15:56:44 -0600 Subject: [PATCH] Sync changes --- .../cyberarm/engine/V2/CyberarmEngine.java | 8 +++++ .../minibots/red_crab/RedCrabMinibot.java | 25 +++++++++++--- .../RedCrabAutonomousBlueAudienceEngine.java | 8 ++++- .../RedCrabAutonomousBlueBackstageEngine.java | 8 ++++- .../RedCrabAutonomousRedAudienceEngine.java | 8 ++++- .../RedCrabAutonomousRedBackstageEngine.java | 8 ++++- .../minibots/red_crab/states/ClawArmMove.java | 6 +++- .../minibots/red_crab/states/ClawArmTask.java | 18 +++++++++- .../minibots/red_crab/states/Move.java | 25 +++++++------- .../red_crab/states/PathSelector.java | 33 +++++++++++-------- .../minibots/red_crab/states/Rotate.java | 19 +++++++---- 11 files changed, 125 insertions(+), 41 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java index 184b993..b5ef54a 100644 --- a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java @@ -39,6 +39,7 @@ public abstract class CyberarmEngine extends OpMode { public boolean showStateChildrenListInTelemetry = false; private GamepadChecker gamepadCheckerGamepad1, gamepadCheckerGamepad2; + private boolean useThreads = true; /** * Called when INIT button on Driver Station is pushed @@ -482,4 +483,11 @@ public abstract class CyberarmEngine extends OpMode { } } } + + /** + * NO OP + */ + public void threadless() { + useThreads = false; + } } 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 1cc2334..388c8ca 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 @@ -32,15 +32,15 @@ public class RedCrabMinibot { /// TUNING CONSTANTS /// public static final double DRIVETRAIN_MAX_SPEED = 0.5; public static final double CLAW_ARM_MAX_SPEED = 0.5; - public static final double CLAW_ARM_kP = 0.025; - public static final double CLAW_ARM_POSITION_TOLERANCE = 3.3; + public static final double CLAW_ARM_kP = 0.1; + public static final double CLAW_ARM_POSITION_TOLERANCE = 1.5; public static final double WINCH_MAX_SPEED = 0.5; public static final double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 public static final double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 public static final double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0; public static final double CLAW_ARM_COLLECT_ANGLE = 200.0; - public static final double CLAW_WRIST_STOW_POSITION = 0.5; + public static final double CLAW_WRIST_STOW_POSITION = 0.7; public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.64; public static final double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64; public static final double CLAW_WRIST_COLLECT_POSITION = 0.64; @@ -59,7 +59,7 @@ public class RedCrabMinibot { /// MOTOR CONSTANTS /// public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; - public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 72; + public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 80; // Technically 72, but there is a lot of slop /// HARDWARE /// public final IMU imu; @@ -104,6 +104,10 @@ public class RedCrabMinibot { RevHubOrientationOnRobot.UsbFacingDirection.UP)); imu.initialize(parameters); + if (autonomous) { + imu.resetYaw(); + } + /// DRIVE TRAIN /// /// ------------------------------------------------------------------------------------ /// frontLeft = new MotorEx(engine.hardwareMap, "frontLeft"); // | Ctrl|Ex Hub, Port: ?? @@ -134,6 +138,19 @@ public class RedCrabMinibot { left = new MotorGroup(frontLeft, backLeft); 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 + + frontLeft.setDistancePerPulse(distancePerTick); + frontRight.setDistancePerPulse(distancePerTick); + backLeft.setDistancePerPulse(distancePerTick); + backRight.setDistancePerPulse(distancePerTick); + /// WINCH /// /// ------------------------------------------------------------------------------------ /// winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ?? 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 0262035..0843747 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 @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; 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 { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_BLUE_Audience" ); 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 e14b4cf..bd646d8 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 @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; 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 { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_BLUE_Backstage" ); 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 b15f54c..56fe2bb 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 @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; 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 { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_RED_Audience" ); 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 e1ab5e0..67cb487 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 @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; 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 { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_RED_Backstage" ); 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 66b1f37..ad5a9ad 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 @@ -27,7 +27,8 @@ public class ClawArmMove extends CyberarmState { public void start() { robot.clawArm.setPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle)); robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); - robot.clawArm.set(power); + + engine.blackboardSet("clawArmPower", power); } @Override @@ -40,8 +41,11 @@ public class ClawArmMove extends CyberarmState { @Override public void telemetry() { engine.telemetry.addLine(); + engine.telemetry.addData("Motor Power", robot.clawArm.get()); 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 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/ClawArmTask.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java index 4ed845a..d7b42c8 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java @@ -1,2 +1,18 @@ -package dev.cyberarm.minibots.red_crab.states;public class ClawArmTask { +package dev.cyberarm.minibots.red_crab.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class ClawArmTask extends CyberarmState { + private final RedCrabMinibot robot; + public ClawArmTask(RedCrabMinibot robot) { + this.robot = robot; + } + + @Override + public void exec() { + double power = engine.blackboardGetDouble("clawArmPower"); + + robot.clawArm.set(power); + } } 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 d363b6b..6701fc6 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 @@ -93,13 +93,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", (strafe ? robot.left.getDistance() : robot.frontLeft.getDistance())); + engine.telemetry.addData("Distance Travelled MM", robot.frontLeft.getDistance()); engine.telemetry.addData("Timeout MS", timeoutMS); progressBar(20, runTime() / timeoutMS); } private void tankMove(){ - double travelledDistance = Math.abs(robot.left.getDistance()); + double travelledDistance = Math.abs(robot.frontLeft.getDistance()); double power = lerpPower(travelledDistance); double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu)); @@ -107,17 +107,19 @@ public class Move extends CyberarmState { double leftPower = power; double rightPower = power; // use +10% of power at 7 degrees of error to correct angle - double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1); - if (angleDiff < -0.5) { + double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power * 0.1); + if (angleDiff > -0.5) { leftPower += correctivePower; - } else if (angleDiff > 0.5) { + } else if (angleDiff < 0.5) { rightPower += correctivePower; } robot.left.set(leftPower); robot.right.set(rightPower); - if (robot.left.atTargetPosition() && robot.right.atTargetPosition()) { + 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); @@ -133,11 +135,11 @@ public class Move extends CyberarmState { double frontPower = power; double backPower = power; - // use +10% of power at 7 degrees of error to correct angle - double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1); - if (angleDiff < -0.5) { + // 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); + if (angleDiff > -0.5) { frontPower += correctivePower; - } else if (angleDiff > 0.5) { + } else if (angleDiff < 0.5) { backPower += correctivePower; } @@ -146,7 +148,8 @@ public class Move extends CyberarmState { robot.backLeft.set(-backPower); robot.backRight.set(backPower); - if (robot.frontLeft.atTargetPosition() && robot.backRight.atTargetPosition()) { + 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); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java index 8780c82..d0de0f9 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java @@ -11,49 +11,56 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot; public class PathSelector extends CyberarmState { private final RedCrabMinibot robot; private final int timeoutMS; - private final int fallbackPath; + private final int path; + private final double minConfidence; private List recognitions = new ArrayList<>(); public PathSelector(RedCrabMinibot robot, String groupName, String actionName) { this.robot = robot; this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); - this.fallbackPath = robot.config.variable(groupName, actionName, "fallbackPath").value(); + this.path = robot.config.variable(groupName, actionName, "path").value(); + this.minConfidence = robot.config.variable(groupName, actionName, "minConfidence").value(); } @Override public void init() { robot.tfod.setClippingMargins(0, 0, 0, 0); - robot.tfod.setMinResultConfidence(0.8f); + robot.tfod.setMinResultConfidence((float) minConfidence); - engine.blackboardSet("autonomousPath", fallbackPath); + engine.blackboardSet("autonomousPath", 0); } @Override public void exec() { + if (engine.blackboardGetInt("autonomousPath") != 0) { + this.finished(); + + return; + } + recognitions = robot.tfod.getRecognitions(); for (Recognition recognition : recognitions) { double x = (recognition.getLeft() + recognition.getRight()) / 2; double y = (recognition.getTop() + recognition.getBottom()) / 2; if (recognition.getLabel().equals("pixel")) { - if (x < 120) { - engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.LEFT.ordinal()); - } else if (x >= 120 && x < 240) { - engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.CENTER.ordinal()); - } else { - engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.RIGHT.ordinal()); - } + engine.blackboardSet("autonomousPath", path); } } if (runTime() >= timeoutMS) { - robot.visionPortal.close(); - robot.tfod.shutdown(); + stopVision(); + finished(); } } + private void stopVision() { + robot.visionPortal.close(); + robot.tfod.shutdown(); + } + @Override public void telemetry() { engine.telemetry.addLine(); 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 e9d640f..454e4a0 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 @@ -13,6 +13,7 @@ public class Rotate extends CyberarmState { final private double maxPower, minPower, lerpDegrees, headingDegrees, toleranceDegrees; final private int timeoutMS; + private boolean commitToRotation = false; public Rotate(RedCrabMinibot robot, String groupName, String actionName) { this.robot = robot; @@ -25,7 +26,7 @@ public class Rotate extends CyberarmState { this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value(); this.headingDegrees = robot.config.variable(groupName, actionName, "headingDEGREES").value(); - this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREEES").value(); + this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREES").value(); this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); } @@ -45,13 +46,17 @@ public class Rotate extends CyberarmState { double power = Utilities.lerp(minPower, maxPower, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0)); - if (angleDiff > 0) { - robot.left.set(-power); - robot.right.set(power); - } else { - robot.left.set(power); - robot.right.set(-power); + if (!commitToRotation) { + if (angleDiff < 0) { + robot.left.set(-power); + robot.right.set(power); + } else { + robot.left.set(power); + robot.right.set(-power); + } } + + commitToRotation = Math.abs(angleDiff) > 170; } @Override