I’m curious how teams use a gyro if they are using motion control. For instance, we use Pid for straight line driving and integrate the gyro with another Pid to control heading. It seems that motion profiles handle the driving straight themselves without the need for a gyro.
Because of wheel scrub, it seems like a gyro would be a more accurate method of turning.
So what is the strategy to combine the gyro?
It depends on how you are implementing it. I can explain what we did last year, which used both encoders and gyro with motion profiling.
This is somewhat true, but not entirely. If you use motion profiling, and track your position goals perfectly, you’ll end up driving straight. However, if you aren’t tracking your goals perfectly, you’ll end up with some drift. We also like to be able to use the same code for point turning, driving straight, and arc turns (point turning and driving straight are just special cases of arc turns in our code).
Yeah, this is correct. This is the main benefit to using a gyro.
We generate a motion profile for distance and angle. Then we calculate feed-forward voltage to apply based on those goals. (I can go into this more if you’d like - basically, you calculate the voltage that you’d need to apply to get to the goal that you’re sending, and then use that as the value that you’re doing PID on top of.)
Doing that gives you (with no sensor feedback) a set of voltages that should (theoretically) have you exactly track your goal.
We then calculate a position error (based on the average values of the two encoders, usually) and a angular error (from the gyro), and run PID on those errors. We then add the output of those PID loops to the feed forward voltages that we found.
Basically
V_left = V_ffleft + PID(distance_error) + PID(angle_error)
V_right = V_ffright + PID(distance_error) - PID(angle_error)
You can see our drivetrain code here, although I’ll warn you that it’s really ugly… (and also has a few bugs in it that we didn’t get a chance to fix) It does work though.
Feel free to ask if you have any questions - this was just a pretty quick overview that glossed over a lot of stuff.
From the past two years, the path planner library we’ve used outputs both wheel speeds, and an expected heading at every time stamp.
We’ve used a simple P controller to convert the heading error into a wheel velocity correction factor. Simple, but worked reasonably well.
Note there are issues with this strategy if you get way off - large wheel velocity “corrections” will violate the assumptions of the path planner, and will throw off your path. The gyro only served some fairly simple cases:
I would advise against this approach, as you will end up with the error terms (distance and angle) fighting each other.
That is, if (due to wheel scrub or uneven weight distribution or whathaveyou) your distances are not correct for achieving the desired angle, any adjustments made by your angle term will necessarily cause some amount of resulting position error. You will therefore end up at whatever position causes the two terms to “balance,” so to speak. This is suboptimal.
If you want to do this properly, you have to interpret the output of your angle error loop as a corrective velocity differential, and incorporate it by adjusting the feedforward term and distance setpoints accordingly.
place bad reply here
I hate gyros because they have given be hardware fails often in the past. Also, as @wesleyac said, the main benefit is the turning and the heading using the gyro. I personally never have used it with a PID or in a-ton but have heard and seen it done before.
Personally i recommend just keep refining your PID loop as it’s industry standard.
thank you for reading this bad response
Why would you give advice that you knowingly qualify as being bad?
It is exceedingly easy to implement a very reliable gyro in FRC. Just buy a NavX.
Thanks - the terms fighting was kind of what I had stuck in the back of my head. Our current PID uses the average of the encoders as distance, so correcting for the gyro doesn’t cause them to ‘fight’, although you have to turn off the drive PID when you are turning in place with the gyro PiD or they will fight.
Another question:
It sounds like there are a couple of correct options:
What about if you’re using the Talon SRX motion profile system? There doesn’t seem to be any way other than calculating points as you go and modifying them with the gyro error. We admittedly haven’t tried using the SRX’s in motion profile mode, but I’m interested in trying it next year.
You can modify the profiles to incorporate the gyro feedback without recalculating them, with a bit of simple math - you need to add the instantaneous velocity correction to the velocity profile for each side, and further add the integral of all past velocity corrections to the position profile for each side.
This, of course, requires you to stream the profiles to the talon in real-time as the profile executes, rather than send it all at the beginning.
Re-calculating the profiles at each timestep is also possible, but would require a very fast co-processor.
254 is doing this at 100+ Hz on the RoboRIO in Java. It’s definitely doable, but requires careful implementation.
If you’d like to share details, we’d be extremely grateful for them. We’ve identified this as the natural “next step” for our team’s autonomous capabilities, and will be working on it this offseason.
Edit: Also, any tips for dealing with time delays between various sensors? I’ve yet to actually test what sort of lag there is on the NavX (if any) or on our vision processing, but I imagine it may well be non-negligible and that could certainly cause problems.
Yeah - after a lot of work at minimizing CPU usage (early numbers had us at 98%) we’re down to 70% or so last time I checked. We should have the spare horsepower to do it in Auto. Pushing off all the PiD info onto the SRX’s should improve that even more, since we’re doing all our drive PiD’s in code.
Define a reference path in a field coordinate frame. The path can take many forms, but it should at the very least be continuous and provide a “closest point on path” function. Your path could be as simple as a series of line segments, or as complex as a spline of arbitrary order. You can optionally include a speed limit for each point or segment.
Use a kinematic model of your robot along with encoder and/or gyro measurements to track your robot’s location in the field coordinate frame over time.
At each time step, choose a point on the reference path to aim for. We look a fixed distance ahead of the closest path point to the robot’s current position (a pure pursuit controller). The choice of fixed distance trades off between tight tracking (with potential oscillation) vs. smoothness (but corner cutting). There are other ways to do this, though.
Generate a path that joins the current robot position and orientation to the point you just chose. We use an arc of constant curvature (so we arrive at the goal point, but not necessarily the goal orientation; the right heading happens because of how the arc changes over time), but you could also fit a spline or other sort of curve to match headings.
Take the path you just generated and “flatten” it into a 1D trajectory so you can time-parameterize it with a motion profile. The initial state is at t=now, x=average of left and right encoder distance, and v=current robot velocity. The final state is at t=?, x=start + length of the path, v=min(max reachable velocity from current state, speed limit at this point). Your profile generator will solve for t and intermediate velocities.
You now have a time-parameterized trajectory for the center of the robot. You can solve for left/right wheel velocities geometrically.
Follow the trajectory for a little while, then go back to 3 and repeat.
I’m having a bit of trouble seeing why this is. Are you referring to the case where you’re doing an arc turn and the profile for one of the components (angular or distance) doesn’t sync up with the other component? The way that I’ve seen to deal with this is to slow down one of the profiles such that following both the angle and the distance are linear to each other. I haven’t actually run that on a robot before though (in 2016 we basically only used straight drives and point turns), so I’m not sure what the behaviour is like if you aren’t tracking the profiles well. The math all works out though…
Thanks for the explanation.
Ah, so if I understand you right, the idea is that rather than having pathfinder (or whatever you’re using) re-generate a full profile at each time step, you instead just generate a partial profile up to a point on the reference path.
The trickiest part, I think, seems like it will be developing a model for determining robot position. Would something as simple as “assuming the center of the robot is moving at a linear speed given by the average of the two sides of the drive, in the direction indicated by the gyro” and then integrating work?
Imagine that you are running a motion profile using the approach described (velocity feedforward plus position servo plus angle servo). Further, for the sake of the thought experiment, let’s imagine that after driving some distance along a path, the robot is currently at rest and the position error is currently near-zero for each side of the drive, but the angular error is nonzero due to wheel scrub (which the encoders, note, cannot see).
Our angle term will then try to adjust for the angular error. So, the angle term will cause a difference in wheel velocity that attempts to correct for the angular error. In doing this, the wheels will have moved, and so the positional error is now nonzero! So, the position term will try to eliminate the adjustments made by your angle term, because the position setpoint “doesn’t know” that the wheels have scrubbed and that the theoretical wheel positions are no longer the correct ones for achieving the desired angle. To put it another way, from the point of view of the distance term, the adjustments made by the angle term are errors that need to be eliminated - and from the point of view of the angle term, the position term’s attempt to keep the wheels at the “correct” position is, in turn, causing angular errors that need to be eliminated.
The result is that the terms (position and angle) will “fight” each other, resulting either in oscillations or in a steady-state solution where the two terms cancel.
Does this make sense?
Ah, I see. The situation that you’re describing makes sense, however, that didn’t happen with our code, since we take the average of the two encoders and use that as the distance. We aren’t doing separate distance compensation on the two sides.
In any case, it sounds like Tom Line is using SRX’s - in that case using the approach that Jared suggested is likely a better option. The onboard 1kHz PID is quite nice 
So what makes generating these profiles fast enough to be done on the RIO? We’re currently using Jaci’s Pathfinder and even on a regular PC it takes a few seconds to generate the profiles, and it took ~30 seconds on the RIO.
Pathfinder is generating quintic splines. The most expensive part of that process is arc-length parameterizing the splines so that you can time parameterize them, which involves a numerical integral.
Conversely, just generating an arc to connect the robot’s (x, y, theta) to a goal (x, y) is a really cheap and straightforward geometry function, and there are analytical solutions for arc-length parameterization.
Huh, small world. We used the exact same system for motion profiling (feed forward, position loop, gyro loop)! For some of our autos though, especially the simpler ones, we found it easier to just cut the feed forward out and just use position and gyro control.