feat:Arm Subsystem

src/main/java/frc/robot/Constants.java

@@ -99,4 +99,8 @@ private DriveConstants() {
             ModuleConstants.GearRatios.L3
     );
   }
+
+  private static class ArmConstants {
+    private ArmConstants(){}
+  }
 }

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

@@ -24,6 +24,9 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
+import frc.robot.subsystems.arm.ArmIO;
+import frc.robot.subsystems.arm.ArmIOPrototype;
+import frc.robot.subsystems.arm.ArmSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon1;
@@ -34,6 +37,7 @@
 import frc.robot.subsystems.shooter.ShooterIOPrototype;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -45,13 +49,17 @@ public class RobotContainer {
   // Subsystems
   private final DriveSubsystem m_driveSubsystem;
   private final ShooterSubsystem m_shooter;
+  private final ArmSubsystem m_armSubsystem;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
 
   // Dashboard inputs
   private final LoggedDashboardChooser<Command> autoChooser;
 
+  private final LoggedDashboardNumber wristPower;
+  private final LoggedDashboardNumber armPower;
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     switch (Constants.currentMode) {
@@ -64,6 +72,7 @@ public RobotContainer() {
               new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
               new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
           m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
+          m_armSubsystem = new ArmSubsystem(new ArmIOPrototype());
         }
     case SIM -> {
       // Sim robot, instantiate physics sim IO implementations
@@ -76,6 +85,7 @@ public RobotContainer() {
             new ModuleIOSim(DriveConstants.BL_MOD_CONSTANTS),
             new ModuleIOSim(DriveConstants.BR_MOD_CONSTANTS));
         m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
+        m_armSubsystem = new ArmSubsystem(new ArmIO() {});
       }
     default -> {
       // Replayed robot, disable IO implementations
@@ -92,12 +102,17 @@ public RobotContainer() {
                         new ModuleIO() {
                         });
         m_shooter = new ShooterSubsystem(new ShooterIO() {});
+        m_armSubsystem = new ArmSubsystem(new ArmIO() {});
       }
     }
 
     // Set up auto routines
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
+
+    armPower = new LoggedDashboardNumber("Arm Power", 0.0);
+    wristPower = new LoggedDashboardNumber("Wrist Power", 0.0);
+
     // Set up feedforward characterization
     autoChooser.addOption(
         "Drive FF Characterization",
@@ -133,9 +148,15 @@ private void configureButtonBindings() {
                             new Pose2d(m_driveSubsystem.getPose().getTranslation(), new Rotation2d())),
                             m_driveSubsystem)
                 .ignoringDisable(true));
+    controller.a().whileTrue(m_armSubsystem.setShoulderPowerFactory())
+            .whileFalse(m_armSubsystem.setShoulderPowerFactory(0.0));
+    controller.y().whileTrue(m_armSubsystem.setShoulderPowerFactory())
+            .whileFalse(m_armSubsystem.setWristPowerFactory(0.0));
+    controller.leftBumper().whileTrue(m_armSubsystem.setWristPowerFactory())
+            .whileFalse(m_armSubsystem.setWristPowerFactory(0.0));
+    controller.rightBumper().whileTrue(m_armSubsystem.setWristPowerFactory())
+            .whileFalse(m_armSubsystem.setWristPowerFactory(0.0));
 
-    controller.a().whileTrue(m_shooter.setShooterPower(0.6, 0.6))
-            .whileFalse(m_shooter.setShooterPower(0.0, 0.0));
   }
 
   /**

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

@@ -0,0 +1,16 @@
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ArmIO {
+    @AutoLog
+    class ShooterIOInputs {}
+
+    default void setShoulderVoltage(double voltage) {}
+
+    default void setShoulderAngle(double degrees) {}
+
+    default void setWristVoltage(double voltage) {}
+
+    default void setWristAngle(double degrees) {}
+}

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

@@ -0,0 +1,31 @@
+package frc.robot.subsystems.arm;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+public class ArmIOPrototype implements ArmIO {
+    private final TalonFX m_shoulder;
+    private final TalonFX m_wrist;
+    public ArmIOPrototype() {
+        m_shoulder = new TalonFX(23);
+        m_wrist = new TalonFX(24);
+
+    }
+
+    @Override
+    public void setShoulderVoltage(double voltage){
+        m_shoulder.setVoltage(voltage);
+    }
+
+    @Override
+    public void setShoulderAngle(double degrees){
+    }
+
+    @Override
+    public void setWristVoltage(double voltage){
+        m_wrist.setVoltage(voltage);
+    }
+
+    @Override
+    public void setWristAngle(double degrees){
+    }
+}

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

@@ -0,0 +1,28 @@
+package frc.robot.subsystems.arm;
+
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ArmSubsystem extends SubsystemBase {
+    private final ArmIO m_io;
+    public ArmSubsystem(ArmIO io) {
+        m_io = io;
+    }
+
+    public void setShoulderPower(double power){
+        m_io.setShoulderVoltage(power * 12.0);
+    }
+    public void setWristPower(double power){
+        m_io.setWristVoltage(power * 12);
+    }
+
+    public Command setShoulderPowerFactory(double power) {
+        return runOnce(() -> setShoulderPower(power));
+    }
+
+    public Command setWristPowerFactory(double power) {
+        return runOnce(() -> setWristPower(power));
+    }
+}
+