NavX MXP Continuous Angle to Calculate Derivative
 

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!

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

Can you please ensure that you are using the latest NavX-MXP libraries? There was a change checked in on Feb 20 of this year that fixed a bug in the getAngle() code. This should be returning a value that is not constrained to 0-360 degrees. Since you've indicated you are seeing a range of 0-360, I'm suspicious somethings out of date.

Also note there's a recent firmware release which increases the update rate maximum to 100Hz, so it might be worth downloading the latest release, running the setup to update the libraries, and updating the firmware.

Please feel free to contact support@kauailabs.com if you still have trouble.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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:C# and Java both support IEEERemainder.


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

EDIT:

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

Re: NavX MXP Continuous Angle to Calculate Derivative
 

We're not running it closed loop. We've implemented a system described in the paper here. Essentially assuming all variables stay the same, powering motors at the same speed for a certain number of 'pulses'/time should send it to very similar locations each time. We used this at the Oklahoma regional but found that using one certain voltage to run the motors was too prone to the variables in our robot, like battery voltage, tire pressure and internal drivetrain friction. To fix this we implemented a system that starts powering the drivetrain at a small power and incrementally steps up the power until the robot begins to move at the speed we want it to. We then use this found speed to run our 'loops cycles'. By doing this our inconsistency problems have been mostly solved (at the Oklahoma regional we missed 3/4 of the last 4 auto shots because our battery voltages had become so consistently high).

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

I haven't updated the NavX libraries yet, as we leave for Missouri State Champs at 3:00 tomorrow and we're in a time crunch. Ether's fix seemed the quickest and easiest.

Thank you everyone for all the help! I can post source code for our autoalignment command and the derivative calculator in a few days if anyone is interested.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

This year on 254 we switched to using a simple Rotation2d class for all angles (because, among other reasons, dealing with angle rollover as you did here is easy to screw up). Internally, this class stores the sine and cosine of an angle explicitly. This has some nice properties:

• We use static utility methods to create a Rotation2d from a vector (x,y -> cos,sin after normalization), absolute angle in radians, or absolute angle in degrees.
• We have accessors that return the absolute angle in degrees or radians (wrapped to -Pi to Pi).
• We have "Inverse()" and "RotateBy(Rotation2d other)" methods for creating a new Rotation2d:
  
  Code:

  ---

  Rotation2d Inverse() {
  return new Rotation2d(cos_angle, -sin_angle);
}

Rotation2d RotateBy(Rotation2d other) {
  return new Rotation2d(cos_angle * other.cos_angle - sin_angle * other.sin_angle,
                cos_angle * other.sin_angle + sin_angle * other.cos_angle);
}

  ---

• If you want to know the shortest rotation from A to B, you can do something like:
  
  Code:

  ---

  double shortest_distance_degrees = A.Inverse().RotateBy(B).ToDegrees();

  ---

  This will always give you a solution on [-180, 180].
• Cheap access to the sine, cosine, and tangent (sin/cos) of the angle because they are already cached.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

Thanks Ether... learn something new everyday!

The way Microsoft's library present's this function is like so:
IEEERemainder = dividend - (divisor * Math.Round(dividend / divisor))

It's great to see such a compatible use-case for this function as written in this thread... this function has also helped improve some bugs in my simulations as well. :)

Re: NavX MXP Continuous Angle to Calculate Derivative
 

Here is a c++ equivalent within standard libraries:
http://en.cppreference.com/w/cpp/numeric/math/remainder

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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?

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

We did something similar this year. We ended up running a rotational velocity P controller with feedforward as the baseline of our turn controller. We summed the output without the feedforward, and limited to +-20% voltage to help prevent windup. A PD controller on turning angle was the input to the velocity controller.

There was definitely a noticeable improvement on the conventional PID controller. One problem we were facing without the velocity controller was that our turns were highly dependent on momentum. We really would have liked to run the velocity loop underneath our teleop driving code too, but integral windup caused a lot of unpredictable behavior over long periods of time.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

The Endnote LV code you reference is functionally equivalent to shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360); provided here ...
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?

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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

Ok. The OP's framing of the problem:

Proposed solution:

Proposed solution amendment:

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

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:

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.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

I am discussing the solution to this aspect of the OP's original framing of the problem (see bolded portion):
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:

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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.

Re: NavX MXP Continuous Angle to Calculate Derivative
 

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.