Rongrui arm skeleton (#27)

* Added ArmSubsystem Skeleton * Added basic commands Extend, retract, stop. * Fixed ArmState spelling and log when commands initialize * Fixed sparkMaxFactory.create() * Removed unused sparkMaxFactory.create() * Fixed minor syntax * Added setPowerMin and setPowerMax * Revised power limit --------- Co-authored-by: Stephen Just <[email protected]>
Team488 · Jan 20, 2024 · 7aa832d · 7aa832d
1 parent 7c3ae40
commit 7aa832d
Showing 1 changed file with 11 additions and 0 deletions.
diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java
@@ -3,6 +3,7 @@
 import competition.electrical_contract.ElectricalContract;
 import xbot.common.command.BaseSubsystem;
 import xbot.common.controls.actuators.XCANSparkMax;
+import xbot.common.math.MathUtils;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
 
@@ -20,6 +21,9 @@ public class ArmSubsystem extends BaseSubsystem {
     public DoubleProperty extendPower;
     public DoubleProperty retractPower;
 
+    private DoubleProperty setPowerMax;
+    private DoubleProperty setPowerMin;
+
     public enum ArmState {
         EXTENDING,
         RETRACTING,
@@ -34,6 +38,9 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
 
         extendPower = pf.createPersistentProperty("ExtendPower", 0.1);
         retractPower = pf.createPersistentProperty("RetractPower", 0.1);
+
+        setPowerMax = pf.createPersistentProperty("SetPowerMax", 0.5);
+        setPowerMin = pf.createPersistentProperty("SetPowerMin", -0.5);
 
         armMotorLeft = sparkMaxFactory.createWithoutProperties(contract.getArmMotorLeft(), this.getPrefix(), "ArmMotorLeft");
         armMotorRight = sparkMaxFactory.createWithoutProperties(contract.getArmMotorRight(), this.getPrefix(), "ArmMotorRight");
@@ -43,6 +50,10 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
     }
 
     public void setPower(double power) {
+
+        // Put power within limit range (if not already)
+        power = MathUtils.constrainDouble(power, setPowerMin.get(), setPowerMax.get());
+
         armMotorLeft.set(power);
         armMotorRight.set(power);
     }