Autonomously Following Cubic Polynomial

Technical Programming
, , , ,
#1

I have successfully generated a polynomial based off of two slopes and two points with thanks to @Ether and this thread.

However, this does not get me as far as I’d like it to. For this to be complete, I need to steer along this path. There are several ways I could do this, and most involve taking the slope of a point along the path, and then adjusting my robot’s heading. In order to get this slope, I need an X value however.

Or do I? Is there any better way to follow this polynomial? I really like the polynomial and would appreciate it if anyone had ideas.

I am currently looking at my navX’s x-displacement, but I hear it is inaccurate, so I’d like to avoid it. Along with that, perhaps some calculus would allow me to find the slope with an arc length based off of my encoders?

And lastly, how accurate is the Pigeon IMU in terms of displacement? Maybe I should look at replacing my navX with this.

Thank you!

#2

Nowadays, there’s support built into WPILib for trajectory generation and tracking. We support clamped cubic splines and quintic splines as well as enforcement of arbitrary user-defined trajectory constraints. I suggest reading the following resources.

Introduces trajectory generation:
https://docs.wpilib.org/en/stable/docs/software/advanced-control/trajectories/trajectory-generation.html

Talks about how our trajectory generation works internally (optional, but in case you’re curious):
https://pietroglyph.github.io/trajectory-presentation/#/

Walks you through implementing trajectory generation/following on an actual robot:
https://docs.wpilib.org/en/stable/docs/software/examples-tutorials/trajectory-tutorial/index.html

6 Likes
#3

It is not a good idea to use your IMU to figure out your displacement along the field. You should use a combination of your encoders and gyro in order to do this.

Conveniently, WPILib has several odometry classes that utilize your encoder and gyro data to calculate your robot’s absolute position on the field. With the information about where you currently are on the field, and where you want to be, you can accurately follow a trajectory.

With regards to pure angular displacement information from the NavX and the Pigeon IMU, I believe they use the same internal sensor so your values should not be too different (if at all). We haven’t had issues with either.

1 Like
#4

We used a similar idea to what I think you’re getting towards in 2018. The team has continued using it and you can find the code here. There is a whitepaper available here.

Effectively what you will want to do is calculate the angle between your current heading and the slope of the tangent line at your current point in the polynomial. If your polynomial is defined on the domain [0, 1], and you have calculated the arc length of the polynomial in advance (we used this JS library), then you can find your desired tangent line by dividing your current distance traveled (average of your 2 encoders) by the total length of the polynomial. You’ll then use a simple P controller to constantly “chase” the next desired angle.

This approach to auto is okay, and I think it’s one of the simpler ways to get it done; it’s relatively consistent, however it isn’t perfect and you’ll have to spend some time getting the autos perfect.

However, if you are looking for long-term auto success, I recommend checking out the solutions currently provided by WPILib as mentioned by Tyler and Prateek above. They will be more accurate and better supported by the community in the long term.

1 Like
#5

The Pigeon and original NavX both use the same MPU-9250 IMU and will perform very similarly.

The new NavX2-micro uses a newer IMU with better performance. You can read about it and determine if the improved displacement accuracy meets your needs: https://pdocs.kauailabs.com/navx-micro/intro/frequently-asked-questions/

#6

In theory, would the domain have to be [0,1], or could we take that percentage and simply multiply it by our upper limit x value instead?

#7

You could probably multiple by your upper limit x value, yeah.

#8

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.