Set arm angle command (#68)

* update SetArmAngleCommand * update SetArmAngleCommand, add UsefulArmPosition
Team488 · Feb 3, 2024 · e062cfc · e062cfc
1 parent f425a70
commit e062cfc
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java
@@ -61,7 +61,8 @@ public enum UsefulArmPosition {
         STARTING_POSITION,
         COLLECTING_FROM_GROUND,
         FIRING_FROM_SPEAKER_FRONT,
-        FIRING_IN_AMP
+        FIRING_IN_AMP,
+        SCOOCH_NOTE
     }
 
     public enum ArmNearLimitState {
@@ -234,6 +235,7 @@ public double getUsefulArmPositionAngle(UsefulArmPosition usefulArmPosition) {
             case COLLECTING_FROM_GROUND -> angle = 0;
             case FIRING_FROM_SPEAKER_FRONT -> angle = 30;
             case FIRING_IN_AMP -> angle = 80;
+            case SCOOCH_NOTE -> angle = 60; // placeholder value, safe angle to let note through while still low
             default -> angle = 40;
         }
         return angle;

diff --git a/src/main/java/competition/subsystems/arm/commands/SetArmAngleCommand.java b/src/main/java/competition/subsystems/arm/commands/SetArmAngleCommand.java
@@ -13,14 +13,17 @@ public class SetArmAngleCommand extends BaseSetpointCommand {
 
     @Inject
     public SetArmAngleCommand(ArmSubsystem armSubsystem) {
-        addRequirements(armSubsystem);
         this.armSubsystem = armSubsystem;
     }
 
     public void setTargetAngle(double targetAngle) {
         this.targetAngle = targetAngle;
     }
 
+    public void setArmPosition(ArmSubsystem.UsefulArmPosition armPosition) {
+        this.targetAngle = armSubsystem.getUsefulArmPositionAngle(armPosition);
+    }
+
     @Override
     public void initialize() {
         armSubsystem.setTargetValue(targetAngle);