Co-authored-by: Elijah <3lijaaah@users.noreply.github.com>

Co-authored-by: williamlin108 <williamlin108@users.noreply.github.com> Co-authored-by: Toomy231 <Toomy231@users.noreply.github.com>
team422 · Jan 19, 2024 · 160cdbb · 160cdbb
1 parent 026056d
commit 160cdbb
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java b/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java
@@ -153,4 +153,10 @@ public Trigger autoIntakeCube() {
   public Trigger ledFlash() {
     return m_rightJoystick.button(8);
   }
+
+  @Override
+  public Trigger jamesButton() {
+    return m_leftJoystick.button(8);
+  }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/wrist/Wrist.java b/src/main/java/frc/robot/subsystems/wrist/Wrist.java
@@ -27,18 +27,24 @@ public class Wrist extends SubsystemBase {
 
   private double m_lastTime;
   private double m_lastVelocitySetpoint;
+  private boolean m_PIDSet;
 
-  public Wrist(WristIO io, ProfiledPIDController wristPIDController, ArmFeedforward feedForward, Rotation2d minAngle,
+  public Wrist(WristIO io, ProfiledPIDController wristFastPIDController, ArmFeedforward feedForward,
+      Rotation2d minAngle,
       Rotation2d maxAngle) {
     m_io = io;
     m_inputs = new WristInputsAutoLogged();
 
-    m_controller = wristPIDController;
-    m_feedforward = feedForward;
-    m_controller.setTolerance(Units.degreesToRadians(2));
-    kMinAngle = minAngle.getRadians();
-    kMaxAngle = maxAngle.getRadians();
-    m_controller.setIntegratorRange(-0.5, 0.5);
+    {
+
+      m_controller = wristFastPIDController;
+      m_feedforward = feedForward;
+      m_controller.setTolerance(Units.degreesToRadians(2));
+      kMinAngle = minAngle.getRadians();
+      kMaxAngle = maxAngle.getRadians();
+      m_controller.setIntegratorRange(-0.5, 0.5);
+      m_PIDSet = false;
+    }
 
     // m_controller.reset(0);
   }
@@ -77,6 +83,7 @@ public void periodic() {
     // Logger.getInstance().recordOutput("Wrist/OutputVoltage", outputVoltage);
     Logger.getInstance().recordOutput("Wrist/SetpointDegrees", m_desiredAngle.getDegrees());
     Logger.getInstance().recordOutput("Wrist/AngleDeg", Units.radiansToDegrees(m_inputs.angleRad));
+    Logger.getInstance().recordOutput("Wrist/FastPID Enabled", m_PIDSet);
 
     m_lastTime = Timer.getFPGATimestamp();
     m_lastVelocitySetpoint = velocitySetpoint;
@@ -116,6 +123,28 @@ public Command testSetAngleCommand(Rotation2d angle) {
     return testSetAngleCommand(angle, Units.degreesToRadians(3.0));
   }
 
+  public void setPIDNo(double kFastWristP, double kFastWristI, double kFastWristD) {
+    m_controller.setP(kFastWristP);
+    m_controller.setD(kFastWristD);
+    m_controller.setI(kFastWristI);
+    m_PIDSet = !m_PIDSet;
+  }
+
+  public Command Rastaclat(double kFastWristP, double kFastWristI, double kFastWristD, double kWristP, double kWristI,
+      double kWristD) {
+    return runOnce(() -> {
+      if (m_PIDSet) {
+        setPIDNo(kFastWristP, kFastWristI, kFastWristD);
+      } else {
+        m_controller.setP(kWristP);
+        m_controller.setD(kWristD);
+        m_controller.setI(kWristI);
+        m_PIDSet = true;
+      }
+    });
+
+  }
+
   public Command testSetAngleCommand(Rotation2d angle, double toleranceRadians) {
     return Commands.sequence(
         testSetAngleCommandOnce(angle),