feat: Arm PID

TitaniumTitans · Jan 28, 2024 · 6650605 · 6650605
1 parent ef54c14
commit 6650605
Show file tree

Hide file tree

Showing 3 changed files with 63 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -58,7 +58,9 @@ public class RobotContainer {
   private final LoggedDashboardChooser<Command> autoChooser;
 
   private final LoggedDashboardNumber wristPower;
+  private final LoggedDashboardNumber wristPosition;
   private final LoggedDashboardNumber armPower;
+  private final LoggedDashboardNumber armPosition;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -83,7 +85,7 @@ public RobotContainer() {
                   new ModuleIO() {},
                   new ModuleIO() {});
           m_shooter = new ShooterSubsystem(new ShooterIO() {});
-          m_armSubsystem = new ArmSubsystem(new ArmIOPrototype());
+          m_armSubsystem = new ArmSubsystem(new ArmIOPrototype() {});
     }
     case SIM -> {
       // Sim robot, instantiate physics sim IO implementations
@@ -122,7 +124,9 @@ public RobotContainer() {
 
 
     armPower = new LoggedDashboardNumber("Arm Power", 0.0);
+    armPosition = new LoggedDashboardNumber("Arm Position", 0.0);
     wristPower = new LoggedDashboardNumber("Wrist Power", 0.0);
+    wristPosition = new LoggedDashboardNumber("Wrist Position", 0.0)
 
     // Set up feedforward characterization
     autoChooser.addOption(
@@ -164,11 +168,15 @@ private void configureButtonBindings() {
             .whileFalse(m_armSubsystem.setShoulderPowerFactory(0.0));
     controller.y().whileTrue(m_armSubsystem.setShoulderPowerFactory(-armPower.get()))
             .whileFalse(m_armSubsystem.setShoulderPowerFactory(0.0));
+    controller.b().whileTrue(m_armSubsystem.setShoulderPositionFactory(armPosition.get()))
+            .whileFalse(m_armSubsystem.setShoulderPositionFactory(0.0));
 
     controller.leftBumper().whileTrue(m_armSubsystem.setWristPowerFactory(wristPower.get()))
             .whileFalse(m_armSubsystem.setWristPowerFactory(0.0));
     controller.rightBumper().whileTrue(m_armSubsystem.setWristPowerFactory(-wristPower.get()))
             .whileFalse(m_armSubsystem.setWristPowerFactory(0.0));
+      controller.x().whileTrue(m_armSubsystem.setWristPositionFactory(wristPosition.get()))
+              .whileFalse(m_armSubsystem.setWristPositionFactory(0.0));
 
   }
 

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java
@@ -1,23 +1,62 @@
 package frc.robot.subsystems.arm;
 
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.hardware.core.CoreTalonFX;
+import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
+import lib.properties.phoenix6.PidPropertyPublic;
 
 public class ArmIOPrototype implements ArmIO {
     private final TalonFX m_shoulder;
     private final TalonFX m_wrist;
+    private final PidPropertyPublic m_shoulderPID;
+    private final PidPropertyPublic m_wristPID;
+
     public ArmIOPrototype() {
         m_shoulder = new TalonFX(23);
         m_wrist = new TalonFX(24);
 
+        m_shoulderPID = new Phoenix6PidPropertyBuilder(
+                "Arm/Shoulder PID",
+                false,
+                m_shoulder,
+                0)
+                .addP(0.0)
+                .addI(0.0)
+                .addD(0.0)
+                .build();
+
+        m_wristPID = new Phoenix6PidPropertyBuilder(
+                "Arm/Wrist PID",
+                false,
+                m_wrist,
+                0)
+                .addP(0.0)
+                .addI(0.0)
+                .addD(0.0)
+                .build();
+
     }
 
     @Override
     public void setShoulderVoltage(double voltage){
         m_shoulder.setVoltage(voltage);
     }
 
+    @Override
+    public void setShoulderAngle(double degrees) {
+        final PositionVoltage m_request = new PositionVoltage(0).withSlot(0);
+        m_shoulder.setControl(m_request.withPosition(degrees / 360.0));
+    }
+
     @Override
     public void setWristVoltage(double voltage){
         m_wrist.setVoltage(voltage);
     }
+
+    @Override
+    public void setWristAngle(double degrees) {
+        final PositionVoltage m_request = new PositionVoltage(0).withSlot(0);
+        m_wrist.setControl(m_request.withPosition(degrees / 360.0));
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
@@ -10,19 +10,28 @@ 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.0);
-    }
+    public void setShoulderPower(double power) {m_io.setShoulderVoltage(power * 12.0);}
+
+    public void setShoulderPosition(double degrees) {m_io.setShoulderAngle(degrees);}
+
+    public void setWristPower(double power) {m_io.setWristVoltage(power * 12.0);}
+
+    public void setWristPosition(double degrees) {m_io.setWristAngle(degrees);}
 
     public Command setShoulderPowerFactory(double power) {
         return runOnce(() -> setShoulderPower(power));
     }
 
+    public Command setShoulderPositionFactory(double degrees) {
+        return runOnce(() -> setShoulderPosition(degrees));
+    }
+
     public Command setWristPowerFactory(double power) {
         return runOnce(() -> setWristPower(power));
     }
+
+    public Command setWristPositionFactory(double degrees) {
+        return runOnce(() -> setWristPosition(degrees));
+    }
 }