See where the problem is? The PID function wants whatever measurement is returned from Hood.getMeasurement() to be as close as possible to the setpoint, which means the setpoint is an angle (since Hood.getMeasurement() returns an angle), yet Hood.atSetpoint() compares it against a voltage.
Rather than fixing your own atSetpoint() implementation, why not use [the WPILib one](https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/PIDController.html#atSetpoint())? It's on PIDController instead of PIDCommand; just have Hood.atSetpoint()return getController().atSetpoint(). This implementation also does a more intelligent/lenient check than strict equality based on a tolerance measurement that you're actually already configuring
The measurement used in
Hood.atSetpoint()
doesn't line up with the actual measurements used elsewhere in the PID. CompareHood.atSetpoint()
https://github.com/FRC5892/2021-Infinite-Recharge-2.0/blob/727d4510f3a098cd120b105c9afb0785f5f91b08/src/main/java/frc/robot/subsystems/Hood.java#L47
to
Hood.getMeasurement()
https://github.com/FRC5892/2021-Infinite-Recharge-2.0/blob/727d4510f3a098cd120b105c9afb0785f5f91b08/src/main/java/frc/robot/subsystems/Hood.java#L60
See where the problem is? The PID function wants whatever measurement is returned from
Hood.getMeasurement()
to be as close as possible to the setpoint, which means the setpoint is an angle (sinceHood.getMeasurement()
returns an angle), yetHood.atSetpoint()
compares it against a voltage.Rather than fixing your own
atSetpoint()
implementation, why not use [the WPILib one](https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/PIDController.html#atSetpoint())? It's onPIDController
instead ofPIDCommand
; just haveHood.atSetpoint()
return getController().atSetpoint()
. This implementation also does a more intelligent/lenient check than strict equality based on a tolerance measurement that you're actually already configuringhttps://github.com/FRC5892/2021-Infinite-Recharge-2.0/blob/727d4510f3a098cd120b105c9afb0785f5f91b08/src/main/java/frc/robot/subsystems/Hood.java#L29
and then never using.