A couple of things:
First thing, in the initialize function of drive to distance, you should reset the PID. Otherwise the second time you call it the PID isn't zero'd
Quote:
|
driveTrain.getPIDController().reset();
|
Second thing,
Quote:
public void usePIDOutput(double output) {
driveMechanum_Cartesian(output, output, 0);
}
|
This seems wrong. I would assume that you only want to use the output on a single axis of the mecanum drive. I would expect it to be
Quote:
|
driveMechanum_Cartesian(output, 0, 0);
|