Merge remote-tracking branch 'origin/master'

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

@@ -13,12 +13,12 @@ import dev.cyberarm.engine.V2.Utilities;
 public class Localizer {
     private final RedCrabMinibot robot;
     private double rawX = 0, rawY = 0, rawR = 0, offsetX = 0, offsetY = 0;
-    private final double trackWidthMM = 365.0, forwardOffsetMM = 140.0, wheelDiameterMM = 90.0;
+    private final double trackWidthMM = 387.35, forwardOffsetMM = 133.35, wheelDiameterMM = 90.0;
     private final int encoderTicksPerRevolution = 8192;
     private final double encoderGearRatio = 1;
     private double lastEncoderXLeftMM, lastEncoderXRightMM, lastEncoderYCenterMM;
 //    private double xDeltaMultiplier = 0.87012987, yDeltaMultiplier = 0.25;
-    private double xDeltaMultiplier = 1, yDeltaMultiplier = 1;
+    private double xPosMultiplier = 0.675956739, yPosMultiplier = 0.941867531;
     private HolonomicOdometry odometry;
     public Localizer(RedCrabMinibot robot) {
         this.robot = robot;
@@ -100,11 +100,11 @@ public class Localizer {
     }
 
     public double xMM() {
-        return odometry.getPose().getX() + offsetX; //rawX;
+        return odometry.getPose().getX() * xPosMultiplier + offsetX; //rawX;
     }
 
     public double yMM() {
-        return odometry.getPose().getY() + offsetY;  //rawY;
+        return odometry.getPose().getY() * xPosMultiplier + offsetY;  //rawY;
     }
 
     public double xIn() {

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java

@@ -6,6 +6,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
 public class RedCrabAutonomousBlueAudienceEngine extends RedCrabEngine {
@@ -13,6 +14,7 @@ public class RedCrabAutonomousBlueAudienceEngine extends RedCrabEngine {
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java

@@ -6,6 +6,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
 public class RedCrabAutonomousBlueBackstageEngine extends RedCrabEngine {
@@ -13,6 +14,7 @@ public class RedCrabAutonomousBlueBackstageEngine extends RedCrabEngine {
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java

@@ -6,6 +6,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab Auto DEBUGGING", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
 public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabEngine {
@@ -13,6 +14,7 @@ public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabEngine {
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java

@@ -6,6 +6,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
 public class RedCrabAutonomousRedAudienceEngine extends RedCrabEngine {
@@ -13,6 +14,7 @@ public class RedCrabAutonomousRedAudienceEngine extends RedCrabEngine {
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java

@@ -6,6 +6,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
 public class RedCrabAutonomousRedBackstageEngine extends RedCrabEngine {
@@ -13,6 +14,7 @@ public class RedCrabAutonomousRedBackstageEngine extends RedCrabEngine {
     public void setup() {
         robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
+        addTask(new LocalizerTask(robot));
 
         setupFromConfig(
                 new TimeCraftersConfiguration("cyberarm_RedCrab"),

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java

@@ -10,9 +10,6 @@ public abstract class RedCrabEngine extends CyberarmEngine {
     @Override
     public void loop() {
         Utilities.hubsClearBulkReadCache(hardwareMap);
-        if (RedCrabMinibot.localizer != null) {
-            RedCrabMinibot.localizer.integrate();
-        }
 
         super.loop();
 

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java

@@ -8,6 +8,7 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.engine.V2.CyberarmState;
 import dev.cyberarm.engine.V2.Utilities;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 
 @TeleOp(name = "Cyberarm Red Crab TeleOp DEBUGGING", group = "MINIBOT")
 public class RedCrabTeleOpDebuggingOnlyEngine extends RedCrabEngine {
@@ -17,6 +18,8 @@ public class RedCrabTeleOpDebuggingOnlyEngine extends RedCrabEngine {
 
         robot = new RedCrabMinibot(false);
 
+        addTask(new LocalizerTask(robot));
+
         addState(new CyberarmState() {
 
             @Override

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java

@@ -6,6 +6,7 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.engine.V2.Utilities;
 import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
+import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
 import dev.cyberarm.minibots.red_crab.states.Pilot;
 
 @TeleOp(name = "Cyberarm Red Crab TeleOp", group = "MINIBOT")
@@ -15,6 +16,7 @@ public class RedCrabTeleOpEngine extends RedCrabEngine {
         robot = new RedCrabMinibot(false);
 
         addTask(new ClawArmTask(robot));
+        addTask(new LocalizerTask(robot));
 
         addState(new Pilot(robot));
     }

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

@@ -0,0 +1,21 @@
+package dev.cyberarm.minibots.red_crab.states;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.engine.V2.Utilities;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+
+public class LocalizerTask extends CyberarmState {
+    private final RedCrabMinibot robot;
+
+    public LocalizerTask(RedCrabMinibot robot) {
+        this.robot = robot;
+    }
+    @Override
+    public void exec() {
+        Utilities.hubsClearBulkReadCache(engine.hardwareMap);
+
+        if (RedCrabMinibot.localizer != null) {
+            RedCrabMinibot.localizer.integrate();
+        }
+    }
+}

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

@@ -98,7 +98,7 @@ public class MoveToCoordinate extends CyberarmState {
 
         double y = direction.x(); // robot forward is in the X axis
         double x = direction.y(); // robot side to side is on the Y axis
-        double rx = rotationStrength; //engine.gamepad1.right_stick_x;
+        double rx = 0; // -rotationStrength; //engine.gamepad1.right_stick_x;
 
         double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
 

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

@@ -27,8 +27,6 @@ public class SodiPizzaTeleOPState extends CyberarmState {
     public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
     public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative;
     public double approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
-    private double lfPower, rfPower, lbPower, rbPower;
-    private float yTransitPercent, xTransitPercent, rotPercent, percentDenom;
     private int objectPos, armPos;
     private boolean droneLaunched;
     private char buttonPressed;
@@ -83,7 +81,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
         gp1checker = new GamepadChecker(engine, engine.gamepad1);
-        gp2checker = new GamepadChecker(engine, engine.gamepad2);
+        gp2checker = new GamepadChecker(engine, engine.gamepad1);
 
         lastMoveTime = System.currentTimeMillis();
         lastDistRead = System.currentTimeMillis();
@@ -117,13 +115,27 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         robot.rightFront.setPower(frontRightPower);
         robot.rightBack.setPower(backRightPower);
 
+        double heading = -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+        double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+        double rotY = x * Math.sin(heading) + y * Math.cos(heading);
 
-        if (engine.gamepad2.left_stick_button) {
+        frontLeftPower = (rotY + rotX + rx) / denominator;
+        backLeftPower = (rotY - rotX + rx) / denominator;
+        frontRightPower = (rotY - rotX - rx) / denominator;
+        backRightPower = (rotY + rotX - rx) / denominator;
+
+        robot.leftBack.setPower(backLeftPower * drivePower);
+        robot.rightBack.setPower(backRightPower * drivePower);
+        robot.leftFront.setPower(frontLeftPower * drivePower);
+        robot.rightFront.setPower(frontRightPower * drivePower);
+
+
+        if (engine.gamepad1.left_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
                 robot.launcher.setPosition(0.98);
                 lastMoveTime = System.currentTimeMillis();
             }
-        } else if (engine.gamepad2.right_stick_button) {
+        } else if (engine.gamepad1.right_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 100) {
                 robot.launcher.setPosition(robot.launcher.getPosition() - 0.2);
                 lastMoveTime = System.currentTimeMillis();
@@ -135,19 +147,19 @@ public class SodiPizzaTeleOPState extends CyberarmState {
             }
         }
 
-        if (!engine.gamepad2.left_stick_button && droneLaunched) {
+        if (!engine.gamepad1.left_stick_button && droneLaunched) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
                 robot.launcher.setPosition(robot.launcher.getPosition() - 0.025);
                 lastMoveTime = System.currentTimeMillis();
             }
         }
 
-        if (engine.gamepad2.left_stick_y > 0.1) {
+        if (engine.gamepad1.dpad_up) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
                 robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
                 lastMoveTime = System.currentTimeMillis();
             }
-        } else if (engine.gamepad2.left_stick_y < -0.1) {
+        } else if (engine.gamepad1.dpad_down) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
                 robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
                 lastMoveTime = System.currentTimeMillis();
@@ -155,7 +167,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         }
 
         // This moves the arm to Collect position, which is at servo position 0.00.
-        if (engine.gamepad2.a && !engine.gamepad2.start) {
+        if (engine.gamepad1.a && !engine.gamepad1.start) {
             armPos = 1;
         }
 
@@ -186,7 +198,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         //End of code for armPos = 1
 
         // This moves the arm to Precollect position, which is at servo position 0.05.
-        if (engine.gamepad2.x) {
+        if (engine.gamepad1.x) {
             armPos = 2;
         }
 
@@ -217,7 +229,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         //End of code for armPos = 2
 
         // This moves the arm to Deliver position, which is at servo position 0.28.
-        if (engine.gamepad2.b && !engine.gamepad2.start) {
+        if (engine.gamepad1.b && !engine.gamepad1.start) {
             armPos = 3;
         }
 
@@ -252,7 +264,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         //End of code for armPos = 3
 
         // This moves the arm to Stow position, which is at servo position 0.72.
-         if (engine.gamepad2.y) {
+         if (engine.gamepad1.y) {
             armPos = 4;
         }
 
@@ -283,9 +295,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         }
 //        End of code for armPos = 4
 
-        if (engine.gamepad2.dpad_left) {
+        if (engine.gamepad1.dpad_left) {
             robot.gripper.setPosition(GRIPPER_OPEN);
-        } else if (engine.gamepad2.dpad_right) {
+        } else if (engine.gamepad1.dpad_right) {
             robot.gripper.setPosition(GRIPPER_CLOSED);
         }