Disable time de-bouncing from GamepadChecker (This already appears to be done for us), misc changes for CHIRON

.idea/deploymentTargetDropDown.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="deploymentTargetDropDown">
+    <runningDeviceTargetSelectedWithDropDown>
+      <Target>
+        <type value="RUNNING_DEVICE_TARGET" />
+        <deviceKey>
+          <Key>
+            <type value="SERIAL_NUMBER" />
+            <value value="192.168.43.1:5555" />
+          </Key>
+        </deviceKey>
+      </Target>
+    </runningDeviceTargetSelectedWithDropDown>
+    <timeTargetWasSelectedWithDropDown value="2023-01-24T21:07:00.815029Z" />
+  </component>
+</project>

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

@@ -13,7 +13,7 @@ public class GamepadChecker {
   private final Gamepad gamepad;
   private final HashMap<String, Boolean> buttons = new HashMap<>();
   private final HashMap<String, Long> buttonsDebounceInterval = new HashMap<>();
-  private final long debounceTime = 20L; // ms
+  private final long debounceTime = 0L; // ms
 
   public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) {
     this.engine = engine;

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

@@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.Blinker;
+import com.qualcomm.robotcore.hardware.CRServoImplEx;
 import com.qualcomm.robotcore.hardware.ColorSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -32,7 +33,8 @@ import java.util.concurrent.TimeUnit;
 public class Robot {
     private static final String TAG = "CHIRON | Robot";
     public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
-    public final ServoImplEx gripper, wrist;
+    public final ServoImplEx gripper;
+    public final CRServoImplEx wrist;
     public final IMU imu;
     public final ColorSensor indicatorA, indicatorB;
     public LynxModule expansionHub;
@@ -102,7 +104,7 @@ public class Robot {
         arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive");          // MOTOR PORT: ?
 
         gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper");       // SERVO PORT: ?
-        wrist = engine.hardwareMap.get(ServoImplEx.class, "wrist");           // SERVO PORT: ?
+        wrist = engine.hardwareMap.get(CRServoImplEx.class, "wrist");         // SERVO PORT: ?
 
         indicatorA = engine.hardwareMap.colorSensor.get("indicator_A"); // I2C
         indicatorB = engine.hardwareMap.colorSensor.get("indicator_B"); // I2C
@@ -141,17 +143,19 @@ public class Robot {
         arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         //      MOTOR POWER
-        arm.setVelocity(
+//        arm.setVelocity(
-                angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value()));
+//                angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value()));
+        arm.setPower(0.75);
 
         //   SERVOS (POSITIONAL)
         //      Gripper
         gripper.setDirection(hardwareConfig("gripper_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE);
         gripper.setPosition(tuningConfig("gripper_initial_position").value());
 
+        //   SERVOS (CONTINUOUS)
         //      Wrist
-        wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE);
+        wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
-        wrist.setPosition(tuningConfig("wrist_initial_position").value());
+        wrist.setPower(tuningConfig("wrist_up_power").value());
 
         //   SENSORS
         //      COLOR SENSORS
@@ -232,7 +236,7 @@ public class Robot {
 
         engine.telemetry.addLine();
 
-        engine.telemetry.addData("      Arm", "%d (%8.2f in)", arm.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, arm.getTargetPosition()));
+        engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
 
         // Motor Velocity
         engine.telemetry.addLine("Motor Velocity");
@@ -283,7 +287,7 @@ public class Robot {
         engine.telemetry.addData("      Gripper Enabled", gripper.isPwmEnabled());
         engine.telemetry.addLine();
         engine.telemetry.addData("      Wrist Direction", wrist.getDirection());
-        engine.telemetry.addData("      Wrist Position", wrist.getPosition());
+        engine.telemetry.addData("      Wrist Power", wrist.getPower());
         engine.telemetry.addData("      Wrist Enabled", wrist.isPwmEnabled());
 
         engine.telemetry.addLine();
@@ -293,6 +297,7 @@ public class Robot {
         engine.telemetry.addData("      Facing (Degrees)", facing());
         engine.telemetry.addData("      Heading (Radians)", heading());
         engine.telemetry.addData("      Turn Rate", turnRate());
+        engine.telemetry.addData("      Angle Offset (Degrees)", imuAngleOffset);
 
         engine.telemetry.addLine();
 
@@ -566,7 +571,10 @@ public class Robot {
         double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
         double deltaTime = (time - lastTiming) * 0.001;
 
-        double error = targetVelocity - arm.getVelocity();
+        double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
+        double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
+
+        double error = adjustedTargetVelocity - arm.getVelocity();
         double kp = 0.9;
 
         newTargetVelocity += error * kp * deltaTime;
@@ -574,7 +582,9 @@ public class Robot {
         motorTargetVelocity.put("Arm", newTargetVelocity);
         motorVelocityLastTiming.put("Arm", time);
 
-        arm.setVelocity(newTargetVelocity);
+//        arm.setVelocity(newTargetVelocity);
+
+        arm.setPower(0.75);
     }
 
     public double getVoltage() {
@@ -584,11 +594,12 @@ public class Robot {
     public double facing() {
         double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
-        return (imuDegrees + imuAngleOffset + 360.0) % 360.0;
+        return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
     }
 
     public double heading() {
-        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+        return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
+//        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
     }
 
     public double turnRate() {

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

@@ -8,7 +8,7 @@ public class Move extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
 
-    private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS;
+    private final double targetDistance, tolerance, facing, targetVelocity, timeInMS;
 
     private final double maxVelocity;
     private double speed;
@@ -21,11 +21,10 @@ public class Move extends CyberarmState {
         this.groupName = groupName;
         this.actionName = actionName;
 
-        positionXInInches = robot.getConfiguration().variable(groupName, actionName, "positionXInInches").value();
+        targetDistance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetDistanceInInches").value());
-        positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value();
+        tolerance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value());
-        toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value();
         facing = robot.getConfiguration().variable(groupName, actionName, "facing").value();
-        targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value());
+        targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value());
         timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
         stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
 
@@ -36,7 +35,7 @@ public class Move extends CyberarmState {
     @Override
     public void start() {
         // TODO: Use a dead wheel for this
-        distanceAlreadyTravelled = robot.frontLeftDrive.getCurrentPosition();
+        distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition();
     }
 
     @Override
@@ -55,23 +54,33 @@ public class Move extends CyberarmState {
             return;
         }
 
-        // TODO: Double check maths
+        int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
-//        int travelledDistance = (robot.frontLeftDrive.getCurrentPosition() - distanceAlreadyTravelled);
-//        int targetDistance = robot.unitToTicks(DistanceUnit.INCH, distanceInInches);
-//        int tolerance = robot.unitToTicks(DistanceUnit.INCH, toleranceInInches);
-//
-//        if (robot.isBetween(targetDistance, travelledDistance - tolerance, travelledDistance + tolerance)) {
-//            stop();
-//
-//            setHasFinished(true);
-//
-//            return;
-//        }
 
-        move();
+        if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
+
+        moveDirectional(travelledDistance);
     }
 
-    private void move() {
+    private void moveDirectional(double travelledDistance) {
+        // TODO: Ease-in-out velocity
+        double velocity = targetVelocity;
+
+        if (targetDistance > travelledDistance) {
+            robot.frontRightDrive.setVelocity(-velocity);
+            robot.backLeftDrive.setVelocity(-velocity);
+        } else {
+            robot.frontRightDrive.setVelocity(velocity);
+            robot.backLeftDrive.setVelocity(velocity);
+        }
+    }
+
+    private void moveOmni() {
         if (Math.abs(speed) > maxVelocity) {
             speed = speed < 0 ? -maxVelocity : maxVelocity;
         }

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

@@ -78,18 +78,22 @@ public class ArmDriverControl extends CyberarmState {
             return;
         }
 
-        double stepInterval = robot.tuningConfig("wrist_manual_step_interval").value();
+        double stepPower = robot.tuningConfig("wrist_manual_step_power").value();
-        double stepSize = robot.tuningConfig("wrist_manual_step_size").value();
 
-        if ((controller.dpad_left || controller.dpad_right) && runTime() - lastArmManualControlTime >= stepInterval) {
+        if ((controller.dpad_left || controller.dpad_right)) {
-            lastArmManualControlTime = runTime();
+            robot.wristManuallyControlled = true;
 
             if (controller.dpad_left) { // Wrist Left
-                robot.wrist.setPosition(robot.wrist.getPosition() - stepSize);
+                robot.wrist.setPower(stepPower);
 
-            } else if (controller.dpad_right) { // Wrist Right
-                robot.wrist.setPosition(robot.wrist.getPosition() + stepSize);
             }
+            if (controller.dpad_right) { // Wrist Right
+                robot.wrist.setPower(-stepPower);
+            }
+        }
+
+        if (!controller.dpad_left && !controller.dpad_right) {
+            robot.wrist.setPower(0);
         }
     }
 
@@ -103,7 +107,7 @@ public class ArmDriverControl extends CyberarmState {
 
     private void automatics() {
         if (!robot.hardwareFault) {
-            automaticWrist();
+//            automaticWrist();
             automaticArmVelocity();
         }
 
@@ -118,9 +122,9 @@ public class ArmDriverControl extends CyberarmState {
         double angle = robot.tuningConfig("wrist_auto_rotate_angle").value();
 
         if (robot.ticksToAngle(robot.arm.getCurrentPosition()) >= angle) {
-            robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
+            robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value());
         } else {
-            robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value());
+            robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value());
         }
     }
 
@@ -209,11 +213,13 @@ public class ArmDriverControl extends CyberarmState {
         if (button.equals("dpad_down")) {
             robot.wristManuallyControlled = false;
 
-            robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
+            robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value());
-        } else if (button.equals("dpad_up")) {
+        }
+
+        if (button.equals("dpad_up")) {
             robot.wristManuallyControlled = false;
 
-            robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value());
+            robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value());
         }
 
         // Automatic Arm Control

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

@@ -1,5 +1,6 @@
 package org.timecrafters.minibots.cyberarm.chiron.states.teleop;
 
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.hardware.Gamepad;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -48,6 +49,7 @@ public class DrivetrainDriverControl extends CyberarmState {
 
         double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
         double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x) * 1.1; // Counteract imperfect strafing
+
         double rx = -controller.right_stick_x;
         double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);