Create arm subsystem

src/main/java/frc/robot/RobotContainer.java

@@ -10,12 +10,16 @@
 import frc.robot.commands.ArcadeDrive;
 import frc.robot.commands.AutonomousDistance;
 import frc.robot.commands.AutonomousTime;
-import frc.robot.subsystems.Arm;
+import frc.robot.commands.DriveDistance;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.arm.ArmIO;
+import frc.robot.subsystems.arm.ArmIOXRP;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.xrp.XRPOnBoardIO;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -31,7 +35,7 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final Drivetrain m_drivetrain = new Drivetrain();
   private final XRPOnBoardIO m_onboardIO = new XRPOnBoardIO();
-  private final Arm m_arm = new Arm();
+  private Arm m_arm = new Arm(new ArmIOXRP());
 
   // Assumes a gamepad plugged into channel 0
   private final Joystick m_controller = new Joystick(0);
@@ -64,13 +68,28 @@ private void configureButtonBindings() {
 
     JoystickButton joystickAButton = new JoystickButton(m_controller, 1);
     joystickAButton
-        .onTrue(new InstantCommand(() -> m_arm.setAngle(45.0), m_arm))
-        .onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm));
+        .onTrue(new InstantCommand(() -> m_arm.goToAngle(45.0), m_arm))
+        .onFalse(new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm));
+
 
     JoystickButton joystickBButton = new JoystickButton(m_controller, 2);
     joystickBButton
-        .onTrue(new InstantCommand(() -> m_arm.setAngle(90.0), m_arm))
-        .onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm));
+        .onTrue(
+          new InstantCommand(() -> m_arm.goToAngle(90.0), m_arm))
+        .onFalse(new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm));
+
+
+    JoystickButton joystickCButton = new JoystickButton(m_controller, 3);
+
+    joystickCButton
+        .onTrue(Commands.sequence(
+            new DriveDistance(0.5, 10, m_drivetrain),
+            new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm),
+            Commands.waitSeconds(1),
+            new InstantCommand(() -> m_arm.goToAngle(180.0), m_arm),
+            new DriveDistance(-0.5, 10, m_drivetrain)
+        ))
+        .onFalse(new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm));
 
     // Setup SmartDashboard options
     m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));

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

@@ -0,0 +1,27 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.arm.ArmIO.ArmIOInputs;
+
+public class Arm extends SubsystemBase {
+    private ArmIO io;
+
+    private final ArmIOInputs inputs = new ArmIOInputs();
+
+    public Arm(ArmIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+    }
+
+    public void goToAngle(double angleDeg) {
+        io.setAngle(angleDeg);
+    }
+
+    public double getAngle() {
+        return inputs.positionDeg;
+    }
+}

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

@@ -0,0 +1,17 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.wpilibj.xrp.XRPServo;
+
+public interface ArmIO {
+
+    public static class ArmIOInputs {
+
+        public double positionDeg = 0.0;
+
+    }
+
+    public default void updateInputs(ArmIOInputs inputs){}
+
+    public default void setAngle(double angleDeg){}
+}
+

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

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.wpilibj.xrp.XRPServo;
+
+public class ArmIOXRP implements ArmIO{
+
+    private final XRPServo armMotor = new XRPServo(4);
+
+    @Override
+    public void updateInputs(ArmIOInputs inputs) {
+        inputs.positionDeg = armMotor.getAngle();
+    }
+
+    @Override
+    public void setAngle(double angleDeg) {
+        armMotor.setAngle(angleDeg);
+    }
+
+}