Merge remote-tracking branch 'origin/master'

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/CompetitionBurnsvilleAudienceBlue.java

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,7 +16,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp")
-
+@Disabled
 public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
 
     CompetitionRobotV1 robot;

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

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
 
 public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine {
 

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

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,6 +16,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
 
 public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
 

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

@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Autonomous(name = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
+@Disabled
 
 public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
 

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

@@ -0,0 +1,73 @@
+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 Audience blue", preselectTeleOp = "Competition V1 TeleOp")
+
+public class StateAudienceBlue 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", "9-00-0"));
+
+
+
+
+
+
+
+
+
+
+    }
+
+}

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

@@ -0,0 +1,70 @@
+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 Audience red", preselectTeleOp = "Competition V1 TeleOp")
+
+public class StateAudienceRed 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 blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
+
+        // bring arm to hover
+        addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
+
+        //open claw
+        addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0"));
+
+
+
+
+
+
+
+    }
+
+}

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 blue", 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 blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+
+        this.robot.setup();
+
+        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0"));
+
+        // drive to the left, center, or right spike mark
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
+
+        // bring arm to hover
+        addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
+
+        //open claw
+        addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "5-00-0"));
+
+        // drive towards backboard
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-1"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-1"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop blue", "6-00-0"));
+
+        // drive into board
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-2"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-2"));
+
+        // pause
+        addState(new ClawArmState(robot,"State BackDrop blue", "6-00-1"));
+
+        //open right close left
+        addState(new ClawFingerState(robot,"State BackDrop blue", "7-00-0"));
+
+        // bring arm up
+        addState(new ClawArmState(robot,"State BackDrop blue", "8-00-0"));
+
+        // drivw to park
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-01-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-02-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-0"));
+        addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-1"));
+
+        addState(new ClawArmState(robot,"State BackDrop blue", "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

@@ -20,6 +20,7 @@ public class DriveToCoordinatesState extends CyberarmState {
     public boolean posSpecific;
     public double maxXPower;
     public double maxYPower;
+    public double maxHPower;
     private String actionName;
 
     public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
@@ -31,6 +32,7 @@ public class DriveToCoordinatesState extends CyberarmState {
         this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
         this.maxXPower = robot.configuration.variable(groupName, actionName, "maxXPower").value();
         this.maxYPower = robot.configuration.variable(groupName, actionName, "maxYPower").value();
+        this.maxHPower = robot.configuration.variable(groupName, actionName, "maxHPower").value();
         this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value();
         this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value();
         this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
@@ -45,22 +47,15 @@ public class DriveToCoordinatesState extends CyberarmState {
             robot.xTarget = xTarget;
             robot.yMaxPower = maxYPower;
             robot.xMaxPower = maxXPower;
+            robot.hMaxPower = maxHPower;
         } else {
             setHasFinished(true);
         }
 
-        Log.d("TTT?", ""  + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
-        Log.d("TTT?", ""  + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget);
-
     }
 
     @Override
     public void exec() {
-//        robot.yMaxPower = maxYPower;
-//        robot.xMaxPower = maxXPower;
-//        robot.hTarget = hTarget;
-//        robot.yTarget = yTarget;
-//        robot.xTarget = xTarget;
 
         if (posSpecific) {
             if (objectPos != robot.objectPos) {
@@ -73,7 +68,8 @@ public class DriveToCoordinatesState extends CyberarmState {
                 }
 
                 if (Math.abs(robot.positionX - robot.xTarget) < 5
-                        && Math.abs(robot.positionY - robot.yTarget) < 5) {
+                        && Math.abs(robot.positionY - robot.yTarget) < 5
+                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
                     setHasFinished(true);
                 }
             }
@@ -83,7 +79,8 @@ public class DriveToCoordinatesState extends CyberarmState {
                 }
 
                 if (Math.abs(robot.positionX - robot.xTarget) < 5
-                        && Math.abs(robot.positionY - robot.yTarget) < 5) {
+                        && Math.abs(robot.positionY - robot.yTarget) < 5
+                        && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
                     setHasFinished(true);
                 }
             }

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

@@ -18,6 +18,7 @@ public class DriveToCoordinatesTask extends CyberarmState {
     public void exec() {
         robot.XDrivePowerModifier();
         robot.YDrivePowerModifier();
+        robot.HDrivePowerModifier();
         robot.DriveToCoordinates();
     }
 }

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java

@@ -103,11 +103,13 @@ public class CompetitionRobotV1 extends Robot {
 
     public double yMaxPower = 1;
     public double xMaxPower = 1;
+    public double hMaxPower = 1;
     public double pidX;
     public double pidY;
+    public double pidH;
     public double rawPidX;
     public double rawPidY;
-
+    public double rawPidH;
     public double xVelocity;
     public double yVelocity;
     public double deltaTime = 0;
@@ -284,7 +286,7 @@ public class CompetitionRobotV1 extends Robot {
      * A Controller taking error of the heading position and converting to a power in the direction of a target.
      * @param reference reference is the target position
      * @param current  current is the measured sensor value.
-     * @return A power to the target the position
+     * @return A power to the target position
      *
      */
     public double HeadingPIDControl(double reference, double current){
@@ -349,15 +351,27 @@ public class CompetitionRobotV1 extends Robot {
             pidX = rawPidX;
         }
     }
+    public void HDrivePowerModifier () {
+
+        rawPidH = HeadingPIDControl(Math.toRadians(hTarget), imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
+        if (Math.abs(rawPidH) > hMaxPower) {
+            if (rawPidH < 0) {
+                pidH = -hMaxPower;
+            } else {
+                pidH = hMaxPower;
+            }
+        } else {
+            pidH = rawPidH;
+        }
+    }
 
     public void DriveToCoordinates () {
         // determine the powers needed for each direction
         // this uses PID to adjust needed Power for robot to move to target
 
         double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-        double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
 
-        double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
+        double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(pidH), 1);
 
         // field oriented math, (rotating the global field to the relative field)
         double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading);
@@ -365,10 +379,10 @@ public class CompetitionRobotV1 extends Robot {
 
 
         // finding approximate power for each wheel.
-        frontLeftPower = (rotY + rotX + rx) / denominator;
-        backLeftPower = (rotY - rotX + rx) / denominator;
-        frontRightPower = (rotY - rotX - rx) / denominator;
-        backRightPower = (rotY + rotX - rx) / denominator;
+        frontLeftPower = (rotY + rotX + pidH) / denominator;
+        backLeftPower = (rotY - rotX + pidH) / denominator;
+        frontRightPower = (rotY - rotX - pidH) / denominator;
+        backRightPower = (rotY + rotX - pidH) / denominator;
 
         // apply my powers
         frontLeft.setPower(frontLeftPower);
@@ -412,12 +426,6 @@ public class CompetitionRobotV1 extends Robot {
 
         }
 
-//        pidController.setPID(p, i, d);
-//        int armPos = clawArm.getCurrentPosition();
-//        double pid = pidController.calculate(armPos, target);
-//
-//        power = pid;
-
         clawArm.setTargetPosition(target);
         clawArm.setPower(0.4);
 

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

@@ -24,8 +24,8 @@ public class CompetitionTeleOpState extends CyberarmState {
     public int target = 0;
 
     // ---------------------------------------------------------------------------------------------------------------------shoot variables:
-    public static double releasePos = 0.95;
-    public static double holdPos = 0.55                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    ;
+    public static double releasePos = 0.45;
+    public static double holdPos = 0.95                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    ;
 
 
     public double maxVelocityX;
@@ -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.55;
+    public static double chinUpServoDown = 0;
     public long lastExecutedTime;
 
 
@@ -299,9 +299,8 @@ public class CompetitionTeleOpState extends CyberarmState {
         }
 
         if (Objects.equals(armPos, "lift up")) {
-            robot.shoulder.setPosition(robot.shoulderCollect);
-            robot.elbow.setPosition(robot.elbowDeposit);
-            target = 120;
+            robot.shoulder.setPosition(0.2);
+            target = 850;
             robot.chinUpServo.setPosition(chinUpServoUp);
         }
 
@@ -311,9 +310,8 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.chinUpServo.setPosition(chinUpServoDown);
             } else {
                 robot.lift.setPower(0);
-                robot.chinUpServo.setPosition(chinUpServoDown);
-                robot.shoulder.setPosition(robot.shoulderCollect);
-                target = 120;
+                robot.shoulder.setPosition(0.2);
+                target = 850;
             }
         }
 

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();
             }