SwerveDriveWheel set about swerve HOT 2 OPEN

We use 0->360 rather than -180->180 (easier to read with the CANcoder absolute position always returning 0->360), so our setpoints are always 0-360. This may explain the confusion surrounding our code.

We had a piece of code that we thought worked, but for some reason did not work:

double angle = (setpoint - current_angle) % 360;
if (angle % 180 > 90) {
     output = -output;
}
if (angle < 90 || angle > 270) {
    speed = -speed;
}
double setpointAngle = (setpoint % 360) - (current_angle % 360); //the %360 for some of these really should not do anything, as setpoint and current_angle are always positive.
double setpointFAngle = ((setpoint+180)%360) - (current_angle % 360);


if (Math.abs(setpointAngle) > 180) {
     setpointAngle = -(Math.signum(setpointAngle) * 360.0) + setpointAngle;
}

if (Math.abs(setpointFAngle) > 180) {
     setpointFAngle = -(Math.signum(setpointFAngle) * 360.0) + setpointFAngle;
}
error = Math.abs(Math.min(setpointAngle, setpointFAngle));
if error > 90 {
     error = 180 - error;
}

However, when we put this in, it did not work as intended, so we have temporarily settled on this long if statement chain until we can figure out what is wrong.

Also, we were originally planning to use WPILib's PID Controller, but a bunch of weird things happened, so we used our own.
I am not sure where the weird function next to error exactly came from or who coded that part, but it seems to do the job when we test our robot. I will look into modifying or removing it to make it more accurate and so it makes sense.

from swerve.

The only reason I used -180 to 180 is it makes the math work out nicer. The signs work out such that it tells you which way you're turning. (This allows you to just say if angle < 0 negate output) It really shouldn't matter which range you normalize to so long as you're consistent.

The snippet you attached there maybe runs into issues at the very first line. In java a % b has the same sign as a (as opposed to python which ensures a % b is positive). This can lead to weird errors when trying to normalize angles. If you want to normalize an angle to be between 0 and 360 in java you have to do ((a % 360) + 360) % 360 which I realize now I forgot when I wrote #8... woops

from swerve.