RedCrab: Make LED rail show launch confirmation progress in teleop, add null check for localizer loadConstants function

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java

@@ -105,6 +105,9 @@ public class RedCrabMinibot {
     public TimeCraftersConfiguration config;
     private final PIDFController clawArmPIDFController;
     public final String webcamName = "Webcam 1";
+    public boolean droneLaunchRequested = false;
+    public double droneLastLaunchRequestStartMS = 0;
+    public boolean droneLaunchAuthorized = false;
 
     private long lastClawArmOverCurrentAnnounced = 0;
     private boolean clawArmOverCurrent = false;
@@ -387,7 +390,8 @@ public class RedCrabMinibot {
         RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value();
         RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value();
 
-        localizer.loadConfigConstants();
+        if (RedCrabMinibot.localizer != null)
+            RedCrabMinibot.localizer.loadConfigConstants();
     }
 
     public void resetDeadWheels() {
@@ -653,6 +657,24 @@ public class RedCrabMinibot {
                 ledAnimateProgress(LED_ON, LED_OFF, (engine.runTime() - 23_000.0) / 5_000.0);
             }
         } else {
+            // Show progress of drone launch authorization
+            if (droneLaunchRequested) {
+                if (droneLaunchAuthorized) {
+                    ledTopRed.setState(LED_OFF);
+                    ledTopGreen.setState(LED_ON);
+
+                    ledSetRailRedLEDs(LED_ON);
+                    ledSetRailRedLEDs(LED_ON);
+                } else {
+                    ledTopRed.setState(LED_ON);
+                    ledTopGreen.setState(LED_ON);
+
+                    ledAnimateProgress(LED_ON, LED_ON, (engine.runTime() - droneLastLaunchRequestStartMS) / RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS);
+                }
+
+                return;
+            }
+
             if (engine.runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
                 ledTopRed.setState(LED_OFF);
                 ledTopGreen.setState(LED_ON);

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java

@@ -17,9 +17,6 @@ public class Pilot extends CyberarmState {
     private boolean rightClawOpen = false;
     private int clawArmPosition = RedCrabMinibot.ClawArm_INITIAL;
     private boolean hookArmUp = false;
-    private boolean droneLaunchAuthorized = false;
-    private boolean droneLaunchRequested = false;
-    private double droneLastLaunchRequestStartMS = 0;
     private double odometryResetRequestStartMS = 0;
     private boolean odometryResetRequested = false;
     private boolean odometryResetRequestLeftStick = false;
@@ -42,7 +39,7 @@ public class Pilot extends CyberarmState {
         clawArmAndWristController();
         clawController();
         droneLatchController();
-        hookArmController(); // disabled for swrist debug
+        hookArmController();
         winchController();
         odometryDebugController();
 
@@ -54,8 +51,8 @@ public class Pilot extends CyberarmState {
         if (gamepad == engine.gamepad1) {
             switch (button) {
                 case "y":
-                    droneLaunchRequested = true;
-                    droneLastLaunchRequestStartMS = runTime();
+                    robot.droneLaunchRequested = true;
+                    robot.droneLastLaunchRequestStartMS = runTime();
                     break;
                 case "x":
                     hookArmUp = true;
@@ -117,8 +114,8 @@ public class Pilot extends CyberarmState {
         if (gamepad == engine.gamepad1) {
             switch (button) {
                 case "y":
-                    droneLaunchRequested = false;
-                    droneLastLaunchRequestStartMS = runTime();
+                    robot.droneLaunchRequested = false;
+                    robot.droneLastLaunchRequestStartMS = runTime();
                     break;
                 case "left_stick_button":
                     odometryResetRequestLeftStick = false;
@@ -234,16 +231,16 @@ public class Pilot extends CyberarmState {
     }
 
     private void droneLatchController() {
-        if (droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
-            droneLaunchAuthorized = true;
+        if (robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
+            robot.droneLaunchAuthorized = true;
 
-        if (droneLaunchAuthorized) {
+        if (robot.droneLaunchAuthorized) {
             robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION);
         }
 
         // Auto reset drone latch after DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1 second
-        if (!droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) {
-            droneLaunchAuthorized = false;
+        if (!robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) {
+            robot.droneLaunchAuthorized = false;
 
             robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION);
         }

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java

@@ -0,0 +1,106 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
+
+public class StateBackDropBlue extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.imu.resetYaw();
+        robot.leftClaw.setPosition(0.25);
+        robot.rightClaw.setPosition(0.6);
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("State BackDrop red");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0"));
+
+        // bring arm to hover
+        addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
+
+        //open claw
+        addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "5-00-0"));
+
+        // drive towards backboard
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
+
+        // drive into board
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
+
+        //open right close left
+        addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
+
+        // bring arm up
+        addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
+
+        // drivw to park
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1"));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
+
+
+
+    // 65, on the left, 235 on the right
+
+
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java

@@ -64,6 +64,8 @@ public class StateBackDropRed extends CyberarmEngine {
         addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
         addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
         addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1"));
 
         // pause
         addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
@@ -71,15 +73,27 @@ public class StateBackDropRed extends CyberarmEngine {
         // drive into board
         addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
         addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2"));
 
         // pause
         addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
 
         //open right close left
         addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
+
         // bring arm up
         addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
 
+        // drivw to park
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1"));
+
+        addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
+
+
+
 
 
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java

@@ -69,7 +69,7 @@ public class DriveToCoordinatesState extends CyberarmState {
 
                 if (Math.abs(robot.positionX - robot.xTarget) < 5
                         && Math.abs(robot.positionY - robot.yTarget) < 5
-                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) < 2) {
+                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
                     setHasFinished(true);
                 }
             }
@@ -80,7 +80,7 @@ public class DriveToCoordinatesState extends CyberarmState {
 
                 if (Math.abs(robot.positionX - robot.xTarget) < 5
                         && Math.abs(robot.positionY - robot.yTarget) < 5
-                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) < 2) {
+                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
                     setHasFinished(true);
                 }
             }

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java

@@ -71,8 +71,8 @@ public class CompetitionTeleOpState extends CyberarmState {
     // ---------------------------------------------------------------------------------------------------------------Arm control Variables:
     public String armPos = "collect";
     // chin up servo
-    public static double chinUpServoUp = 0.58;
-    public static double chinUpServoDown = 1;
+    public static double chinUpServoUp = 0.7;
+    public static double chinUpServoDown = 0;
     public long lastExecutedTime;
 
 

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java

@@ -85,7 +85,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
 //        lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
         gp1checker = new GamepadChecker(engine, engine.gamepad1);
-        gp2checker = new GamepadChecker(engine, engine.gamepad1);
+        gp2checker = new GamepadChecker(engine, engine.gamepad2);
 
         lastMoveTime = System.currentTimeMillis();
         lastDistRead = System.currentTimeMillis();
@@ -138,6 +138,12 @@ public class SodiPizzaTeleOPState extends CyberarmState {
             robot.imu.resetYaw();
         }
 
+        if (Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) > 0.000001 &&
+            Math.abs(robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)) > 0.000001 &&
+            Math.abs(robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)) > 0.000001) {
+                robot.imu.resetYaw();
+        }
+
         if (engine.gamepad1.left_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
                 robot.launcher.setPosition(0.98);
@@ -163,12 +169,12 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.dpad_up) {
-            if (System.currentTimeMillis() - lastMoveTime >= 200) {
+            if (System.currentTimeMillis() - lastMoveTime >= 150) {
                 robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
                 lastMoveTime = System.currentTimeMillis();
             }
         } else if (engine.gamepad1.dpad_down) {
-            if (System.currentTimeMillis() - lastMoveTime >= 200) {
+            if (System.currentTimeMillis() - lastMoveTime >= 150) {
                 robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
                 lastMoveTime = System.currentTimeMillis();
             }