TurnRobotRelative Never finishes

[dcowden]

When alliance is RED, all works well
But when alliance is BLUE, turnrobotrelative doesnt ever finish.

The issue appears to be a bug in which the requested angle has not been wrapped around, as shown in the image below, produced by this code:

    	String msg = String.format("TurnRobotRelative:Yaw=%.2f, waiting for %.2f", yawSupplier.getYawAngleDegrees(),angleToWait);
    	DriverStation.reportWarning(msg , false);

[dcowden]

Note: the command physically finishes, but then it never actually signals finished. I put a timer around it to finish the command after a timeout, and that works around the problem.

We seem to have other moves randomly not finishing too

[dcowden]

I have tried a fix for this: normalizing the angle before waiting for it:

angleToWait = EntechUtils.normalizeAngle( yawSupplier.getYawAngleDegrees() + robotRelativeAngle );