diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/Vector2D.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/Vector2D.java new file mode 100644 index 0000000..64e2975 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/Vector2D.java @@ -0,0 +1,108 @@ +package dev.cyberarm.engine.V2; + +public class Vector2D { + double x, y; + public Vector2D() { + this.x = 0; + this.y = 0; + } + + public Vector2D(double x, double y) { + this.x = x; + this.y = y; + } + + public double x() { + return x; + } + + public double y() { + return y; + } + + public Vector2D plus(Vector2D other) { + return new Vector2D( + this.x + other.x(), + this.y + other.y() + ); + } + + public Vector2D plus(double other) { + return new Vector2D( + this.x + other, + this.y + other + ); + } + + public Vector2D minus(Vector2D other) { + return new Vector2D( + this.x - other.x(), + this.y - other.y() + ); + } + + public Vector2D minus(double other) { + return new Vector2D( + this.x - other, + this.y - other + ); + } + + public Vector2D multiply(Vector2D other) { + return new Vector2D( + this.x * other.x(), + this.y * other.y() + ); + } + + public Vector2D multiply(double other) { + return new Vector2D( + this.x * other, + this.y * other + ); + } + + public Vector2D divide(Vector2D other) { + return new Vector2D( + (this.x == 0 ? 0.0 : this.x / other.x()), + (this.y == 0 ? 0.0 : this.y / other.y()) + ); + } + + public Vector2D divide(double other) { + return new Vector2D( + (this.x == 0 ? 0.0 : this.x / other), + (this.y == 0 ? 0.0 : this.y / other) + ); + } + + public double magnitude() { + return Math.sqrt((this.x * this.x) + (this.y * this.y)); + } + + public Vector2D normalize() { + double mag = magnitude(); + + return divide(mag); + } + + public double distance(Vector2D other) { + return Math.sqrt((this.x - other.x) * (this.x - other.x) + (this.y - other.y) * (this.y - other.y)); + } + + public Vector2D inverse() { + return new Vector2D(1.0 / this.x, 1.0 / this.y); + } + + public double dot(Vector2D other) { + double product = 0.0; + product += this.x * other.x; + product += this.y * other.y; + + return product; + } + + public double sum() { + return this.x + this.y; + } +} 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 b21e8fb..2c0470f 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 @@ -6,7 +6,9 @@ import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.LED; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; @@ -85,6 +87,7 @@ public class RedCrabMinibot { public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; public final DcMotorEx deadWheelXLeft, deadWheelXRight; public final EncoderCustomKB2040 deadWheelYCenter; + public final DigitalChannel greenLED, redLED; final CyberarmEngine engine; @@ -245,6 +248,15 @@ public class RedCrabMinibot { if (autonomous) resetDeadWheels(); + /// LED(s) /// + greenLED = engine.hardwareMap.get(DigitalChannel.class, "led_green"); // GREEN LED NOT WORKING + redLED = engine.hardwareMap.get(DigitalChannel.class, "led_red"); + + greenLED.setMode(DigitalChannel.Mode.OUTPUT); + redLED.setMode(DigitalChannel.Mode.OUTPUT); + greenLED.setState(true); + redLED.setState(true); + // Bulk read from hubs Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL); 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 22b0917..2a1906e 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":"2024-01-30 14:46:07 -0600","spec_version":2,"revision":1131},"data":{"groups":[{"name":"Autonomous_BLUE_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":"Ix500"}]},{"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":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx220"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix10"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"02-02","comment":"@ServoMove - Move wrist servo","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx10"},{"name":"position","value":"Dx0.51"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix10"}]},{"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":true,"variables":[{"name":"forcePath","value":"Ix-1"}]},{"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":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"06-01","comment":"@ServoMove - Move wrist to stow position","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.74"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1"}]},{"name":"07-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":"08-00","comment":"@Move - Move back","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-207"},{"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 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":"10-00","comment":"@Move - Strafe to right","enabled":true,"variables":[{"name":"distanceMM","value":"Dx610"},{"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":"Ix3000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"11-00","comment":"@Rotate - Rotate 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":"12-00","comment":"@Move - Move forward unto Dawn","enabled":true,"variables":[{"name":"distanceMM","value":"Dx1043"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx900"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"13-00","comment":"@Rotate - Rotate to 270d","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":"14-00","comment":"@Move - Move forward to backstage","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2844"},{"name":"lerpMM_DOWN","value":"Dx500"},{"name":"lerpMM_UP","value":"Dx500"},{"name":"maxVelocityMM","value":"Dx1220"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"15-00","comment":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx180"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"16-00","comment":"@ServoMove - Open left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]},{"name":"17-00","comment":"@ClawArmMove - Move claw arm to stow","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2500"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"17-01","comment":"@ServoMove - close left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.2"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]}]},{"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":"Ix500"}]},{"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":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx220"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix10"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"02-02","comment":"@ServoMove - Move wrist servo","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx10"},{"name":"position","value":"Dx0.51"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix10"}]},{"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":true,"variables":[{"name":"forcePath","value":"Ix-1"}]},{"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":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"06-01","comment":"@ServoMove - Move wrist to stow position","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.74"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1"}]},{"name":"07-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":"08-00","comment":"@Move - Move back","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-207"},{"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 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":"10-00","comment":"@Move - Strafe to left","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-610"},{"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":"Ix3000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"11-00","comment":"@Rotate - Rotate 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":"12-00","comment":"@Move - Move forward unto Dawn","enabled":true,"variables":[{"name":"distanceMM","value":"Dx1043"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx900"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"13-00","comment":"@Rotate - Rotate 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":"14-00","comment":"@Move - Move forward to backstage","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2844"},{"name":"lerpMM_DOWN","value":"Dx500"},{"name":"lerpMM_UP","value":"Dx500"},{"name":"maxVelocityMM","value":"Dx1220"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"15-00","comment":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx180"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"16-00","comment":"@ServoMove - Open left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]},{"name":"17-00","comment":"@ClawArmMove - Move claw arm to stow","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2500"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"17-01","comment":"@ServoMove - close left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.2"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]}]},{"name":"Autonomous_RED_Backstage","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":"Ix500"}]},{"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":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx220"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix10"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"02-02","comment":"@ServoMove - Move wrist servo","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx10"},{"name":"position","value":"Dx0.51"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix10"}]},{"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":true,"variables":[{"name":"forcePath","value":"Ix-1"}]},{"name":"06-00","comment":"@ClawArmMove - Move claw arm to deposit position","enabled":true,"variables":[{"name":"angle","value":"Dx165.0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"06-01","comment":"@ServoMove - Move wrist to deposit position","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.508"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1"}]},{"name":"07-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":"08-00","comment":"@Move - Move back","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-207"},{"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 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":"10-00","comment":"@Move - Strafe to the right","enabled":true,"variables":[{"name":"distanceMM","value":"Dx610"},{"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":"11-00","comment":"@Rotate - Rotate 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":"12-00","comment":"@Move - Strafe to the left","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-101"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxtrue"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"13-00","comment":"@Rotate - Rotate 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":"14-00","comment":"@Move - Move to backboard","enabled":true,"variables":[{"name":"distanceMM","value":"Dx475"},{"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":"15-00","comment":"@ServoMove - Open left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix1000"}]},{"name":"16-00","comment":"@Move - Move back a bit","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-75"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"17-00","comment":"@Move - Move right the right","enabled":true,"variables":[{"name":"distanceMM","value":"Dx600"},{"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":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"17-01","comment":"@ClawArmMove - Move claw arm to stow position","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"17-02","comment":"@ServoMove - Move wrist to stow position","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":"17-03","comment":"@ServoMove - Close left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1"},{"name":"position","value":"Dx0.2"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix1000"}]}]},{"name":"Autonomous_SpikePath_CENTER","actions":[{"name":"01-00","comment":"@Move - Move forward to spike mark","enabled":true,"variables":[{"name":"distanceMM","value":"Dx152"},{"name":"lerpMM_DOWN","value":"Dx50"},{"name":"lerpMM_UP","value":"Dx50"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"02-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":"02-01","comment":"@ServoMove - Open right claw (purple)","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx500"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix750"}]},{"name":"04-00","comment":"@Move - Move back from spike mark","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-152"},{"name":"lerpMM_DOWN","value":"Dx50"},{"name":"lerpMM_UP","value":"Dx50"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"05-00","comment":"@ServoMove - Close right claw (purple)","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx500"},{"name":"position","value":"Dx0.74"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix750"}]},{"name":"05-01","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":"Autonomous_SpikePath_LEFT","actions":[{"name":"00-00","comment":"@Rotate - Rotate robot to towards left spike mark","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx316"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"01-00","comment":"@Move - Move up a bit","enabled":false,"variables":[{"name":"distanceMM","value":"Dx101"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"02-00","comment":"@ServoMove - Drop pixel","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix500"}]},{"name":"03-00","comment":"@ServoMove - Close claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.72"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"04-00","comment":"@Move - Move back a bit","enabled":false,"variables":[{"name":"distanceMM","value":"Dx-101"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"05-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":"Autonomous_SpikePath_RIGHT","actions":[{"name":"00-00","comment":"@Rotate - Rotate robot to towards right spike mark","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx45"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"01-00","comment":"@Move - Move up a bit","enabled":true,"variables":[{"name":"distanceMM","value":"Dx50"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx10"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"02-00","comment":"@ServoMove - Drop pixel","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix500"}]},{"name":"03-00","comment":"@ServoMove - Close claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.72"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"04-00","comment":"@Move - Move back a bit","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-50"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx2"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"05-00","comment":"@Rotate - Rotate robot to 0d","enabled":false,"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":"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":"Dx242"},{"name":"collect_float_angle","value":"Dx237"},{"name":"deposit_angle","value":"Dx165.0"},{"name":"gear_ratio","value":"Dx1"},{"name":"kD","value":"Dx1"},{"name":"kF","value":"Dx0.0"},{"name":"kI","value":"Dx1.5"},{"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":"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.52"},{"name":"collect_position","value":"Dx0.505"},{"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.35"},{"name":"up_position","value":"Dx0.8"}]},{"name":"Winch_Tuning","comment":"Winch","enabled":true,"variables":[{"name":"max_speed","value":"Dx1.0"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","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":"Dx0"},{"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-30 17:45:43 -0600","spec_version":2,"revision":1146},"data":{"groups":[{"name":"Autonomous_BLUE_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":"Ix500"}]},{"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":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx220"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix10"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"02-02","comment":"@ServoMove - Move wrist servo","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx10"},{"name":"position","value":"Dx0.51"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix10"}]},{"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":true,"variables":[{"name":"forcePath","value":"Ix-1"}]},{"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":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"06-01","comment":"@ServoMove - Move wrist to stow position","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.74"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1"}]},{"name":"07-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":"08-00","comment":"@Move - Move back","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-207"},{"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 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":"10-00","comment":"@Move - Strafe to right","enabled":true,"variables":[{"name":"distanceMM","value":"Dx610"},{"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":"Ix3000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"11-00","comment":"@Rotate - Rotate 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":"12-00","comment":"@Move - Move forward unto Dawn","enabled":true,"variables":[{"name":"distanceMM","value":"Dx1043"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx900"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"13-00","comment":"@Rotate - Rotate to 270d","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":"14-00","comment":"@Move - Move forward to backstage","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2844"},{"name":"lerpMM_DOWN","value":"Dx500"},{"name":"lerpMM_UP","value":"Dx500"},{"name":"maxVelocityMM","value":"Dx1220"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"15-00","comment":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx180"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"16-00","comment":"@ServoMove - Open left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]},{"name":"17-00","comment":"@ClawArmMove - Move claw arm to stow","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2500"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"17-01","comment":"@ServoMove - close left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.2"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]}]},{"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":"Ix500"}]},{"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":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx220"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix10"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"02-02","comment":"@ServoMove - Move wrist servo","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx10"},{"name":"position","value":"Dx0.51"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix10"}]},{"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":true,"variables":[{"name":"forcePath","value":"Ix-1"}]},{"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":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"06-01","comment":"@ServoMove - Move wrist to stow position","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.74"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1"}]},{"name":"07-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":"08-00","comment":"@Move - Move back","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-207"},{"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 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":"10-00","comment":"@Move - Strafe to left","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-610"},{"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":"Ix3000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"11-00","comment":"@Rotate - Rotate 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":"12-00","comment":"@Move - Move forward unto Dawn","enabled":true,"variables":[{"name":"distanceMM","value":"Dx1043"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx900"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"13-00","comment":"@Rotate - Rotate 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":"14-00","comment":"@Move - Move forward to backstage","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2844"},{"name":"lerpMM_DOWN","value":"Dx500"},{"name":"lerpMM_UP","value":"Dx500"},{"name":"maxVelocityMM","value":"Dx1220"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix8000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"15-00","comment":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx180"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"16-00","comment":"@ServoMove - Open left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]},{"name":"17-00","comment":"@ClawArmMove - Move claw arm to stow","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix2500"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"17-01","comment":"@ServoMove - close left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.2"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix250"}]}]},{"name":"Autonomous_RED_Backstage","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":"Ix500"}]},{"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":"@ClawArmMove - Move claw arm down","enabled":true,"variables":[{"name":"angle","value":"Dx220"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix10"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"02-02","comment":"@ServoMove - Move wrist servo","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx10"},{"name":"position","value":"Dx0.51"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix10"}]},{"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":true,"variables":[{"name":"forcePath","value":"Ix-1"}]},{"name":"06-00","comment":"@ClawArmMove - Move claw arm to deposit position","enabled":true,"variables":[{"name":"angle","value":"Dx165.0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"06-01","comment":"@ServoMove - Move wrist to deposit position","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.508"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix1"}]},{"name":"07-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":"08-00","comment":"@Move - Move back","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-207"},{"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 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":"10-00","comment":"@Move - Strafe to the right","enabled":true,"variables":[{"name":"distanceMM","value":"Dx610"},{"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":"11-00","comment":"@Rotate - Rotate 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":"12-00","comment":"@Move - Strafe to the left","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-101"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxtrue"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"13-00","comment":"@Rotate - Rotate 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":"14-00","comment":"@Move - Move to backboard","enabled":true,"variables":[{"name":"distanceMM","value":"Dx475"},{"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":"15-00","comment":"@ServoMove - Open left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix1000"}]},{"name":"16-00","comment":"@Move - Move back a bit","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-75"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"17-00","comment":"@Move - Move right the right","enabled":true,"variables":[{"name":"distanceMM","value":"Dx600"},{"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":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"17-01","comment":"@ClawArmMove - Move claw arm to stow position","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Ix1"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"17-02","comment":"@ServoMove - Move wrist to stow position","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":"17-03","comment":"@ServoMove - Close left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1"},{"name":"position","value":"Dx0.2"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix1000"}]}]},{"name":"Autonomous_SpikePath_CENTER","actions":[{"name":"01-00","comment":"@Move - Move forward to spike mark","enabled":true,"variables":[{"name":"distanceMM","value":"Dx152"},{"name":"lerpMM_DOWN","value":"Dx50"},{"name":"lerpMM_UP","value":"Dx50"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"02-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":"02-01","comment":"@ServoMove - Open right claw (purple)","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx500"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix750"}]},{"name":"04-00","comment":"@Move - Move back from spike mark","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-152"},{"name":"lerpMM_DOWN","value":"Dx50"},{"name":"lerpMM_UP","value":"Dx50"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"05-00","comment":"@ServoMove - Close right claw (purple)","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx500"},{"name":"position","value":"Dx0.74"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix750"}]},{"name":"05-01","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":"Autonomous_SpikePath_LEFT","actions":[{"name":"00-00","comment":"@Rotate - Rotate robot to towards left spike mark","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx316"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"01-00","comment":"@Move - Move up a bit","enabled":false,"variables":[{"name":"distanceMM","value":"Dx101"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"02-00","comment":"@ServoMove - Drop pixel","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix500"}]},{"name":"03-00","comment":"@ServoMove - Close claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.72"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"04-00","comment":"@Move - Move back a bit","enabled":false,"variables":[{"name":"distanceMM","value":"Dx-101"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx25"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"05-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":"Autonomous_SpikePath_RIGHT","actions":[{"name":"00-00","comment":"@Rotate - Rotate robot to towards right spike mark","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx45"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"01-00","comment":"@Move - Move up a bit","enabled":true,"variables":[{"name":"distanceMM","value":"Dx50"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx10"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"02-00","comment":"@ServoMove - Drop pixel","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix500"}]},{"name":"03-00","comment":"@ServoMove - Close claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.72"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"04-00","comment":"@Move - Move back a bit","enabled":true,"variables":[{"name":"distanceMM","value":"Dx-50"},{"name":"lerpMM_DOWN","value":"Dx25"},{"name":"lerpMM_UP","value":"Dx2"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"05-00","comment":"@Rotate - Rotate robot to 0d","enabled":false,"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":"Autonomous_Z_DEBUG","actions":[{"name":"00-00","comment":"@MoveToCoordinate - Trial run(s)","enabled":true,"variables":[{"name":"angleToleranceDEGREES","value":"Dx5"},{"name":"minDistanceMM","value":"Dx10.0"},{"name":"stopDrivetrain","value":"Bxtrue"},{"name":"targetAngleDEGREES","value":"Dx0"},{"name":"targetPosMM","value":"Sx500.0x0.0"},{"name":"timeoutMS","value":"Ix15000"}]}]},{"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":"Dx242"},{"name":"collect_float_angle","value":"Dx237"},{"name":"deposit_angle","value":"Dx165.0"},{"name":"gear_ratio","value":"Dx1"},{"name":"kD","value":"Dx1"},{"name":"kF","value":"Dx0.0"},{"name":"kI","value":"Dx1.5"},{"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":"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.52"},{"name":"collect_position","value":"Dx0.505"},{"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.35"},{"name":"up_position","value":"Dx0.8"}]},{"name":"Winch_Tuning","comment":"Winch","enabled":true,"variables":[{"name":"max_speed","value":"Dx1.0"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","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":"Dx0"},{"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/RedCrabAutonomousDebuggingOnlyEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java new file mode 100644 index 0000000..1dd38d7 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java @@ -0,0 +1,25 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.states.ClawArmTask; + +@Autonomous(name = "Cyberarm Red Crab Auto DEBUGGING", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabAutonomousEngine { + @Override + public void setup() { + robot = new RedCrabMinibot(true); + addTask(new ClawArmTask(robot)); + + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + robot, + RedCrabMinibot.class, + "Autonomous_Z_DEBUG" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/MoveToCoordinate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/MoveToCoordinate.java new file mode 100644 index 0000000..4370f5f --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/MoveToCoordinate.java @@ -0,0 +1,114 @@ +package dev.cyberarm.minibots.red_crab.states; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.engine.V2.Vector2D; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class MoveToCoordinate extends CyberarmState { + private final RedCrabMinibot robot; + private final String groupName, actionName; + private final boolean stopDrivetrain; + private final int timeoutMS; + private final double targetAngleDegrees, angleToleranceDegrees, minDistanceMM, targetX_MM, targetY_MM; + private final Vector2D targetPosMM; + + public MoveToCoordinate(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + this.stopDrivetrain = robot.config.variable(groupName, actionName, "stopDrivetrain").value(); + this.angleToleranceDegrees = robot.config.variable(groupName, actionName, "angleToleranceDEGREES").value(); + this.targetAngleDegrees = robot.config.variable(groupName, actionName, "targetAngleDEGREES").value(); + this.minDistanceMM = robot.config.variable(groupName, actionName, "minDistanceMM").value(); + String targetPosMM = robot.config.variable(groupName, actionName, "targetPosMM").value(); + String[] targetPos = targetPosMM.split("x"); + this.targetX_MM = Double.parseDouble(targetPos[0]); + this.targetY_MM = Double.parseDouble(targetPos[1]); + + this.targetPosMM = new Vector2D(this.targetX_MM, this.targetY_MM); + } + + @Override + public void exec() { + Vector2D robotPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM()); + Vector2D direction = targetPosMM.minus(robotPosMM).normalize(); + double distanceMM = robotPosMM.distance(this.targetPosMM); + + if (distanceMM <= minDistanceMM || runTime() >= timeoutMS) { + if (stopDrivetrain) { + robot.frontLeft.setVelocity(0); + robot.frontRight.setVelocity(0); + robot.backLeft.setVelocity(0); + robot.backRight.setVelocity(0); + } + + finished(); + return; + } + + drivetrain(direction); + } + + private void drivetrain(Vector2D direction) { + // https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric + + double y = direction.x(); // robot forward is in the X axis + double x = direction.y(); // robot side to side is on the Y axis + // FIXME: Dynamically set rotation as needed to achieve target heading using shortest rotation + double rx = 0;//engine.gamepad1.right_stick_x; + + double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + // Rotate the movement direction counter to the bot's rotation + double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); + double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); + + rotX = rotX * 1.1; // Counteract imperfect strafing + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY - rotX - rx) / denominator; + double backRightPower = (rotY + rotX - rx) / denominator; + + double maxVelocity = Utilities.unitToTicks( + RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.DRIVETRAIN_GEAR_RATIO, + RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + RedCrabMinibot.DRIVETRAIN_VELOCITY_MAX_MM); + double slowVelocity = Utilities.unitToTicks( + RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.DRIVETRAIN_GEAR_RATIO, + RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + RedCrabMinibot.DRIVETRAIN_VELOCITY_SLOW_MM); + // FIXME: Lerp up/down when starting move and getting close to target + double velocity = slowVelocity; //robotSlowMode ? slowVelocity : maxVelocity; + + robot.frontLeft.setVelocity(frontLeftPower * velocity); + robot.backLeft.setVelocity(backLeftPower * velocity); + robot.frontRight.setVelocity(frontRightPower * velocity); + robot.backRight.setVelocity(backRightPower * velocity); + } + + @Override + public void telemetry() { + Vector2D robotPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM()); + Vector2D direction = targetPosMM.minus(robotPosMM).normalize(); + + engine.telemetry.addData("Current Position MM", "X: %.2f Y: %.2f", robotPosMM.x(), robotPosMM.y()); + engine.telemetry.addData("Target Position MM", "X: %.2f Y: %.2f", targetPosMM.x(), targetPosMM.y()); + engine.telemetry.addData("Move NORMAL", "X: %.2f Y: %.2f", direction.x(), direction.y()); + engine.telemetry.addData("Distance MM", "%.2fmm", robotPosMM.distance(targetPosMM)); + } +}