Feature/prepare arm for testing

src/main/java/competition/electrical_contract/CompetitionContract.java

@@ -175,17 +175,17 @@ public DeviceInfo getReadyToFireNoteSensorDio() {
 
     @Override
     public boolean isArmReady() {
-        return false;
+        return true;
     }
 
     @Override
     public DeviceInfo getArmMotorLeft() {
-        return new DeviceInfo("ArmMotorLeft", 10, true);
+        return new DeviceInfo("ArmMotorLeft", 32, true);
     }
 
     @Override
     public DeviceInfo getArmMotorRight() {
-        return new DeviceInfo("ArmMotorRight", 11, true);
+        return new DeviceInfo("ArmMotorRight", 27, true);
     }
 
     @Override

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -67,9 +67,6 @@ public void setupFundamentalCommands(
         oi.operatorGamepad.getXboxButton(XboxButton.X).whileTrue(shooterWarmUp);
         oi.operatorGamepad.getXboxButton(XboxButton.A).whileTrue(fireCollectorCommand);
 
-        // Adding armMaintainer command to smartdashboard
-        armMaintainer.includeOnSmartDashboard();
-
         // Arms are taken care of via their maintainer & human overrides.
         armAngle.setArmPosition(ArmSubsystem.UsefulArmPosition.SCOOCH_NOTE);
         var scoochNote = scoocherIntake.alongWith(armAngle);

src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java

@@ -4,6 +4,7 @@
 import javax.inject.Singleton;
 
 import competition.subsystems.arm.ArmSubsystem;
+import competition.subsystems.arm.commands.ArmMaintainerCommand;
 import competition.subsystems.arm.commands.StopArmCommand;
 import competition.subsystems.collector.CollectorSubsystem;
 import competition.subsystems.collector.commands.StopCollectorCommand;
@@ -34,7 +35,7 @@ public void setupDriveSubsystem(DriveSubsystem driveSubsystem, SwerveDriveWithJo
     }
 
     @Inject
-    public void setupArmSubsystem(ArmSubsystem armSubsystem, StopArmCommand command) {
+    public void setupArmSubsystem(ArmSubsystem armSubsystem, ArmMaintainerCommand command) {
         armSubsystem.setDefaultCommand(command);
     }
     @Inject

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

@@ -50,6 +50,7 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF
     boolean hasCalibratedRight;
 
     private double targetAngle;
+    private final DoubleProperty armPowerClamp;
 
 
 
@@ -113,6 +114,8 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
         speedLimitForNotCalibrated = pf.createPersistentProperty(
                 "SpeedLimitForNotCalibrated", -0.1);
 
+        armPowerClamp = pf.createPersistentProperty("ArmPowerClamp", 0.05);
+
         hasCalibratedLeft = false;
         hasCalibratedRight = false;
 
@@ -128,11 +131,14 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
                     this.getPrefix() + "ArmEncoder",
                     contract.getArmEncoderInverted());
 
+            armMotorLeft.setSmartCurrentLimit(20);
+            armMotorRight.setSmartCurrentLimit(20);
+
             // Enable hardware limits
-            armMotorLeft.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed, true);
-            armMotorLeft.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed, true);
-            armMotorRight.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed, true);
-            armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed, true);
+            armMotorLeft.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
+            armMotorLeft.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
+            armMotorRight.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
+            armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
         }
 
         this.armState = ArmState.STOPPED;
@@ -157,15 +163,26 @@ public double constrainPowerIfAtLimit(double power) {
         return power;
     }
 
+    boolean unsafeMinOrMax = false;
 
     public void setPowerToLeftAndRightArms(double leftPower, double rightPower) {
+
+        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
         if (armPowerMax.get() < 0 || armPowerMin.get() > 0 || speedLimitForNotCalibrated.get() > 0) {
             armMotorLeft.set(0);
             armMotorRight.set(0);
-            log.error("armPowerMax or armPowerMin or speedLimitForNotCalibrated values out of bound!");
+            if (!unsafeMinOrMax) {
+                log.error("armPowerMax or armPowerMin or speedLimitForNotCalibrated values out of bound!");
+                unsafeMinOrMax = true;
+            }
             return;
         }
+        unsafeMinOrMax = false;
 
         // If not calibrated, motor can only go down at slow rate
         if (!(hasCalibratedLeft && hasCalibratedRight)) {
@@ -260,8 +277,8 @@ public LimitState getLimitState(XCANSparkMax motor) {
         boolean lowerHit = false;
 
         if (contract.isArmReady()) {
-            upperHit = motor.getForwardLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen);
-            lowerHit = motor.getReverseLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen);
+            upperHit = motor.getForwardLimitSwitchPressed();
+            lowerHit = motor.getReverseLimitSwitchPressed();
         }
 
         if (upperHit && lowerHit) {
@@ -332,6 +349,14 @@ public boolean isCalibrated() {
         return false;
     }
 
+    /**
+     * Do not call this from competition code.
+     * @param clampPower maximum power under any circumstance
+     */
+    public void setClampLimit(double clampPower) {
+        armPowerClamp.set(Math.abs(clampPower));
+    }
+
     public void periodic() {
         if (contract.isArmReady()) {
             armEncoderTicksUpdate();

src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java

@@ -18,7 +18,6 @@ public class ArmMaintainerCommand extends BaseMaintainerCommand {
     // oi used for human input
     private final OperatorInterface oi;
 
-
     @Inject
     public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf,
                                 HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory,
@@ -40,7 +39,7 @@ public void initialize() {
 
     @Override
     protected void coastAction() {
-        //Don't think anything is needed here...
+        arm.setPower(0.0);
     }
 
     @Override

src/test/java/competition/subsystems/arm/ArmSubsystemTest.java

@@ -18,6 +18,7 @@ public class ArmSubsystemTest extends BaseCompetitionTest {
     public void setUp() {
         super.setUp();
         arm = getInjectorComponent().armSubsystem();
+        arm.setClampLimit(1.0);
     }
 
     private void checkMotorPower(double leftPower, double rightPower) {
@@ -91,6 +92,7 @@ public void testReverseLimitOffset() {
         // You hit the reverse limit switch and a periodic runs
         ((MockCANSparkMax)arm.armMotorLeft).setReverseLimitSwitchStateForTesting(true);
         ((MockCANSparkMax)arm.armMotorRight).setReverseLimitSwitchStateForTesting(true);
+        arm.refreshDataFrame();
         arm.periodic();
 
         // Check offset, offset is the actual position when you had initially started
@@ -121,6 +123,7 @@ public void testLimits() {
         // Reverse limit hit (Usually you only get here for calibration)
         ((MockCANSparkMax)arm.armMotorLeft).setReverseLimitSwitchStateForTesting(true);
         ((MockCANSparkMax)arm.armMotorRight).setReverseLimitSwitchStateForTesting(true);
+        arm.refreshDataFrame();
         arm.periodic();
         checkLimitState(ArmSubsystem.LimitState.LOWER_LIMIT_HIT);
 
@@ -143,6 +146,7 @@ public void testLimits() {
 
         ((MockCANSparkMax)arm.armMotorLeft).setForwardLimitSwitchStateForTesting(true);
         ((MockCANSparkMax)arm.armMotorRight).setForwardLimitSwitchStateForTesting(true);
+        arm.refreshDataFrame();
         arm.periodic();
         checkLimitState(ArmSubsystem.LimitState.UPPER_LIMIT_HIT);
 
@@ -155,6 +159,7 @@ public void testLimits() {
         // Test LimitState: BOTH_LIMITS_HIT (Activates when there is issues...)
         ((MockCANSparkMax)arm.armMotorLeft).setReverseLimitSwitchStateForTesting(true);
         ((MockCANSparkMax)arm.armMotorRight).setReverseLimitSwitchStateForTesting(true);
+        arm.refreshDataFrame();
         arm.periodic();
         checkLimitState(ArmSubsystem.LimitState.BOTH_LIMITS_HIT);
 

src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java

@@ -28,6 +28,7 @@ public void setUp() {
         retract = getInjectorComponent().retractArmCommand();
         stop = getInjectorComponent().stopArmCommand();
         reconcile = getInjectorComponent().reconcileArmAlignmentCommand();
+        arm.setClampLimit(1.0);
     }
 
     private void checkMotorPower(double power) {