NavX MXP Continuous Angle to Calculate Derivative

[lethc]

For our autoalignment sequence, we have an algorithm to 'find' the correct motor speed necessary to turn the robot at a certain velocity. The algorithm bumps up motor speed every 100ms until the robot is turning at a certain very slow velocity. The NavX's getRate() function had very low resolution so we wrote a class to calculate the average change in yaw over time (derivative). However, for this to work at all headings we'd need a method on the NavX to return a continuous angle. Currently both getAngle() and getYaw() are not continuous, and their ranges are [0, 360] and [-180, 180] respectively, so at the point where the robot crosses the threshold of a range the values jump ~360 degrees which makes the robot think that it just turned the full 360 degrees in a matter of milliseconds.

Does anyone have any ideas on how I would get a continuous angle heading from the NavX? I.E. one that doesn't jump from 360 to 0 instantly, because that messes up the derivative calculation.

To get around this issue currently, we're checking if the derivative is less than something like 30 deg/s, and if it isn't just return 0 deg/s, but that's really hacky and I'd like to actually fix the problem.

My team is using Java. Thanks for your help!

[virtuald]

I think that was a software bug... have you updated to the latest software?

[JamesTerm]

Would x=x%180.... (modulo operator) work? May need to preserve sign.

[Ether]

Originally Posted by dougwilliams

You should be able to use some modulo arithmetic operations to solve the jump from 0 / 360.

Originally Posted by JamesTerm

Would x=x%180.... (modulo operator) work?

modulo is not what you want here.

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

C# and Java both support IEEERemainder.


If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

EDIT:

Test code:

#include <math.h>
#include <stdio.h>

double shortest_angle(double previous, double present){
	return (present-previous) - 360.0*floor(0.5+(present-previous)/360.0);}

void test(double previous, double present){
	printf("previous=%f\tpresent=%f\tshortest_angle=%f\n",previous,present,shortest_angle(previous,present));
	}

void main(void){
	test(360,0);
	test(0,360);
	test(360,1);
	test(1,360);
	test(359,0);
	test(0,359);
	test(1,359);
	test(359,1);
	test(359,-1);
	test(-1,359);
	test(3*360+1,-1);
	test(-1,3*360+1);
	}

Output:

previous=360.000000     present=0.000000        shortest_angle=0.000000
previous=0.000000       present=360.000000      shortest_angle=0.000000
previous=360.000000     present=1.000000        shortest_angle=1.000000
previous=1.000000       present=360.000000      shortest_angle=-1.000000
previous=359.000000     present=0.000000        shortest_angle=1.000000
previous=0.000000       present=359.000000      shortest_angle=-1.000000
previous=1.000000       present=359.000000      shortest_angle=-2.000000
previous=359.000000     present=1.000000        shortest_angle=2.000000
previous=359.000000     present=-1.000000       shortest_angle=0.000000
previous=-1.000000      present=359.000000      shortest_angle=0.000000
previous=1081.000000    present=-1.000000       shortest_angle=-2.000000
previous=-1.000000      present=1081.000000     shortest_angle=2.000000

¹ REMAINDER function "x REM y" per IEC 60559 as specified on Page 235 Section 7.12.10.2 of ISO/IEC 9899:TC3

[Richard100]

Originally Posted by Ether

modulo is not what you want here.

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

C# and Java both support IEEERemainder.


If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

Some practical considerations regarding this approach ...

Note that if one is streaming readings from an angular position sensor (such as a gyro or IMU), one would need to add an accumulator to this shortest_angle function to continually maintain angular position.

Also be aware that the function is sensitive to sampling rate. This is because a late sample might allow a current position (the 'present' variable) to get more than 180 degrees away the previous position (the 'previous' variable). If this happens then the shortest angle will switch to the other side of the cycle. If you've ever noticed movies where wagon or car wheels appear to be moving backwards then you've seen this aliasing effect (this can happen to your sensor, too).

To avoid this, ensure the iteration period is at least shorter than (1 / 2*RPS), where RPS is the maximum revolutions per second expected from the sensor reading stream. For example, if your robot maximum yaw rotation rate is 1000 deg/sec, you must iterate the function no slower than every 180 ms ... (someone check my math).

This is likely not a problem for most FRC robots, as yaw rates don't normally get that high (ours don't get much higher than 500 deg/sec), and computational iteration rates are probably much faster - but one should be aware of the constraint. You might try the function on a robot mechanism that rotates much faster and wonder why your code is suddenly unreliable.

If the sensor readings are noisy, this will further erode the sampling margin. If the peak noise level in degrees is 'n', then the sampling constraint becomes:

Loop Period < (180 - 2*n)/(360 * RPS)

Of course, excessive noise should be remedied (fixing the root cause). One should just keep in mind that it's good practice to maintain healthy margin for practical issues like noise, and in fact whether your loop can always be trusted to occur on schedule.

The exploit of the shortest directional path property of the IEEERemainder function is quite clever for dealing with these orientation discontinuities. I wasn't aware of it until Ether pointed it out - (thank you Ether). It doesn't appear that LabVIEW has this function (although easy enough to build from primitives) ... unless I'm just not finding it - anyone know for sure?

[Ether]

Originally Posted by Richard100

The exploit of the shortest directional path property of the IEEERemainder function is quite clever for dealing with these orientation discontinuities. I wasn't aware of it until Ether pointed it out - (thank you Ether). It doesn't appear that LabVIEW has this function (although easy enough to build from primitives) ... unless I'm just not finding it - anyone know for sure?

Look at the Endnote at the bottom of the last page.

[Richard100]

Originally Posted by Ether

Look at the Endnote at the bottom of the last page.

The Endnote LV code you reference is functionally equivalent to shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360); provided here ...

Originally Posted by Ether

If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

Both can be written in LV by building up the function from other primitive programming functions (as you depict in the endnote image).

What I wanted to know was whether LabVIEW has IEEERemainder as a programming function (call it Rem). There is a Mod (%) function (known in LV as Quotient & Remainder), but of course Rem and Mod differ in how they internally round, which is what makes Rem useful here.

Same question as you asked here, only for the LabVIEW language. Equivalent to asking: Can it be done in LabVIEW "... with one line of code".

Any LabVIEW experts know? If not, might this be in the development pipeline?

[Ether]

Originally Posted by Richard100

Note that if one is streaming readings from an angular position sensor (such as a gyro or IMU), one would need to add an accumulator to this shortest_angle function to continually maintain angular position.

It's not clear what you're saying here.

You don't need an accumulator when using a gyro if all you care about is your heading.

The desired heading and the gyro angle do not have a range constraint in this case.

For example, the gyro angle could be 721 degrees and the desired heading could be -1 degree. The function would return -2 degrees as the shortest angle, which is the value you want.

[Richard100]

Originally Posted by Ether

You don't need an accumulator when using a gyro if all you care about is your heading.

Not finding this to be very clear either. We're likely discussing different aspects of the problem.

Originally Posted by Ether

It's not clear what you're saying here.

Ok. The OP's framing of the problem:

Originally Posted by lethc

Currently both getAngle() and getYaw() are not continuous, and their ranges are [0, 360] and [-180, 180] respectively, so at the point where the robot crosses the threshold of a range the values jump ~360 degrees ...

Does anyone have any ideas on how I would get a continuous angle heading ...?

Proposed solution:

Originally Posted by Ether

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

C# and Java both support IEEERemainder.


If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

Proposed solution amendment:

Originally Posted by Richard100

Note that if one is streaming readings from an angular position sensor (such as a gyro or IMU), one would need to add an accumulator to this shortest_angle function to continually maintain angular position.

If the OP streamed a series of sensor readings, range constrained as [0,360], into a function

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360)

the resulting output would need to be accumulated to maintain position. Say robot yaw initializes at 0 degrees, moves clockwise over time to 15 degrees, then counter-clockwise to 350 degrees, as reported by the range constrained sensor. Consider:

Iteration       Previous        Present         Pres-Prev       Function       Accum
1		0		0	        0               0              0
2	        0               5	        5               5	       5
3	        5	        10	        5	        5	       10
4	        10	        15	        5	        5	       15
5	        15	        10	       -5	       -5	       10
6	        10	        350	        340	       -20	      -10

Rather than using what the OP started with, a Present value exhibiting the range-constrained discontinuity, it sounds like the OP needs the accumulated function result.

I'm interpreting the OPs desire for a function that converts his range-constrained sensor output to a non-range-constrained one. Your IEEERemainder function, with an accumulator, accomplishes this.

[Ether]

Originally Posted by Richard100

Not finding this to be very clear either. We're likely discussing different aspects of the problem.

I am discussing the solution to this aspect of the OP's original framing of the problem (see bolded portion):

Originally Posted by lethc

Does anyone have any ideas on how I would get a continuous angle heading from the NavX? I.E. one that doesn't jump from 360 to 0 instantly, because that messes up the derivative calculation

You don't need a continuous angle to do a simple difference calculation (which is what I anticipated the OP was really wanting).

The IEEERemainder gives you that answer:

Originally Posted by Ether

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

Originally Posted by lethc

Just tried this out and it works perfectly. Thank you for your help.

[Richard100]

Thanks for taking time to walk through this! The distinction you reference clears up the differing perspectives / solution space.

[Alan Anderson]

Originally Posted by Richard100

Same question as you asked here, only for the LabVIEW language. Equivalent to asking: Can it be done in LabVIEW "... with one line of code".

Any LabVIEW experts know? If not, might this be in the development pipeline?

You can do a "line of code" in LabVIEW by putting it in a Formula Node. It's exactly like writing an expression in C.

[tr6scott]

Labview Subvi implementation of this attached.
Tested with the data set published, and checked results.
Sorry, not a LV guru, so did not take the time create input matrix and results.
(started too, but I knew there was a much simpler way than the way I was doing it.)

Thanks, we struggled with the gyro rollover in the past and this implementation seems to solve all of the issues we have tried to conditionally fix in the past.

Thanks Ether.

[dougwilliams]

Originally Posted by lethc

For our autoalignment sequence, we have an algorithm to 'find' the correct motor speed necessary to turn the robot at a certain velocity....
!

You should be able to use some modulo arithmetic operations to solve the jump from 0 / 360. I'm not sure how you have it configured but on our we have the issue because it jumps from -180 / 180.

Question though - what are you trying to achieve by closing the loop to a specific turning speed? We use the same setup and close the loop on the current field orientation angle. In that way you can have the robot quickly turn when it's farther away from the set point, and slow down as it approaches where you want it to be so you have minimal overshoot.

[notmattlythgoe]

Originally Posted by dougwilliams

You should be able to use some modulo arithmetic operations to solve the jump from 0 / 360. I'm not sure how you have it configured but on our we have the issue because it jumps from -180 / 180.

Question though - what are you trying to achieve by closing the loop to a specific turning speed? We use the same setup and close the loop on the current field orientation angle. In that way you can have the robot quickly turn when it's farther away from the set point, and slow down as it approaches where you want it to be so you have minimal overshoot.

One thing we've been trying is using a closed loop to control the rate at which the robot turns and then use a P controller to tell the first controller how fast to turn based on how far away from our target angle we are.