Quote:
Originally Posted by dougwilliams
We take an easy piecewise linear approach ... If it needs to rotate more than 90 degrees it can be set to rotate at full speed. When it has 45 degrees left to rotate it needs to be scaled down to .4 speed. Once you are within 20 degrees the rotational speed needs to be scaled down to only .2
|
Yes an approach like that can be quite effective in many situations.
Quote:
Also - looking at your code - do the angles work in that scenario? What angle are you trying to rotate to? Maybe to zero - but then when you are less than -140 degrees it stops. That seems backwards to me.
And I think in the NavX 180 and -180 are the same position, and you need t account for that in your math.
|
The gyro_reading and your desired_angle need to share the same zero position, and rotation direction. Given that, it's simple to compute the shortest angle to your destination:
Code:
angle_error = desired_angle - gyro_reading;
shortest_angle = angle_error - 360*floor(0.5+angle_error/360);
If your compiler supports the REMAINDER function "x REM y" per IEC 60559 as specified on Page 235 Section 7.12.10.2 of ISO/IEC 9899:TC3 (which java does I believe), then the following single line of code should work:
Code:
shortest_angle = Math.IEEEremainder(desired_angle-gyro_reading,360.0);
The above works regardless of the range of either the desired_angle or the gyro_reading (as long as they share the same zero and rotation direction, as mentioned above).
So for example if your desired_angle is -3 degrees and the gyro is reading 359 degrees, the shortest angle will be -2 degrees.