Arm: clarify target units and display Mechanism2d in AdvantageScope

src/main/java/competition/simulation/Simulator2024.java

@@ -92,6 +92,19 @@ public void update() {
             rightMockMotor.setPosition(rightArmPositionCalculator.getAverage());
         }
 
+        // simulate the arm limit switches being pressed when a certain position is reached
+        if (leftMockMotor.getPosition() < -3) {
+            leftMockMotor.setReverseLimitSwitchStateForTesting(true);
+        } else {
+            leftMockMotor.setReverseLimitSwitchStateForTesting(false);
+        }
+
+        if (rightMockMotor.getPosition() < -3) {
+            rightMockMotor.setReverseLimitSwitchStateForTesting(true);
+        } else {
+            rightMockMotor.setReverseLimitSwitchStateForTesting(false);
+        }
+
         // The shooter wheel should pretty much always be in velocity mode.
         var shooterMockMotor = (MockCANSparkMax)shooter.leader;
         shooterVelocityCalculator.add(shooterMockMotor.getReference());

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -5,6 +5,9 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 import xbot.common.advantage.DataFrameRefreshable;
 import xbot.common.command.BaseSetpointSubsystem;
 import xbot.common.controls.actuators.XCANSparkMax;
@@ -29,29 +32,35 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF
 
     public ArmState armState;
 
-    public DoubleProperty extendPower;
-    public DoubleProperty retractPower;
+    public final DoubleProperty extendPower;
+    public final DoubleProperty retractPower;
 
-    private DoubleProperty armPowerMax;
-    private DoubleProperty armPowerMin;
+    private final DoubleProperty armPowerMax;
+    private final DoubleProperty armPowerMin;
 
 
-    public DoubleProperty ticksToMmRatio; // Millimeters
-    public DoubleProperty armMotorLeftRevolutionOffset; // # of revolutions
-    public DoubleProperty armMotorRightRevolutionOffset;
-    public DoubleProperty armMotorRevolutionLimit;
-    public DoubleProperty armAbsoluteEncoderOffset;
-    public DoubleProperty armAbsoluteEncoderTicksPerDegree;
-    public DoubleProperty softUpperLimit;
-    public DoubleProperty softLowerLimit;
-    public DoubleProperty speedLimitForNotCalibrated;
-    public DoubleProperty angleTrim;
+    public final DoubleProperty ticksToMmRatio; // Millimeters
+    private double armMotorLeftRevolutionOffset; // # of revolutions
+    private double armMotorRightRevolutionOffset;
+    public final DoubleProperty armMotorRevolutionLimit;
+    public final DoubleProperty armAbsoluteEncoderOffset;
+    public final DoubleProperty armAbsoluteEncoderTicksPerDegree;
+    public final DoubleProperty softUpperLimit;
+    public final DoubleProperty softLowerLimit;
+    public final DoubleProperty speedLimitForNotCalibrated;
+    public final DoubleProperty angleTrim;
     boolean hasCalibratedLeft;
     boolean hasCalibratedRight;
+    private final DoubleProperty maximumArmDesyncInMm;
 
-    private double targetAngle;
+    private double targetExtension;
     private final DoubleProperty armPowerClamp;
 
+    // for rendering the arm in advantage scope
+    public final Mechanism2d armActual2d;
+    public final MechanismLigament2d armLigament;
+    // what angle does the arm make with the pivot when it's at our concept of zero?
+    public final double armPivotAngleAtArmAngleZero = 45;
 
 
     public enum ArmState {
@@ -96,11 +105,6 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
 
         angleTrim = pf.createPersistentProperty("AngleTrim", 0);
 
-        armMotorLeftRevolutionOffset = pf.createPersistentProperty(
-                "ArmMotorLeftRevolutionOffset", 0);
-        armMotorRightRevolutionOffset = pf.createPersistentProperty(
-                "ArmMotorRightRevolutionOffset", 0);
-
         armAbsoluteEncoderOffset = pf.createPersistentProperty(
                 "ArmAbsoluteEncoderOffset", 0);
         armAbsoluteEncoderTicksPerDegree = pf.createPersistentProperty(
@@ -115,6 +119,7 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
                 "SpeedLimitForNotCalibrated", -0.1);
 
         armPowerClamp = pf.createPersistentProperty("ArmPowerClamp", 0.05);
+        maximumArmDesyncInMm = pf.createPersistentProperty("MaximumArmDesyncInMm", 5);
 
         hasCalibratedLeft = false;
         hasCalibratedRight = false;
@@ -142,6 +147,15 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
         }
 
         this.armState = ArmState.STOPPED;
+
+        armActual2d = new Mechanism2d(10, 10, new Color8Bit(255, 255, 255));
+        armLigament = new MechanismLigament2d("arm", 5, getArmAngle() - armPivotAngleAtArmAngleZero, 10, new Color8Bit(255, 0, 0)); 
+        armActual2d.getRoot("base", 3, 5)
+            .append(armLigament)
+            .append(new MechanismLigament2d("box-right", 1, 90))
+            .append(new MechanismLigament2d("box-top", 2, 90))
+            .append(new MechanismLigament2d("box-left", 1, 90));
+
     }
 
     public double constrainPowerIfNearLimit(double power, double actualPosition) {
@@ -164,15 +178,66 @@ public double constrainPowerIfAtLimit(double power) {
     }
 
     boolean unsafeMinOrMax = false;
+
+    private double getLeftArmPosition() {
+        return armMotorLeft.getPosition() + armMotorLeftRevolutionOffset;
+    }
+
+    private double getRightArmPosition() {
+        return armMotorRight.getPosition() + armMotorRightRevolutionOffset;
+    }
 
     public void setPowerToLeftAndRightArms(double leftPower, double rightPower) {
 
+        // First, if we are calibrated, apply a power factor based on the difference between the two
+        // arms to make sure they stay in sync
+        if (hasCalibratedLeft && hasCalibratedRight) {
+            double distanceLeftAhead = convertTicksToMm(getLeftArmPosition() - getRightArmPosition());
+
+            // If the left arm is ahead, and the left arm wants to go up/forward, reduce its power.
+            // If the left arm is ahead, and the left arm wants to go down/backward, make no change to power.
+            // If the right arm is ahead, and the right arm wants to go up/forward, reduce its power.
+            // If the right arm is ahead, and the right arm wants to go down/backward, make no change to power.
+
+            // If we have to make any changes to power, do so by a factor proportional to the maximum
+            // allowed desync in mm. At 50% of the desync, it would restrict power by 50%.
+
+            double potentialReductionFactor = Math.min(1, Math.abs(distanceLeftAhead) / maximumArmDesyncInMm.get());
+            aKitLog.record("PotentialReductionFactor", potentialReductionFactor);
+
+            // If left arm is ahead
+            if (distanceLeftAhead> 0) {
+                // and left arm wants to go more ahead, slow down (or stop)
+                if (leftPower > 0) {
+                    leftPower *= potentialReductionFactor;
+                }
+                // and right arm wants to get further behind, slow down (or stop)
+                if (rightPower < 0) {
+                    rightPower *= potentialReductionFactor;
+                }
+            }
+            // If right arm is ahead
+            else if (distanceLeftAhead < 0) {
+                // and right arm wants to go more ahead, slow down (or stop)
+                if (rightPower > 0) {
+                    rightPower *= potentialReductionFactor;
+                }
+                // and left arm wants to get further behind, slow down (or stop)
+                if (leftPower < 0) {
+                    leftPower *= potentialReductionFactor;
+                }
+            }
+        }
+
+        // Next, completely flatten the power within a known range.
+        // Primarily used for validating the arm behavior with very small power values.
         double clampLimit = Math.abs(armPowerClamp.get());
 
         leftPower = MathUtils.constrainDouble(leftPower, -clampLimit, clampLimit);
         rightPower = MathUtils.constrainDouble(rightPower, -clampLimit, clampLimit);
 
-        // Check if armPowerMin/armPowerMax are safe values
+        // Next, a sanity check; if we have been grossly misconfigured to where the
+        // max/min powers are out of bounds (e.g. a max smaller than min), freeze the arm entirely.
         if (armPowerMax.get() < 0 || armPowerMin.get() > 0 || speedLimitForNotCalibrated.get() > 0) {
             armMotorLeft.set(0);
             armMotorRight.set(0);
@@ -184,26 +249,30 @@ public void setPowerToLeftAndRightArms(double leftPower, double rightPower) {
         }
         unsafeMinOrMax = false;
 
-        // If not calibrated, motor can only go down at slow rate
+        // If not calibrated, motor can only go down at slow rate since we don't know where we are.
         if (!(hasCalibratedLeft && hasCalibratedRight)) {
             leftPower = MathUtils.constrainDouble(leftPower, speedLimitForNotCalibrated.get(), 0);
             rightPower = MathUtils.constrainDouble(rightPower, speedLimitForNotCalibrated.get(), 0);
+        }
 
-        } else {
-            // If calibrated, restrict movement to area
+
+        // If calibrated, but near limits, slow the system down a bit so we
+        // don't slam into the hard limits.
+        if (hasCalibratedLeft && hasCalibratedRight)
+        {
             leftPower = constrainPowerIfNearLimit(
                     leftPower,
-                    armMotorLeft.getPosition() + armMotorLeftRevolutionOffset.get());
+                    getLeftArmPosition());
             rightPower = constrainPowerIfNearLimit(
                     rightPower,
-                    armMotorRight.getPosition() + armMotorRightRevolutionOffset.get());
+                    getRightArmPosition());
         }
 
-        // Arm at limit hit power restrictions
+        // If we are actually at our hard limits, stop the motors
         leftPower = constrainPowerIfAtLimit(leftPower);
         rightPower = constrainPowerIfAtLimit(rightPower);
 
-        // Put power within limit range (if not already)
+        // Respect overall max/min power limits.
         leftPower = MathUtils.constrainDouble(leftPower, armPowerMin.get(), armPowerMax.get());
         rightPower = MathUtils.constrainDouble(rightPower, armPowerMin.get(), armPowerMax.get());
 
@@ -240,16 +309,6 @@ public double convertTicksToMm(double ticks) {
         return ticksToMmRatio.get() * ticks;
     }
 
-    // TO-DO
-    public double convertTicksToShooterAngle(double ticks) {
-        return ticks * 1000; // To be modified into ticks to shooter angle formula
-    }
-
-    // TO-DO
-    public double convertShooterAngleToTicks(double angle) {
-        return 0;
-    }
-
     public double getArmAngleFromDistance(double distanceFromSpeaker) {
         // Distance: Inches; Angle: Degrees; Distance = Measured Distance - Calibration Offset
         return (0.0019 * Math.pow(distanceFromSpeaker, 2) + (-0.7106 * distanceFromSpeaker) + 82.844) + angleTrim.get();
@@ -298,57 +357,94 @@ public double getArmAbsoluteAngle() {
     public void armEncoderTicksUpdate() {
         aKitLog.record("ArmMotorLeftTicks", armMotorLeft.getPosition());
         aKitLog.record("ArmMotorRightTicks", armMotorRight.getPosition());
-        aKitLog.record("ArmMotorLeftMm", convertTicksToMm(
-                armMotorLeft.getPosition() + armMotorLeftRevolutionOffset.get()));
-        aKitLog.record("ArmMotorRightMm", convertTicksToMm(
-                armMotorRight.getPosition() + armMotorRightRevolutionOffset.get()));
-
-        aKitLog.record("ArmMotorToShooterAngle", convertTicksToShooterAngle(
-                (armMotorLeft.getPosition() + armMotorRight.getPosition() + armMotorLeftRevolutionOffset.get()
-                        + armMotorRightRevolutionOffset.get()) / 2));
-
+        aKitLog.record("ArmMotorLeftMm", convertTicksToMm(getLeftArmPosition()));
+        aKitLog.record("ArmMotorRightMm", convertTicksToMm(getRightArmPosition()));
         aKitLog.record("ArmAbsoluteEncoderAngle", getArmAbsoluteAngle());
     }
 
 
     // Update the offset of the arm when it touches either forward/reverse limit switches for the first time.
     public void calibrateArmOffset() {
-        LimitState leftArmLimitState = getLimitState(armMotorLeft);
-        LimitState rightArmLimitState = getLimitState(armMotorRight);
-
-        if (!hasCalibratedLeft && leftArmLimitState == LimitState.LOWER_LIMIT_HIT) {
+        // For now keeping the concept of left/right having independent calibration,
+        // but for simplicity, hitting either limit will calibrate both for now.
+        if ((!hasCalibratedLeft || !hasCalibratedRight)
+                && (getLimitState(armMotorLeft) == LimitState.LOWER_LIMIT_HIT
+                    || getLimitState(armMotorRight) == LimitState.LOWER_LIMIT_HIT))
+        {
             hasCalibratedLeft = true;
-            armMotorLeftRevolutionOffset.set(-armMotorLeft.getPosition());
-        }
-
-        if (!hasCalibratedRight && rightArmLimitState == LimitState.LOWER_LIMIT_HIT) {
             hasCalibratedRight = true;
-            armMotorRightRevolutionOffset.set(-armMotorRight.getPosition());
+            armMotorLeftRevolutionOffset = -armMotorLeft.getPosition();
+            armMotorRightRevolutionOffset = -armMotorRight.getPosition();
         }
+    }
+
+    /**
+     * Get the current angle of the arm in degrees based on the extension distance.
+     */
+    public double getArmAngle() {
+        return getArmAngleForExtension(getCurrentValue());
+    }
 
-        aKitLog.record("HasCalibratedLeftArm", hasCalibratedLeft);
-        aKitLog.record("HasCalibratedRightArm", hasCalibratedRight);
+    public double getArmAngleForExtension(double extension) {
+        // TODO: This is just a placeholder, the relationship will actually be nonlinear
+        var degreesPerMmExtension = 0.01;
+        return extension * degreesPerMmExtension;
     }
+
+    public double getExtensionForArmAngle(double angle) {
+        // TODO: this is just a placeholder, the relationship will be nonlinear
+        var degreesPerMmExtension = 0.01;
+
+        // unncessarily paranoid avoid divide by 0 check
+        if(degreesPerMmExtension == 0) {
+            return 0;
+        } else {
+            return angle / degreesPerMmExtension;
+        }
+    }
+
     @Override
     public Double getCurrentValue() {
-        return convertTicksToMm(armMotorLeft.getPosition() + armMotorLeftRevolutionOffset.get());
+        return getExtensionDistance();
     }
 
+    public double getExtensionDistance() {
+        return convertTicksToMm(armMotorLeft.getPosition() + armMotorLeftRevolutionOffset);
+    }
+
+    /** 
+     * This is the extension distance the arm is trying to reach via PID 
+     */
     @Override
     public Double getTargetValue() {
-        return targetAngle;
+        return targetExtension;
     }
 
+    /**
+     * the current target extension distance the arm is trying to reach via PID
+     */
     @Override
-    public void setTargetValue(Double value) {
-         targetAngle = value;
+    public void setTargetValue(Double targetExtension) {
+         this.targetExtension = targetExtension;
+    }
+
+    public void setTargetAngle(Double targetAngle) {
+        targetExtension = getExtensionForArmAngle(targetAngle);
     }
 
     @Override
     public boolean isCalibrated() {
         return false;
     }
 
+    public double getLeftArmOffset() {
+        return armMotorLeftRevolutionOffset;
+    }
+
+    public double getRightArmOffset() {
+        return armMotorRightRevolutionOffset;
+    }
+
     /**
      * Do not call this from competition code.
      * @param clampPower maximum power under any circumstance
@@ -365,12 +461,22 @@ public void periodic() {
             armMotorRight.periodic();
         }
 
-        aKitLog.record("Target Angle", targetAngle);
+        aKitLog.record("HasCalibratedLeftArm", hasCalibratedLeft);
+        aKitLog.record("HasCalibratedRightArm", hasCalibratedRight);
+
+        aKitLog.record("Target Extension", targetExtension);
+        aKitLog.record("TargetAngle", getArmAngleForExtension(targetExtension));
         aKitLog.record("Arm3dState", new Pose3d(
                 new Translation3d(0, 0, 0),
                 new Rotation3d(0, 0, 0)));
+
+        var color = isCalibrated() ? new Color8Bit(0, 255, 0) : new Color8Bit(255, 0, 0);
+        armLigament.setAngle(getArmAngle() - armPivotAngleAtArmAngleZero);
+        armLigament.setColor(color);
+        aKitLog.record("Arm2dStateActual", armActual2d);
     }
 
+
     @Override
     public void refreshDataFrame() {
         if (contract.isArmReady()) {