Confusing Issues with Swerve

[GeeTwo]

Originally Posted by Calasada

and the efficiency test specifically is here:

double curAngle = getAngle();
if(Math.abs(angle - curAngle) > 90 && Math.abs(angle - curAngle) < 270 && angle != 0) {
      angle = ((int)angle + 180)%360;
      speed = -speed;
}

Thanks in advance for any help.

Originally Posted by kylelanman

That code looks familiar

There is one addition on the end that is likely causing you problems.

&& angle != 0

In general everything works. If your commanded angle happens to be 0 then regardless of what your current angle is you skip the optimization. In the case of being optimized at 180 and then getting a 0 angle no rotation is required but due to the additional check it thinks a 180 degree rotation is required so it spins 180.

What problem were you trying to solve by adding the additional check?

I agree this appears to be the issue. Did you perhaps mean to specify speed!=0, so deal with the case where it just doesn't matter ?