Angle subtraction code

Angles.java

// This only works if both of these variables are in the domain of [-180,180]
double currentHeading=90;
double desiredHeading=49;
// Do the subtraction
double error=currentHeading-desiredHeading;

// If the error is under -180, it is inefficient. So, add a full turn to that number. That way something like -190 becomes 170. The magnitude is smaller but if you plot that angle, it is the same angle.
if (error<-180)
 error+=360;
// Same logic, but different signs.
else if (error>180)
 error-=360;

SmartDashboard.putNumber("Angle error", angle);