I also found some inconsistencies in MecanumDrive and KilloughDrive and fixed
them.
Drive now uses the NED axes convention. Therefore, the positive X axis points
ahead, the positive Y axis points to the right, and the positive Z axis points
down.
Translation in X assumes forward is positive. Translation in Y assumes right is
positive. Rotation rate assumes clockwise is positive. Angles are measured
clockwise from the positive X axis.
Based on the angle origin convention, DrivePolar() for both Mecanum and Killough
needed the normalization removed, sine used to compute the Y component, and
cosine used to compute the X component.
The vector rotation done in DriveCartesian() needs to rotate by a negative angle
instead of positive to undo the robot's rotation. RobotDrive assumed a clockwise
angle and sensors returned counter-clockwise angles, which is why it used a
positive angle for rotation.
