create the commands for the arm & claw

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

@@ -5,14 +5,22 @@
 package frc.robot;
 
 import com.dacubeking.AutoBuilder.robot.GuiAuto;
+import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderAccessible;
 import com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.RamseteController;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.arm.ArmIO;
+import frc.robot.subsystems.arm.ArmIOSparkMax;
+import frc.robot.subsystems.claw.Claw;
+import frc.robot.subsystems.claw.ClawIO;
+import frc.robot.subsystems.claw.ClawIOSparkMax;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveIO;
 import frc.robot.subsystems.drive.DriveIOFalcon500;
@@ -28,33 +36,68 @@
  */
 public class RobotContainer {
     // Subsystems
-    private final Drive drive;
+    private Drive drive;
+    private Claw claw;
+    private Arm arm;
     // Controller
     private final CommandXboxController controller = new CommandXboxController(0);
 
     // Dashboard inputs
     private final LoggedDashboardChooser<String> autoChooser = new LoggedDashboardChooser<>("Auto Choices");
     private final LoggedDashboardChooser<String> sideChooser = new LoggedDashboardChooser<>("Side Chooser");
 
-    private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0);
+    private final LoggedDashboardNumber flywheelSpeed = new LoggedDashboardNumber("Flywheel Speed", 1500.0);
+    private final LoggedDashboardNumber shootAngle = new LoggedDashboardNumber("Shoot Angle", 40);
+    private final LoggedDashboardNumber pickupAngle = new LoggedDashboardNumber("Pickup Angle", -20);
+    private final LoggedDashboardNumber trapAngle = new LoggedDashboardNumber("Trap Angle", 100);
+
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         switch (Constants.currentMode) {
             // Real robot, instantiate hardware IO implementations
-            case REAL -> drive = new Drive(new DriveIOFalcon500());
+            case REAL -> {
+                drive = new Drive(new DriveIOFalcon500());
+                claw = new Claw(new ClawIOSparkMax());
+                arm = new Arm(new ArmIOSparkMax());
+            }
 
             // drive = new Drive(new DriveIOFalcon500());
             // Sim robot, instantiate physics sim IO implementations
-            case SIM -> drive = new Drive(new DriveIOSim());
+            case SIM -> {
+                drive = new Drive(new DriveIOSim());
+                claw = new Claw(new ClawIOSparkMax());
+                arm = new Arm(new ArmIOSparkMax());
+            }
 
             // Replayed robot, disable IO implementations
-            default -> drive = new Drive(new DriveIO() {
-            });
+            default -> {
+                drive = new Drive(new DriveIO() {
+                });
+                claw = new Claw(new ClawIO() {
+                });
+                arm = new Arm(new ArmIO() {
+                });
+            }
         }
 
+        // Configure the button bindings
+        configureButtonBindings();
+
+        claw.setDefaultCommand(new RunCommand(
+                () -> {
+                    claw.setShooterVelocity(0, 0);
+                    claw.setIntakeVelocity(0);
+                }, claw
+        ));
+
+        arm.setDefaultCommand(new RunCommand(
+                () -> arm.setArmPosition(arm.getArmPosition()),
+                arm
+        ));
+
         // Initialize autonomous container
         AutonomousContainer.getInstance().setDebugPrints(true);
         AutonomousContainer.getInstance().initialize(
@@ -81,11 +124,47 @@ public RobotContainer() {
         sideChooser.addOption("Red", "red");
 
         AutonomousContainer.getInstance().getAutonomousNames().forEach(name -> autoChooser.addOption(name, name));
-
-        // Configure the button bindings
-        configureButtonBindings();
     }
 
+
+    @AutoBuilderAccessible
+    Command intakeCommand = new RunCommand(
+            () -> {
+                claw.setIntakeVelocity(0.5);
+                arm.setArmPosition(pickupAngle.get());
+            }, claw, arm
+    );
+
+    @AutoBuilderAccessible
+    Command outtakeCommand = new RunCommand(
+            () -> {
+                claw.setIntakeVelocity(-0.5);
+            }, claw
+    );
+
+    @AutoBuilderAccessible
+    Command shootCommand = new RunCommand(
+            () -> {
+                claw.setShooterVelocity(flywheelSpeed.get(), flywheelSpeed.get());
+                arm.setArmPosition(shootAngle.get());
+            }, claw, arm)
+            .until(() -> Math.abs(arm.getArmPosition() - shootAngle.get()) < 5
+                    && Math.abs(claw.getShooterVelocity() - flywheelSpeed.get()) < 100)
+            .andThen(() -> {
+                claw.setIntakeVelocity(1000);
+                claw.setShooterVelocity(flywheelSpeed.get(), flywheelSpeed.get());
+                arm.setArmPosition(shootAngle.get());
+            }, claw, arm);
+
+    @AutoBuilderAccessible
+    Command scoreTrapCommand = new RunCommand(() -> {
+        arm.setArmPosition(trapAngle.get());
+        claw.setIntakeVelocity(0);
+    }, arm, claw)
+            .until(() -> Math.abs(arm.getArmPosition() - trapAngle.get()) < 5)
+            .andThen(() -> claw.setIntakeVelocity(100), arm, claw);
+
+
     /**
      * Use this method to define your button->command mappings. Buttons can be created by instantiating a
      * {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}),

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

@@ -1,7 +1,30 @@
 package frc.robot.subsystems.arm;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.arm.ArmIO.ArmInputs;
 
 public class Arm extends SubsystemBase {
 
+    ArmIO io;
+    ArmInputs inputs = new ArmInputs();
+
+    public Arm(ArmIO io) {
+        this.io = io;
+    }
+
+    /**
+     * @param position The position to set the arm to in degrees
+     */
+    public void setArmPosition(double position) {
+        io.setArmPosition(position);
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+    }
+
+    public double getArmPosition() {
+        return inputs.armPosition;
+    }
 }

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

@@ -1,7 +1,7 @@
 package frc.robot.subsystems.arm;
 
 public interface ArmIO {
-    public static class ArmIOInputs {
+    public static class ArmInputs {
         public double armPosition = 0.0;
         public double armVelocity = 0.0;
         public double armCurrent = 0.0;
@@ -12,7 +12,7 @@ public static class ArmIOInputs {
     /**
      * Updates the set of loggable inputs.
      */
-    public default void updateInputs(ArmIOInputs inputs) {
+    public default void updateInputs(ArmInputs inputs) {
 
     }
 

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

@@ -22,7 +22,7 @@ public ArmIOSparkMax() {
 
     }
 
-    public void updateInputs(ArmIOInputs inputs) {
+    public void updateInputs(ArmInputs inputs) {
         inputs.armPosition = arm.getEncoder().getPosition();
         inputs.armVelocity = arm.getEncoder().getVelocity();
         inputs.armCurrent = arm.getOutputCurrent();

src/main/java/frc/robot/subsystems/claw/Claw.java

@@ -1,7 +1,31 @@
 package frc.robot.subsystems.claw;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.claw.ClawIO.ClawInputs;
 
 public class Claw extends SubsystemBase {
 
+    ClawIO io;
+    ClawInputs inputs = new ClawInputs();
+
+    public Claw(ClawIO io) {
+        this.io = io;
+    }
+
+    public void setShooterVelocity(double leftVelocity, double rightVelocity) {
+        io.setShooterVelocity(leftVelocity, rightVelocity);
+    }
+
+    public void setIntakeVelocity(double velocity) {
+        io.setIntakeVelocity(velocity);
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+    }
+
+    public double getShooterVelocity() {
+        return (inputs.shooterLeftVelocity + inputs.shooterRightVelocity) / 2;
+    }
 }

src/main/java/frc/robot/subsystems/claw/ClawIO.java

@@ -1,7 +1,7 @@
 package frc.robot.subsystems.claw;
 
 public interface ClawIO {
-    public static class ClawIOInputs {
+    public static class ClawInputs {
         public double shooterLeftVoltage = 0.0;
         public double shooterRightVoltage = 0.0;
         public double shooterLeftCurrent = 0.0;
@@ -26,7 +26,7 @@ public static class ClawIOInputs {
     /**
      * Updates the set of loggable inputs.
      */
-    public default void updateInputs(ClawIOInputs inputs) {
+    public default void updateInputs(ClawInputs inputs) {
     }
 
     public default void setShooterVelocity(double leftVelocity, double rightVelocity) {

src/main/java/frc/robot/subsystems/claw/ClawIOSparkMax.java

@@ -49,7 +49,7 @@ public ClawIOSparkMax() {
         intake.getPIDController().setFF(0.0);
     }
 
-    public void updateInputs(ClawIOInputs inputs) {
+    public void updateInputs(ClawInputs inputs) {
         inputs.shooterLeftVoltage = shooterLeft.getBusVoltage();
         inputs.shooterRightVoltage = shooterRight.getBusVoltage();
         inputs.shooterLeftCurrent = shooterLeft.getOutputCurrent();