added clamp to PID output

SaintsRobotics · Feb 24, 2024 · 4a6d9f8 · 4a6d9f8
1 parent 2ba8338
commit 4a6d9f8
Showing 1 changed file with 8 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -10,6 +10,7 @@
 import com.revrobotics.Rev2mDistanceSensor;
 import com.revrobotics.Rev2mDistanceSensor.Port;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -23,13 +24,14 @@ public class IntakeSubsystem extends SubsystemBase {
   private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
   private CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless);
 
-  private PIDController m_armPID = new PIDController(0.5, 0, 0);
+  private PIDController m_armPID = new PIDController(0.0015, 0, 0);
 
   private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
 
   private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
 
   private double m_intakeSpeed = 0;
+  private double m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
 
   /** Creates a new IntakeSubsystem */
   public IntakeSubsystem() {
@@ -40,7 +42,7 @@ public IntakeSubsystem() {
     m_armEncoder.setDistancePerRotation(360);
 
     m_intakeMotor.setIdleMode(IdleMode.kCoast);
-    m_armMotor.setIdleMode(IdleMode.kBrake);
+    m_armMotor.setIdleMode(IdleMode.kCoast);
 
     // m_armPID.disableContinuousInput();
     m_armPID.setTolerance(0.05);
@@ -50,15 +52,15 @@ public IntakeSubsystem() {
   }
 
   public void armExtend() {
-    m_armPID.setSetpoint(IntakeConstants.kIntakeLoweredAngle);
+    m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
 
     stopIntake();
 
     deployed = true;
   }
 
   public void armRetract() {
-    m_armPID.setSetpoint(IntakeConstants.kIntakeRaisedAngle);
+    m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
 
     stopIntake();
 
@@ -99,13 +101,13 @@ public void periodic() {
       armRetract();
     }
 
-    // m_armMotor.set(m_armPID.calculate(m_armEncoder.getDistance()));
+    // m_armMotor.set(MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3));
     // m_intakeMotor.set(m_intakeSpeed);
 
     SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
     SmartDashboard.putBoolean("Arm Deployed?", deployed);
     SmartDashboard.putBoolean("Have Note?", haveNote);
-    SmartDashboard.putNumber("pid output", m_armPID.calculate(m_armEncoder.getDistance()));
+    SmartDashboard.putNumber("pid output", m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint));
   }
 
   public boolean readyToShoot() {