Using a rangefinder and a PID controller with a lift (Java)

We are attempting to use a rangefinder and a PIDController to set the height of our lift. Our lift has a winch that is only able to lift the elevator, and relies on gravity to pull the elevator down. When I currently test the code, I set the pidController to values between 0.1134 volts and 0.6048 volts, which correspond to ranges between 12 and 64 inches. These values have been verified by printing both the inches and volts output from the rangefinder.

If I set the PID controller to 0.1 volts, the winch spins in the wrong direction at a slow speed, and it slowly unspools the cable. If I set it to 0.2 volts, it also unspools in the wrong direction, just faster. As I increase the PID controller voltage setpoint, the speed it unspools increases.

What the controller should do is raise until the setpoint matches the voltage, and then maintain position, but it only unspools the wire at various speeds.

Does anyone have any idea what my problem is? Here is the code:



public class Robot extends IterativeRobot {

  Victor elevatorMotor;
  AnalogInput rangefinder;
  Joystick right;
  PIDController elevatorControl;

  double elevatorPoint = 0;

  public void robotInit() {
    
    elevatorMotor = new Victor(4);
    rangefinder = new AnalogInput(0);
    right = new Joystick(1);
    elevatorControl = new PIDController(0.5, 0.001, 0, rangefinder, elevatorMotor);
    elevatorControl.enable();

  }

  public void teleopPeriodic() {

    double range = (rangefinder.getVoltage()) / 0.00945;
    double volts = rangefinder.getVoltage();

    if (right.getRawButton(6) == true) {
      elevatorPoint = 0.1134;
    } else if (right.getRawButton(7) == true) {
      elevatorPoint = 0.2268;
    } else if (right.getRawButton(8) == true) {
       elevatorPoint = 0.3402;
    } else if (right.getRawButton(9) == true) {
       elevatorPoint = 0.4536;
    } else if (right.getRawButton(10) == true) { 
       elevatorPoint = 0.5670;
    } else if (right.getRawButton(11) == true) {
       elevatorPoint = 0.6048;
    }

    elevatorControl.setSetPoint(elevatorControl);

    Timer.delay(0.01);

  }

}



Is the problem that the motor is just running in the wrong direction? The simple fix is to swap its wires coming from the speed controller. :wink: The software fix is to negate the PID constants so that it computes a control value that goes in the other direction from what it’s doing now.

The fix really was that simple. Thanks for your help!