.gitignore

@@ -160,3 +160,4 @@ bin/
 
 # Simulation GUI and other tools window save file
 *-window.json
+simgui*.json

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

@@ -5,19 +5,20 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.button.Button;
 import frc.robot.commands.ArcadeDrive;
 import frc.robot.commands.AutonomousDistance;
 import frc.robot.commands.AutonomousTime;
+import frc.robot.commands.TeleopArm;
+import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.OnBoardIO;
 import frc.robot.subsystems.OnBoardIO.ChannelMode;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
-import edu.wpi.first.wpilibj2.command.button.Button;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -29,9 +30,13 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final Drivetrain m_drivetrain = new Drivetrain();
   private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
+  private final Arm m_arm = new Arm();
 
+
   // Assumes a gamepad plugged into channnel 0
-  private final Joystick m_controller = new Joystick(0);
+  private final XboxController m_controller = new XboxController(0);
+
+  private final TeleopArm teleopArm = new TeleopArm(m_arm, m_controller);
 
   // Create SmartDashboard chooser for autonomous routines
   private final SendableChooser<Command> m_chooser = new SendableChooser<>();
@@ -63,6 +68,7 @@ private void configureButtonBindings() {
     // Default command is arcade drive. This will run unless another command
     // is scheduled over it.
     m_drivetrain.setDefaultCommand(getArcadeDriveCommand());
+    m_arm.setDefaultCommand(teleopArm);
 
     // Example of how to use the onboard IO
     Button onboardButtonA = new Button(m_onboardIO::getButtonAPressed);

src/main/java/frc/robot/commands/TeleopArm.java

@@ -0,0 +1,42 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Arm;
+
+public class TeleopArm extends CommandBase {
+
+    private final XboxController controller;
+    private final Arm arm;
+
+    public TeleopArm(Arm arm, XboxController controller) {
+        this.arm = arm;
+        this.controller = controller;
+        addRequirements(arm);
+    }
+
+    @Override
+    public void execute() {
+        // Move lift with X and Y
+        if (controller.getYButton()) {
+            arm.raiseLift();
+        } else if (controller.getXButton()) {
+            arm.lowerLift();
+        }
+
+        // Move tilt with bumpers
+        if (controller.getRightBumper()) {
+            arm.tiltForward();
+        } else if (controller.getLeftBumper()) {
+            arm.tiltBackward();
+        }
+
+        // Move gripper with A and B
+        if (controller.getBButton()) {
+            arm.openGripper();
+        } else if (controller.getAButton()) {
+            arm.closeGripper();
+        }
+    }
+
+}

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

@@ -0,0 +1,120 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * Subsystem for Romi arm
+ */
+public class Arm extends SubsystemBase {
+
+    // Constants for min-max arm movments
+    public static final double LIFT_MIN = 0;
+    public static final double LIFT_MAX = 180;
+    public static final double TILT_MIN = 0;
+    public static final double TILT_MAX = 180;
+    public static final double GRIPPER_MIN = 0;
+    public static final double GRIPPER_MAX = 180;
+
+    // Servos that move the arm
+    private final Servo lift = new Servo(2);
+    private final Servo tilt = new Servo(3);
+    private final Servo gripper = new Servo(4);
+
+    // Current requested positions for the servos
+    private double liftAngle = 0;
+    private double tiltAngle = 0;
+    private double gripperAngle = 0;
+
+    @Override
+    public void periodic() {
+        // Send current requested positions to the SmartDashboard
+        SmartDashboard.putNumber("Lift Angle", liftAngle);
+        SmartDashboard.putNumber("Tilt Angle", tiltAngle);
+        SmartDashboard.putNumber("Gripper Angle", gripperAngle);
+    }
+
+    /**
+     * Raise the lift by 1-degree
+     * @return new lift angle
+     */
+    public double raiseLift() {
+        return setLift(liftAngle + 1);
+    }
+
+    /**
+     * Lower the lift by 1-degree
+     * @return new lift angle
+     */
+    public double lowerLift() {
+        return setLift(liftAngle - 1);
+    }
+
+    /**
+     * Set the lift angle. Should be a value between {@link #LIFT_MIN} and {@link #LIFT_MAX}.
+     * @param angle angle to set
+     * @return new lift angle
+     */
+    public double setLift(double angle) {
+        liftAngle = MathUtil.clamp(angle, LIFT_MIN, LIFT_MAX);
+        lift.setAngle(liftAngle);
+        return liftAngle;
+    }
+
+    /**
+     * Tilt the lift forward 1-degree
+     * @return new tilt angle
+     */
+    public double tiltForward() {
+        return setTilt(tiltAngle + 1);
+    }
+
+    /**
+     * Tilt the lift backward 1-degree
+     * @return new tilt angle
+     */
+    public double tiltBackward() {
+        return setTilt(tiltAngle - 1);
+    }
+
+    /**
+     * Set the lift tilt angle. Should be a value between {@link #TILT_MIN} and {@link #TILT_MAX}
+     * @param angle angle to set
+     * @return new tilt angle
+     */
+    public double setTilt(double angle) {
+        tiltAngle = MathUtil.clamp(angle, TILT_MIN, TILT_MAX);
+        tilt.setAngle(tiltAngle);
+        return tiltAngle;
+    }
+
+    /**
+     * Open the gripper 1-degree
+     * @return new gripper angle
+     */
+    public double openGripper() {
+        return setGripper(gripperAngle + 1);
+    }
+
+    /** 
+     * Close the gripper 1-degree
+     * @return new gripper angle
+     */
+    public double closeGripper() {
+        return setGripper(gripperAngle - 1);
+    }
+
+    /**
+     * Set the gripper angle. Should be a value between {@link #GRIPPER_MIN} and {@link #GRIPPER_MAX}
+     * @param angle
+     * @return
+     */
+    public double setGripper(double angle) {
+        gripperAngle = MathUtil.clamp(angle, GRIPPER_MIN, GRIPPER_MAX);
+        gripper.setAngle(gripperAngle);
+        return gripperAngle;
+    }
+
+}