Configure pid for climber

[erickrause]

Found this example of on the rev site. Looks like they use really small values. https://github.com/REVrobotics/SPARK-MAX-Examples/blob/master/Java/Position%20Closed%20Loop%20Control/src/main/java/frc/robot/Robot.java#L48

Interesting vid to help explain the different settings. I'm still confused, but it helped. https://www.youtube.com/watch?v=dZ8lzDi3cXY

Some comments from a few years ago. https://www.chiefdelphi.com/t/tune-rev-spark-max-pid-for-shooter/379068

[erickrause]

I roughly put the problem into chatgpt. Looks like the issue was that we weren't using the periodic method to set outputs. This isn't perfect for us, but you should be able to get the interesting bits from this example and conform to your class

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.AHRS;
import edu.wpi.first.wpilibj.SPI;

public class ClimberSubsystem extends SubsystemBase {
    private final CANSparkMax motor1 = new CANSparkMax(1, MotorType.kBrushless);
    private final CANSparkMax motor2 = new CANSparkMax(2, MotorType.kBrushless);
    private final PIDController pidController1 = new PIDController(0.1, 0, 0);
    private final PIDController pidController2 = new PIDController(0.1, 0, 0);
    private final AHRS gyro = new AHRS(SPI.Port.kMXP);

    private double targetPosition = 0;

    public ClimberSubsystem() {
        // Reset motors to ensure they're in a known state
        motor1.restoreFactoryDefaults();
        motor2.restoreFactoryDefaults();

        // Assume motors need to be inverted to move in the same direction
        motor1.setInverted(false);
        motor2.setInverted(true);

        // Setup PID controllers
        pidController1.setTolerance(0.5); // Assuming a tolerance of 0.5 units for position
        pidController2.setTolerance(0.5);

        // Setup gyro if needed
        gyro.reset();
    }

    public void moveToPosition(double position) {
        this.targetPosition = position;
    }

    @Override
    public void periodic() {
        // Adjust target positions based on gyro to maintain balance
        double angle = gyro.getAngle();
        double adjustment = angle * 0.05; // Example proportional adjustment

        double motor1Target = this.targetPosition + adjustment;
        double motor2Target = this.targetPosition - adjustment;

        // Example PID control loop for each motor
        double motor1Output = pidController1.calculate(motor1.getEncoder().getPosition(), motor1Target);
        double motor2Output = pidController2.calculate(motor2.getEncoder().getPosition(), motor2Target);

        motor1.set(motor1Output);
        motor2.set(motor2Output);
    }

    public boolean atTargetPosition() {
        return pidController1.atSetpoint() && pidController2.atSetpoint();
    }
}