Is this the right way to do PID? currently have ID disabled for simplicity.
error = hypot(global_y - ytarget, global_x - xtarget);
derivative = error - prevError;
integral = integral + error;
if (error == 0){
integral = 0;
}
if (error > 40){
integral = 0;
}
double scaling = fmin(kP * error, 10);
There is an issue where scaling grows instead of shrinking even if the robot is getting closer to it’s target.
I believe I’ve gotten it fixed. It was a problem with the movement algorithm.