NavX MXP Continuous Angle to Calculate Derivative

[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.

[MichaelBick]

Originally Posted by notmattlythgoe

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.

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.

[lethc]

Originally Posted by dougwilliams

Question though - what are you trying to achieve by closing the loop to a specific turning speed?

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).

Originally Posted by Ether

The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

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.

[Jared Russell]

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:
  

  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:
  

  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.

[JamesTerm]

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.

[JamesTerm]

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