merging intake + mode progress

src/main/java/frc/robot/commands/arm/ArmMove.java

@@ -3,32 +3,27 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.arm.ArmSubsystem;
+import frc.robot.subsystems.arm.ArmSubsystem.ArmPositions;
 
 public class ArmMove extends CommandBase {
 
     private ArmSubsystem arm;
-    private double wristGoalRad, shooterPowerGoal;
-    private boolean intakeMotorStatus;
+    private ArmPositions armPos;
 
-    public ArmMove (ArmSubsystem arm, double wristGoalRad, double shooterPowerGoal, boolean intakeMotorStatus) {
+    public ArmMove (ArmSubsystem arm, ArmPositions armPos) {
         this.arm = arm;
-        this.wristGoalRad = wristGoalRad;
-        this.shooterPowerGoal = shooterPowerGoal;
-        this.intakeMotorStatus = intakeMotorStatus;
+        this.armPos = armPos;
     }
 
     @Override
     public void initialize() {
-        wristGoalRad = SmartDashboard.getNumber("wrist goal rad", wristGoalRad);
-        shooterPowerGoal = SmartDashboard.getNumber("shooter power goal", shooterPowerGoal);
-        intakeMotorStatus = SmartDashboard.getBoolean("intake motor activated", intakeMotorStatus);
-        arm.setWristGoalRads(wristGoalRad); arm.setShooterGoalPower(shooterPowerGoal); 
+        arm.setMode(armPos);
     }
 
     @Override
     public boolean isFinished() {
         // TODO Auto-generated method stub
-        return super.isFinished();
+        return arm.getArmPos() == ArmPositions.Idle;
     }
 
 

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -24,6 +24,25 @@ public class ArmSubsystem extends SubsystemBase{
     private double intakeGoalPower;
 
 
+    public static enum ArmPositions {
+
+        Intake(0,0,0),
+        ShootHigh(0,0,0),
+        ShootMid(0,0,0),
+        ShootLow(0,0,0),
+        Idle(0,0,0);
+
+        public final int wristPos, shooterPower, intakePower;
+
+        private ArmPositions(int wristPos, int shooterPower, int intakePower) {
+            this.wristPos = wristPos;
+            this.shooterPower = shooterPower;
+            this.intakePower = intakePower;
+        }
+    }
+
+    private ArmPositions currentArmPos = ArmPositions.Idle;
+
     public ArmSubsystem() {
         wristMotor = new CANSparkMax(Constants.ArmConstants.wristMotorID, MotorType.kBrushless);
         intakeMotor = new CANSparkMax(Constants.ArmConstants.intakeMotorID, MotorType.kBrushless);
@@ -33,6 +52,18 @@ public ArmSubsystem() {
 
     }
 
+
+    public void setMode(ArmPositions armPos) {
+        currentArmPos = armPos;
+        wristGoalRad = armPos.wristPos;
+        shooterGoalPower = armPos.shooterPower;
+        intakeGoalPower = armPos.intakePower;
+    }
+
+    public ArmPositions getArmPos() {
+        return currentArmPos;
+    }
+
     ////////////////////////////////////////////////////////////////////////////////////////////////////
 
     public double getWristPositionRad() {
@@ -56,9 +87,6 @@ private void checkWristAmps() {
         }
     }
 
-    public void setWristGoalRads(double rad) {
-        wristGoalRad = rad;
-    }
 
     private void wristControl() {
         // add TrapezoidProfile + feedforward later?
@@ -91,9 +119,6 @@ private void checkShooterAmps() {
         }
     }
 
-    public void setShooterGoalPower(double power) {
-        shooterGoalPower = power;
-    }
 
     public void shooterControl() {
         setShooterMotorPower(shooterGoalPower);