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 78e139e..123787d 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,9 +1,6 @@ package dev.cyberarm.minibots.red_crab; 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; @@ -12,7 +9,6 @@ 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; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; @@ -46,6 +42,7 @@ public class RedCrabMinibot { public static double CLAW_ARM_MAX_SPEED = 0.5; public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10; private static double CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = 1588.0; + private static long CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = 5000; public static double CLAW_ARM_kP = 0.0; public static double CLAW_ARM_kI = 0.0; public static double CLAW_ARM_kD = 0.0; @@ -92,6 +89,8 @@ public class RedCrabMinibot { private final PIDFController clawArmPIDFController; public final String webcamName = "Webcam 1"; + private long lastClawArmOverCurrentAnnounced = 0; + private boolean clawArmOverCurrent = false; public enum Path { LEFT, CENTER, @@ -258,6 +257,7 @@ 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(); RedCrabMinibot.CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = config.variable("Robot", "ClawArm_Tuning", "max_current_milliamps").value(); + RedCrabMinibot.CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = config.variable("Robot", "ClawArm_Tuning", "warn_overcurrent_after_ms").value(); /// WINCH @@ -412,6 +412,18 @@ public class RedCrabMinibot { public void controlClawArm() { Action action = config.action("Robot", "ClawArm_Tuning"); + long milliseconds = System.currentTimeMillis(); + if (clawArm.isOverCurrent()) + { + if (milliseconds - lastClawArmOverCurrentAnnounced >= CLAW_ARM_WARN_OVERCURRENT_AFTER_MS) { + lastClawArmOverCurrentAnnounced = System.currentTimeMillis(); + + engine.telemetry.speak("WARNING. ARM. OVER. CURRENT."); + } + } else { + lastClawArmOverCurrentAnnounced = milliseconds; + } + double ticksInDegree = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1); for (Variable v : action.getVariables()) { @@ -450,6 +462,18 @@ public class RedCrabMinibot { double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES); + double currentAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition()); + double targetAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getTargetPosition()); + double angleDiff = Math.abs(Utilities.angleDiff(currentAngle, targetAngle)); + + // Turn off motor if it is stowed or all the way down + if (targetAngle <= CLAW_ARM_STOW_ANGLE + 5.0 && angleDiff <= 5.0) { + velocity = 0.0; + } else if (targetAngle >= CLAW_ARM_COLLECT_ANGLE - 5.0 && angleDiff <= 5.0) + { + velocity = 0.0; + } + clawArm.setVelocity(velocity); } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java index 76076c8..43bb3df 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java @@ -158,6 +158,15 @@ public class TeamPropVisionProcessor implements VisionProcessor { return saturationRight; } + public double getHighestSaturation() { + if (getSaturationLeft() > getSaturationCenter() && getSaturationLeft() > getSaturationRight()) + return getSaturationLeft(); + if (getSaturationCenter() > getSaturationLeft() && getSaturationCenter() > getSaturationRight()) + return getSaturationCenter(); + + return getSaturationRight(); + } + private android.graphics.Rect makeDrawableRect(Rect rect, float scaleBmpPxToCanvasPx) { int left = Math.round(rect.x * scaleBmpPxToCanvasPx); int top = Math.round(rect.y * scaleBmpPxToCanvasPx); 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 ef4f270..c231009 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-30 12:42:26 -0600","spec_version":2,"revision":629},"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":"Dx240"},{"name":"collect_float_angle","value":"Dx220"},{"name":"deposit_angle","value":"Dx165.0"},{"name":"gear_ratio","value":"Dx1"},{"name":"kD","value":"Dx1"},{"name":"kF","value":"Dx0.0"},{"name":"kI","value":"Dx3"},{"name":"kP","value":"Dx6"},{"name":"KPosP","value":"Dx1"},{"name":"max_current_milliamps","value":"Dx1588.0"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx60"},{"name":"motor_ticks","value":"Ix8192"},{"name":"stow_angle","value":"Dx18"},{"name":"tolerance","value":"Ix288"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.51"},{"name":"collect_position","value":"Dx0.51"},{"name":"deposit_position","value":"Dx0.508"},{"name":"stow_position","value":"Dx0.74"}]},{"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.35"}]},{"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 +{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2024-01-06 12:14:40 -0600","spec_version":2,"revision":686},"data":{"groups":[{"name":"Autonomous_RED_Audience","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx130.0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2500"},{"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.74"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1000"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx18.0"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix5000"}]},{"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":"02-01","comment":"@ServoMove - Move wrist servo","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.51"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1000"}]},{"name":"02-02","comment":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx220"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"04-00","comment":"@Rotate - Rotate robot to 0d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"05-00","comment":"@PathEnactor - Enact path; paths must return to \"origin\" when done","enabled":false,"variables":[]},{"name":"06-00","comment":"@ClawArmMove - Move claw arm to stow position","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"07-00","comment":"@Rotate - Rotate robot to 90d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx270"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"08-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":"09-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":"10-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":"Autonomous_SpikePath_CENTER","actions":[]},{"name":"Autonomous_SpikePath_LEFT","actions":[]},{"name":"Autonomous_SpikePath_RIGHT","actions":[]},{"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":"Dx240"},{"name":"collect_float_angle","value":"Dx220"},{"name":"deposit_angle","value":"Dx165.0"},{"name":"gear_ratio","value":"Dx1"},{"name":"kD","value":"Dx1"},{"name":"kF","value":"Dx0.0"},{"name":"kI","value":"Dx3"},{"name":"kP","value":"Dx6"},{"name":"KPosP","value":"Dx1"},{"name":"max_current_milliamps","value":"Dx1588.0"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx90"},{"name":"motor_ticks","value":"Ix8192"},{"name":"stow_angle","value":"Dx0"},{"name":"tolerance","value":"Ix288"},{"name":"warn_overcurrent_after_ms","value":"Lx5000"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.51"},{"name":"collect_position","value":"Dx0.51"},{"name":"deposit_position","value":"Dx0.508"},{"name":"stow_position","value":"Dx0.74"}]},{"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.35"}]},{"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":"Ix1000"},{"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/engines/RedCrabAutonomousBlueAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java index 544d31f..8e17fff 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 @@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask; public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine { @Override public void setup() { - RedCrabMinibot robot = new RedCrabMinibot(true); - blackboardSet("clawArmPower", 0.0); - + robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); setupFromConfig( 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 9533993..12e1140 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 @@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask; public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine { @Override public void setup() { - RedCrabMinibot robot = new RedCrabMinibot(true); - blackboardSet("clawArmPower", 0.0); - + robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); setupFromConfig( 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 57d932f..e160c1f 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 @@ -2,12 +2,18 @@ package dev.cyberarm.minibots.red_crab.engines; import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; public abstract class RedCrabAutonomousEngine extends CyberarmEngine { + protected RedCrabMinibot robot; + @Override public void loop() { Utilities.hubsClearBulkReadCache(hardwareMap); + if (robot != null) + robot.standardTelemetry(); + 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 779359d..43929aa 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 @@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask; public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine { @Override public void setup() { - RedCrabMinibot robot = new RedCrabMinibot(true); - blackboardSet("clawArmPower", 0.0); - + robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); setupFromConfig( 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 9066d58..a9bc032 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 @@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask; public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine { @Override public void setup() { - RedCrabMinibot robot = new RedCrabMinibot(true); - blackboardSet("clawArmPower", 0.0); - + robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); setupFromConfig( diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java index bea9898..da4636c 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java @@ -25,7 +25,7 @@ public class PathEnactor extends CyberarmState { break; } - this.pathGroupName = String.format("AutonomousPixelPath_%s", path); + this.pathGroupName = String.format("Autonomous_SpikePath_%s", path); } @Override 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 b6cc8c9..75ff343 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 @@ -18,7 +18,6 @@ public class PathSelector extends CyberarmState { private final int timeoutMS; private final int path; private final double minConfidence; - private List recognitions = new ArrayList<>(); public PathSelector(RedCrabMinibot robot, String groupName, String actionName) { this.robot = robot; @@ -34,40 +33,27 @@ public class PathSelector extends CyberarmState { robot.visionPortal = VisionPortal.easyCreateWithDefaults( engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.teamProp); -// robot.tfPixel.setClippingMargins(0, 0, 0, 0); -// robot.tfPixel.setMinResultConfidence((float) minConfidence); - engine.blackboardSet("autonomousPath", path); } @Override public void exec() { - if (engine.blackboardGetInt("autonomousPath") != 0) { - this.finished(); - - return; - } - -// recognitions = robot.tfPixel.getRecognitions(); -// for (Recognition recognition : recognitions) { -// double x = (recognition.getLeft() + recognition.getRight()) / 2; -// double y = (recognition.getTop() + recognition.getBottom()) / 2; -// -// if (recognition.getLabel().equals("pixel")) { -// engine.blackboardSet("autonomousPath", path); -// } -// } - - switch (robot.teamProp.getLocation()) { - case LEFT: - engine.blackboardSet("autonomousPath", 0); - break; - case CENTER: - engine.blackboardSet("autonomousPath", 1); - break; - case RIGHT: - engine.blackboardSet("autonomousPath", 2); - break; + // If we've got enough Saturation for our min confidence then do the needful. + if (robot.teamProp.getHighestSaturation() >= minConfidence) { + switch (robot.teamProp.getLocation()) { + case LEFT: + engine.blackboardSet("autonomousPath", 0); + break; + case CENTER: + engine.blackboardSet("autonomousPath", 1); + break; + case RIGHT: + engine.blackboardSet("autonomousPath", 2); + break; + } + } else + { + engine.blackboardSet("autonomousPath", path); // min confidence not meant, default to center. } if (runTime() >= timeoutMS) { @@ -85,19 +71,9 @@ public class PathSelector extends CyberarmState { public void telemetry() { engine.telemetry.addLine(); -// engine.telemetry.addData("Saturation", robot.spikeMark.getSaturation()); - -// engine.telemetry.addData("# Objects Detected", recognitions.size()); -// -// for(Recognition recognition : recognitions) { -// double x = (recognition.getLeft() + recognition.getRight()) / 2; -// double y = (recognition.getTop() + recognition.getBottom()) / 2; -// -// engine.telemetry.addLine(); -// -// engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); -// engine.telemetry.addData("- Position", "%.0f / %.0f", x, y); -// engine.telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); -// } + engine.telemetry.addData("Location", robot.teamProp.getLocation()); + engine.telemetry.addData("Saturation Left", robot.teamProp.getSaturationLeft()); + engine.telemetry.addData("Saturation Center", robot.teamProp.getSaturationCenter()); + engine.telemetry.addData("Saturation Right", robot.teamProp.getSaturationRight()); } } 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 ccf20f3..172da1d 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 @@ -209,17 +209,6 @@ public class Pilot extends CyberarmState { robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION); break; - - } - - if (clawArmPosition == RedCrabMinibot.ClawArm_COLLECT && - robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks( - RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, - RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, - RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 5.0) { - robot.clawArm.setPower(0); - } else { - robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED); } }