Advice for precise autonomous routines?

Hello all,

Something I have struggled with for ages (though I have gotten better at) is getting my autonomous routines to be precise and consistent. Mostly, what is the best way to accurately drive a specific distance? I use a passive tracking wheel with a quadrature encoder for straight-line travel and a gyroscope for turning. Both use PID loops. But, I generally find that I can’t get the best mix of speed and accuracy with PID (mostly with straight-line driving; the turning is okay). Does anyone have any better methods? My biggest problem is that when my robot starts forward in the beginning of autonomous, it jerks to the right. We have tensioned our drive chains and replaced the plastic inserts on our omni wheels with metal inserts in order to reduce slop as much as possible (so much so that if I turn one wheel, it only goes about 10 degrees before the other wheel starts turning, too). I think the biggest issue, though, is that a PID loop with a long way to go generally just starts at full power, so the robot tends to jerk. The only way to avoid this is to make it rely way too much on the integral term. Would I be better off making some custom control scheme, so that the drive starts out slower and speeds up? Or is there some way to tune the PID so that this is not an issue?

It’s also worth noting that our robot has no noticeable curving when driving straight in opcontrol, or once it has accelerated.

Any help would be appreciated!

Accelerate it slowly at first (manually with 20ms increments), and we put quad encoders on both sides along with a gyro to double check everything.

Accelerating more slowly can help with the jerking at the start. Also for more consistency implement something in the drive straight function that uses the gyro to keep a consistent heading. This can speed up your turns also, since they don’t need to be completely accurate.

1 Like

What you need is a full motion planning. You want to ramp up the motor power (I intentionally didn’t use the term “speed”) slowly at start, then start ramping it down soon enough before your target. If you just say, “Hey, PID, my target is that faaaar, get there!”, your PID will get crazy. But if you use a speed PID instead and prepare a speed profile for your path (ramp up, keep the speed, ramp down, stop), the robot will glide smoothly. You can do a nested controller, with the outer one setting up the speed based on the part of the path travelled and the inner one doing full PID for speed.

But what we’ve got really good results with so far (I have middle schoolers) is a kind of P within P controller,
where the outer does the speed ramp, while the inner uses a gyro to steer the power between the two drivebase sides. Because they have empirically hardcoded the ramps, it can’t go less than 1 feet, but for any longer travel, it is precise to 1".

That! Our test routine is a “square dance” (run around a 2’ square) and while the turns aren’t as precise, the next straight segments quickly compensate for that.
Edit: spelling.

1 Like

Thanks for all the answers! I will definitely look into having velocity PID so the power ramps up more slowly. Controlling angles on straight line driving is also something I had considered, so I should try implementing that.

My question with doing velocity PID though, is: how do I figure out what my velocity targets should be based on the distance I have to go? A little bit of calculus and some testing? Or is the target velocity just going to be proportional to the distance I have left to go, roughly speaking?

You could start by doing a simple P controller. The target velocity would just be proportional to the distance left.

(Apparently, while I was editing my post @nenik already said most of what I was going to post :slight_smile:

@Aponthis you may want to take a look at a motion planning library, for example, https://vexforum.com/t/qcc2-gyro-and-pid-control-code/29292/1

Your observations are correct: PID is only guaranteed to work around small linear region. Over the long distances, especially when PID values get saturated and you start clipping max power, it is no longer mathematically consistent. What you have in those cases is not a proper PID and textbook tuning methods are no longer applicable.

Here is a simple way to think about motion planning for starters:

Instead of targeting final encoder position, target robot velocity and left-right encoder diff over the long distances. You will still need a separate PID loop for each (maintaining desired velocity and encoders diff between left and right), but they will be much simpler to handle.

I thought I’ve seen recently a sample code posted by @tabor473, but I couldn’t find it for some reason.

As you get close to your target position you switch from velocity targeting to position PID to precisely stop.

Also, you would need to do some smoothing to get good velocity estimates, or your inputs will be noisy with round off errors and similar stuff.

The simplest way to try motion planning, even before you get up to speed with the library, would be to create a function that takes target position and desired velocity (values for which you determine experimentally).

Then inside that function you first start gradually increasing motor power using some fixed slew rate until you get close to the target velocity.

Then you start maintaining the desired velocity for a while using its own PID loop (with separately tuned coefficients) until you get into vicinity of the target position.

Once you close to the target position, you switch to the next step of regulating position (instead of velocity) with a next PID loop having its own coefficients.

When you arrive within some small error of the target position you exit from that function, ready to do the next step.

Try to code the above function first, mostly for educational purposes. The next step would be to add mini power corrections for driving straight to both velocity and position targeting segments. There were a number of threads that popped up on the forums regarding this topic lately, with some really good tips and tricks for doing that.

The goal of this exercise is to get to the point when you understand what the library is doing. At that point you could either integrate the whole library into your code or just include the relevant portions of the code.

Finally, to take precision driving to the next level, you would have to switch from the individual sensors used as feedback input to your PID loops, to the sensor fusion model.

In sensor fusion model you take input from all sensors at once and process through some math model, for example, using Kalman filter to estimate some parameters of the system like “true” position and velocity. The math behind the model does not necessarily 100% believes each individual sensor at each point of time. Instead, it believes each of them less than 100% and “averages” various sensors over short period of time thus improving overall accuracy by smoothing over noise in the individual input measurements.

I’ve seen a library from one of the VEXU teams (BNS?) posted a while ago. When I have time later today I will try to find those links and post them here, but if somebody else knows those links and could do that first, it would be great.

1 Like

You form a trapezoid. On ramp up, you can freely pick reasonable acceleration to not overstress your motors and avoid jerk. On ramp down, the theory would be to use constant deceleration, but the robot inertia and aggregate torque of your motors limit the slope - If you’re too aggresive, the velocity PID would just quickly saturate at -127 motor power. So, pick a number, get a datalog and see what did your vPID done to your motors. Tune the slope, rinse and repeat.

(and I see that @technik3k posted a much more advanced and thorough answer, thanks!)

1 Like

Also, it helps to put a brake/delay after ever movement so that extra momentum is eliminated.

Yes definitely do this

Thanks for the advice, everyone. I am now using velocity PID when I am far from my target and adjusting the angle of the robot using the gyroscope while driving straight, and my confidence in my auton consistency has soared. Here’s a video (only had time for 5pt zone so far, and it could be speed up). Notice the robot drives straight at the beginning. Also, it gets angled when it hits the mobile goal, but it adjusts on the way back! https://youtu.be/wiDBLqDYxSM

Are you only using only one encoder on the base? I’m curious about what other people do for straight drive.

I have 2 encoders, one one on each side if the base, and i use sync PID to try and make each sides encoder values as equal as possible.

I was wondering if it was better to use a gyro to straight drive, as it seems better if it’s used well.

A gyro would be useful for maintaining heading and keeping the orientation.