Sync changes

TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java

@@ -1,11 +1,14 @@
 package org.cyberarm.engine.V2;
 
+import android.util.Log;
+
 import com.qualcomm.robotcore.hardware.Gamepad;
 
 import java.lang.reflect.Field;
 import java.util.HashMap;
 
 public class GamepadChecker {
+  private final String TAG = "GamepadChecker";
   private final CyberarmEngine engine;
   private final Gamepad gamepad;
   private final HashMap<String, Boolean> buttons = new HashMap<>();
@@ -49,16 +52,23 @@ public class GamepadChecker {
           if (button) {
             if (!buttons.get(btn)) {
               engine.buttonDown(gamepad, btn);
+
+              Log.d(TAG, "Button '" + btn + "' is DOWN [" + buttons.size() + "]");
             }
 
             buttons.put(btn, true);
           } else {
             if (buttons.get(btn)) {
               engine.buttonUp(gamepad, btn);
+
+              Log.d(TAG, "Button '" + btn + "' is UP");
             }
 
             buttons.put(btn, false);
           }
+
+          buttonsDebounceInterval.put(btn, milliseconds);
+
         } else if (button == buttons.get(btn)) {
           buttonsDebounceInterval.put(btn, milliseconds);
         }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java

@@ -222,7 +222,8 @@ public class Robot {
 
         // Sensors / IMU
         engine.telemetry.addLine("IMU");
-        engine.telemetry.addData("      Facing", facing());
+        engine.telemetry.addData("      Facing (Degrees)", facing());
+        engine.telemetry.addData("      Heading (Radians)", heading());
         engine.telemetry.addData("      Turn Rate", turnRate());
     }
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java

@@ -26,13 +26,13 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine {
     public void setup() {
         robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"));
 
-        addState(new MotorSetupState("Left Drive", robot.backLeftDrive, "left_drive_direction_forward", robot));
+        addState(new MotorSetupState("Back Left Drive", robot.backLeftDrive, "back_left_drive_direction_forward", robot));
-        addState(new MotorSetupState("Right Drive", robot.frontRightDrive, "right_drive_direction_forward", robot));
+        addState(new MotorSetupState("Front Right Drive", robot.frontRightDrive, "front_right_drive_direction_forward", robot));
 
-        addState(new MotorSetupState("Front Drive", robot.frontLeftDrive, "front_drive_direction_forward", robot));
+        addState(new MotorSetupState("Front Left Drive", robot.frontLeftDrive, "front_left_drive_direction_forward", robot));
-        addState(new MotorSetupState("Back Drive", robot.backRightDrive, "back_drive_direction_forward", robot));
+        addState(new MotorSetupState("Back Right Drive", robot.backRightDrive, "back_right_drive_direction_forward", robot));
 
-        addState(new MotorSetupState("Lift Drive", robot.arm, "lift_drive_direction_forward", robot));
+        addState(new MotorSetupState("Arm", robot.arm, "arm_direction_forward", robot));
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java

@@ -1,5 +1,7 @@
 package org.timecrafters.minibots.cyberarm.chiron.states;
 
+import android.util.Log;
+
 import com.qualcomm.robotcore.hardware.Gamepad;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -13,6 +15,8 @@ public class DriverControlState extends CyberarmState {
     private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0;
     private boolean LEDStatusToggle = false;
     private boolean fieldCentricControl = true;
+    private boolean invertRobotForward = false;
+    private boolean robotSlowMode = false;
 
     public DriverControlState(Robot robot) {
         this.robot = robot;
@@ -41,6 +45,7 @@ public class DriverControlState extends CyberarmState {
         engine.telemetry.addData("Wrist Interval", lastWristManualControlTime);
         engine.telemetry.addData("LED Status Interval", lastLEDStatusAnimationTime);
         engine.telemetry.addData("Field Centric Control", fieldCentricControl);
+        engine.telemetry.addData("Invert Robot Forward", invertRobotForward);
     }
 
     // FIXME: replace .setPower with .setVelocity
@@ -50,27 +55,32 @@ public class DriverControlState extends CyberarmState {
             return;
         }
 
-        double y = -engine.gamepad1.left_stick_y;
+        double maxSpeed = robot.tuningConfig("drivetrain_max_power").value();
-        double x = engine.gamepad1.left_stick_x * 1.1;
+        double slowSpeed = robot.tuningConfig("drivetrain_slow_power").value();
-        double rx = engine.gamepad1.right_stick_x;
+        double speedLimiter = robotSlowMode ? slowSpeed : maxSpeed;
+
+        double y = (invertRobotForward ? engine.gamepad1.left_stick_y : -engine.gamepad1.left_stick_y) * speedLimiter;
+        double x = ((invertRobotForward ? engine.gamepad1.left_stick_x : -engine.gamepad1.left_stick_x) * speedLimiter) * 1.1; // Counteract imperfect strafing
+        double rx = -engine.gamepad1.right_stick_x * speedLimiter;
         double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
 
+
         double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
 
         if (fieldCentricControl) {
-            double heading = -robot.heading();
+            double heading = robot.heading();
             double rotX = x * Math.cos(heading) - y * Math.sin(heading);
             double rotY = x * Math.sin(heading) + y * Math.cos(heading);
 
             frontLeftPower = (rotY + rotX + rx) / denominator;
-            backLeftPower = (rotY - rotX + rx) / denominator;
+            backLeftPower = (rotY - rotX - rx) / denominator;
-            frontRightPower = (rotY - rotX - rx) / denominator;
+            frontRightPower = (rotY - rotX + rx) / denominator;
             backRightPower = (rotY + rotX - rx) / denominator;
 
         } else {
             frontLeftPower = (y + x + rx) / denominator;
-            backLeftPower = (y - x + rx) / denominator;
+            backLeftPower = (y - x - rx) / denominator;
-            frontRightPower = (y - x - rx) / denominator;
+            frontRightPower = (y - x + rx) / denominator;
             backRightPower = (y + x - rx) / denominator;
         }
 
@@ -289,6 +299,16 @@ public class DriverControlState extends CyberarmState {
         } else if (button.equals("y")) {
             armPosition(Robot.ArmPosition.MEDIUM);
         }
+
+        if (button.equals("pause")) {
+            invertRobotForward = !invertRobotForward;
+        }
+        if (button.equals("guide")) {
+            robot.hardwareFault = !robot.hardwareFault;
+        }
+        if (button.equals("start")) {
+            fieldCentricControl = !fieldCentricControl;
+        }
     }
 
     @Override
@@ -296,11 +316,5 @@ public class DriverControlState extends CyberarmState {
         if (gamepad != engine.gamepad1) {
             return;
         }
-
-        if (button.equals("guide")) {
-            robot.hardwareFault = !robot.hardwareFault;
-        } else if (button.equals("start")) {
-            fieldCentricControl = !fieldCentricControl;
-        }
     }
 }