UsefulArmPositions and starting new test writing

Team488 · Jan 31, 2024 · 594061a · 594061a
1 parent 603d5e4
commit 594061a
Show file tree

Hide file tree

Showing 4 changed files with 55 additions and 20 deletions.
diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java
@@ -1,6 +1,10 @@
 package competition.injection.components;
 
 import competition.subsystems.arm.ArmSubsystem;
+import competition.subsystems.arm.commands.ExtendArmCommand;
+import competition.subsystems.arm.commands.ReconcileArmAlignmentCommand;
+import competition.subsystems.arm.commands.RetractArmCommand;
+import competition.subsystems.arm.commands.StopArmCommand;
 import competition.subsystems.collector.CollectorSubsystem;
 import competition.subsystems.schoocher.ScoocherSubsystem;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
@@ -31,6 +35,10 @@ public abstract class BaseRobotComponent extends BaseComponent {
     public abstract VisionSubsystem visionSubsystem();
 
     public abstract ArmSubsystem armSubsystem();
+    public abstract ExtendArmCommand extendArmCommand();
+    public abstract RetractArmCommand retractArmCommand();
+    public abstract StopArmCommand stopArmCommand();
+    public abstract ReconcileArmAlignmentCommand reconcileArmAlignmentCommand();
 
     public abstract ScoocherSubsystem scoocherSubsystem();
 

diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java
@@ -41,8 +41,8 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF
     boolean hasCalibratedLeft;
     boolean hasCalibratedRight;
 
-    LimitState leftArmAtLimit;
-    LimitState rightArmAtLimit;
+    LimitState leftArmLimitState;
+    LimitState rightArmLimitState;
 
     private double targetAngle;
 
@@ -58,8 +58,11 @@ public enum LimitState {
         NOT_AT_LIMIT
     }
 
-    public enum UsefulArmPositions {
-
+    public enum UsefulArmPosition {
+        DEFAULT,
+        RAISED,
+        COLLECTING_FROM_GROUND,
+        FIRING_FROM_SPEAKER_FRONT
     }
 
     @Inject
@@ -91,8 +94,8 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
         }
 
         this.armState = ArmState.STOPPED;
-        this.leftArmAtLimit = LimitState.NOT_AT_LIMIT;
-        this.rightArmAtLimit = LimitState.NOT_AT_LIMIT;
+        this.leftArmLimitState = LimitState.NOT_AT_LIMIT;
+        this.rightArmLimitState = LimitState.NOT_AT_LIMIT;
     }
 
     public void setPower(double leftPower, double rightPower) {
@@ -106,15 +109,15 @@ public void setPower(double leftPower, double rightPower) {
         }
 
         // Check if arm at limit
-        if (leftArmAtLimit == LimitState.AT_FORWARD) {
+        if (leftArmLimitState == LimitState.AT_FORWARD) {
             leftPower = MathUtils.constrainDouble(leftPower, armPowerMin.get(), 0);
-        } else if (leftArmAtLimit == LimitState.AT_REVERSE) {
+        } else if (leftArmLimitState == LimitState.AT_REVERSE) {
             leftPower = MathUtils.constrainDouble(leftPower, 0, armPowerMax.get());
         }
 
-        if (rightArmAtLimit == LimitState.AT_FORWARD) {
+        if (rightArmLimitState == LimitState.AT_FORWARD) {
             rightPower = MathUtils.constrainDouble(rightPower, armPowerMin.get(), 0);
-        } else if (rightArmAtLimit == LimitState.AT_REVERSE) {
+        } else if (rightArmLimitState == LimitState.AT_REVERSE) {
             rightPower = MathUtils.constrainDouble(rightPower, 0, armPowerMax.get());
         }
 
@@ -164,6 +167,23 @@ public double ticksToShooterAngle(double ticks) {
         return ticks * 1000; // To be modified into ticks to shooter angle formula
     }
 
+    public double shooterAngleToTicks(double angle) {
+        return 0;
+    }
+
+    public double getUsefulArmPosition(UsefulArmPosition usefulArmPosition) {
+        double revolutions;
+        switch(usefulArmPosition) {
+            case DEFAULT -> revolutions = shooterAngleToTicks(40);
+            case RAISED -> revolutions = shooterAngleToTicks(40);
+            case COLLECTING_FROM_GROUND -> revolutions = shooterAngleToTicks(40);
+            case FIRING_FROM_SPEAKER_FRONT -> revolutions = shooterAngleToTicks(40);
+            default -> revolutions = shooterAngleToTicks(40);
+        }
+        return revolutions;
+    }
+
+
     public void armEncoderTicksUpdate() {
         aKitLog.record("ArmMotorLeftTicks", armMotorLeft.getPosition());
         aKitLog.record("ArmMotorRightTicks", armMotorRight.getPosition());
@@ -199,18 +219,18 @@ public void calibrateArmOffset() {
             }
         }
 
-        leftArmAtLimit = LimitState.NOT_AT_LIMIT;
-        rightArmAtLimit = LimitState.NOT_AT_LIMIT;
+        leftArmLimitState = LimitState.NOT_AT_LIMIT;
+        rightArmLimitState = LimitState.NOT_AT_LIMIT;
 
         if (armMotorLeft.getForwardLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) {
-            leftArmAtLimit = LimitState.AT_FORWARD;
+            leftArmLimitState = LimitState.AT_FORWARD;
         } else if (armMotorLeft.getReverseLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) {
-            leftArmAtLimit = LimitState.AT_REVERSE;
+            leftArmLimitState = LimitState.AT_REVERSE;
         }
         if (armMotorRight.getForwardLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) {
-            rightArmAtLimit = LimitState.AT_FORWARD;
+            rightArmLimitState = LimitState.AT_FORWARD;
         } else if (armMotorRight.getReverseLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) {
-            rightArmAtLimit = LimitState.AT_REVERSE;
+            rightArmLimitState = LimitState.AT_REVERSE;
         }
 
         aKitLog.record("HasCalibratedLeftArm", hasCalibratedLeft);

diff --git a/src/test/java/competition/subsystems/arm/ArmSoftLimitTest.java b/src/test/java/competition/subsystems/arm/ArmSoftLimitTest.java
@@ -0,0 +1,7 @@
+package competition.subsystems.arm;
+
+import competition.BaseCompetitionTest;
+
+public class ArmSoftLimitTest extends BaseCompetitionTest {
+
+}
diff --git a/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java b/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java
@@ -24,10 +24,10 @@ public class BaseArmCommandsTest extends BaseCompetitionTest {
     public void setUp() {
         super.setUp();
         arm = getInjectorComponent().armSubsystem();
-        extend = new ExtendArmCommand(arm);
-        retract = new RetractArmCommand(arm);
-        stop = new StopArmCommand(arm);
-        reconcile = new ReconcileArmAlignmentCommand(arm);
+        extend = getInjectorComponent().extendArmCommand();
+        retract = getInjectorComponent().retractArmCommand();
+        stop = getInjectorComponent().stopArmCommand();
+        reconcile = getInjectorComponent().reconcileArmAlignmentCommand();
     }
 
     private void checkMotorPower(double power) {