Do I need PID control for positional angle control?

Hello CD,
I am attempting to get the robot to turn to a specific angle and I have seen a lot of posts on CD about using PID control for turning to an angle, although it seems like I could just use a while loop to check the angle of the gyro while turning. Does anyone have any suggestions?

Thanks!!

So there’s two topics you’re hitting on at once here:

This is the difference between declarative and imperitive architectures. Or, in FRC parlance, TimedRobot vs. CommandBased.

This speaks to, within the chosen architecture, how to calculate motor commands from angle measurements and commands.

I think the gap is that you’re constructing the problem a notch or two simpler than a lot of folks here post about. That’s not a bad thing - simple solutions which are functional beat out complex solutions every day. But there is a gap if you’re looking to connect your current notion of the world with the posts you’re seeing.

To make a guess, I’m assuming when you say “while loop”, you’re implying something like this:

while(gyro.getAngle() < 45.0){
   leftMotor.set(1.0);
   rightMotor.set(-1.0);
}

leftMotor.set(0.0);
rightMotor.set(0.0);

This approach actually is a PID controller. It just makes a bunch of assumptions:

  1. kP is extremely large, such that motor commands are just “railed” at maximum for any non-zero error.
  2. The target (“desired”) angle is hardcoded to 45.0
  3. The robot only needs to rotate in one direction
  4. kI and kD are 0.0
  5. As soon as error crosses zero for the first time, you can stop the control action immediately.

Separately, this is a declarative structure - the code declares the steps you want your robot to take over time: first move to the angle, and then stop once you get there.

So, why would someone say you can’t do this? It’s not because your logic itself is flawed, it’s because the assumptions around writing exactly that code in TimedRobot aren’t quite right:

  1. Consider that usually, there are other things that need to happen. Intakes need to run, shooters need to be controlled, dashboards updated, LED’s need to be blinked :slight_smile: . The whole time you’re in the while loop, you’re doing nothing but drivetrain and gyro. While this keeps drivetrain and gyro happy, it generally won’t produce the desired behavior in other parts of the robot.
  2. You might need to rotate to angles other than 45.0, sometimes which means you have to go left or right (not just one direction).
  3. Sometimes I = D = 0 and P = many isn’t the right set of gains. Depends on the system being controlled, and the desired outcome of motion.

Using a standard PID controller takes care of a lot of the potential issues around 2 and 3. Moving to CommandBased, or “unrolling” the while loop takes care of 1.

3 Likes

Do I need PID control for positional angle control?

Simply, no. It is not required.

That question is different than, Can my positional angle control be more efficient if I use PID than if I don’t?"

The answer to that question is, more than likely Yes for your use cases in FRC.

That doesn’t mean that PID is required, but it would probably make it better. Here’s why:

PID controls at their most basic forms are feedback control loops. In your case of turning to an angle, it’s very easy to use the simple logic of If I’m traveling toward my desired point, but haven’t gotten there yet, keep going. Otherwise stop traveling. That effectively describes the while loop you mentioned without PID.

If you want your turn to be precise, and turn to that angle as quickly as possible with as perfect accuracy as possible, then you’d probably want to introduce PID. What do I mean by that? PID feedback loops are based on measuring the difference between two points:

  1. Where am I right now?
  2. Where do I want to be?

Let’s call the result of that difference calculation the Error measurement. When talking about turning toward a given angle, we’d want to drive as fast as possible, without overshooting so we can use the error measurement to slow down our turn as our error decreases.

It should make sense that if I’m 90 degrees away from my target I want to be driving faster than if I’m 9 degrees away from my target. That feedback is PID control in its simplest form.

1 Like

You have the right idea. This is exactly the solution I would use in writing a video game, with objects on the screen that did exactly what I told them to do, updating at possibly hundreds of frames per second.

Unfortunately, there are a ton of “gotchas” in writing software for a robot. Here are a number of things you should know that surprised me when I first started doing this:

  • The gyro is approximate

Check the accuracy specifications of any sensor before relying on it. The NavX is neat, but it drifts over time. It doesn’t have GPS, it’s accelerometer-only, so it will gradually get less and less accurate.

  • The RoboRIO and field management system are slow

The robot is only running the “periodic” functions quite periodically indeed. Once every 20ms. Keep in mind that the robot is still moving in between checks! One time through the event loop, the robot could be at 40 degrees, the next time it could be at 48. If you’re aiming for 45 degrees, you’ve now overshot. The overshooting can be quite dramatic depending on software/field lag if you’re doing the control yourself.

What’s key about this is that you’re probably already thinking: What if I just slow down as I approach the target so that I don’t overshoot? Yep. That’s absolutely correct. Let’s say you’re trying to turn 90 degrees. You go to full power, then at 45 degrees left you go to 1/2 power, at 22.5 degrees left you go to 1/4 power, etc., and you ease into the target perfectly, right? That’s exactly what a PID does! P means “proportional” and it means your power output is proportional to the remaining distance to the target. (The I and D terms are worth reading about; they adjust for error and deceleration rate, sort of. The reality is that a P term by itself will usually overshoot and oscillate around the target, or will shop short, so you can fix that.)

  • wheel encoders are more useful than you would guess

If you have encoders on your wheels, or are using Falcon 500s, you can read exactly how far the wheels have turned. The +/- error on this measurement is usually less than the error on the NavX (depending on a bunch of factors.) We found that turning in place by using purely the encoder measurements gave us surprisingly accurate angles.

I have an amusing video here from last season. Yes, I know. We should have used the Pathweaver library for smooth, continuous paths when doing auto. We’re still learning that library. It’s a lot. In the meantime, we leaned heavily on the TalonSRX motor controllers.

https://drive.google.com/file/d/1QIWzcpqvqCBcL80im9ZfZABHmrIPEmP-/view?usp=sharing

Yes, using drive/turn/drive/turn like this is slow, but this was largely an experimental exercise: These drive/turn commands are just TalonSRX PID position controls on the drive motors! Seriously, like two or three lines of code each. We’re not even using the NavX here. The huge wobbles are due to the robot being on pneumatic 6-wheel drive and accelerating and braking quite aggressively. The wheels are so grippy that the encoder readings we were getting were quite accurate and tell us exactly what the robot has done. The command-based system went to the next command when the robot stopped wobbling (when the encoders gave sufficient consecutive unchanging readings.)

  • Some controllers have built-in PID. Consider this instead of the wpilib PID.

Talon SRX (and Falcon 500) have built-in PID controls, and can read encoders DIRECTLY, so the encoders are wired into the controller, not the RoboRIO.

And this is the key point: The PID built into the controllers have a cycle time of 1ms instead of 20ms. That’s a huge difference. They’re checking the sensors 1000 times per second, and once you tell them what to do, they go do it, without waiting around for the RoboRIO or the field management system or any of your other code.

This could even work in conjunction with the gyro. If you know you need to spin 60 degrees, just tell your motor controllers to spin the robot 60 degrees. You can take measurements and make calculations to know how many wheel encoder ticks that is.

Learning about PID is worth doing; it’s basically allowing a lot of other code to do the hard work for you. You supply some numbers and magic happens. It’s frustrating at first. I started a whole separate thread here because I was completely wrong about what the Feedforward term did when using FPID for velocity control (which is an entirely different thing from position control. Also worth using; telling your intake/shooter/etc. to spin at an EXACT desired speed regardless of battery level, friction, load, etc., is soooooper useful. Imperative, even.)

In fact, if you post some information about your robot’s specifications here, I’m betting some helpful readers might even be able to suggest some starting points for code, including some reasonable initial values for the PID controls on particular systems. Make sure to include motors, controllers, encoders, gear ratios, wheel sizes, and desired outcomes. A few of us could likely save you many hours of headaches with just a few minutes of checking.

If you decide to use the WPILib PID controller (there will be good reasons to do so and I agree usually the motor controller is best), the Robot addPeriodic method speeds up the calculations. The docs even use PID as an example https://docs.wpilib.org/en/stable/docs/software/convenience-features/scheduling-functions.html

1 Like

A few thoughts. While I think I’m on roughly the same page as you, I think some of the justifications need qualification -

FMS should not be in the loop, so its incurred latency should not matter much.

20ms is a fairly normal controls frequency in industry. Certain applications may go faster, but I would be careful characterizing it as “slow” without qualification.

It will drift - 2 to 3 degrees over the course of the match. Lots of vibration or heavy collisions may increase this error.

I’d be careful with “Accelerometer-only” here - the guts of the nav-x aren’t super familiar to me, but I know for sure it uses a gyro, fused with other sensors. It can be better that a gyro-only setup in some cases, but comes with caveats.

This would imply a 400 deg/sec rotation rate. While not unreasonable, it’s pretty darn fast.

Do you have info on what sorts of wheels you were using in these cases? The details of how they scrub over the carpet (which in turn depends on materials, radius, center drop… lots of things) impacts how accurate this will be.

To be clear - I agree with pretty much everything else you said, thanks for the feedback and info! I don’t want you to walk away feeling bad. I’m guessing all the things you observed hold true for the robot’s you’ve seen. I just wanna make sure OP or someone else doesn’t take an assumption that’s specific to your robot and run with it, and hit issues later on.

1 Like

Well, I admittedly did some hand-waving here. We’re pushing the RoboRio pretty hard with extras like vision processing because we don’t have a Limelight and our experiments with RaspberryPi vision coprocessing, while successful, were just one more thing to break and we’ve set them aside for now.

But either way, if you’re doing PID control on the RIO, then everything you add to the code that could be a performance hit could affect your PID loops, even when multi-threading. 400 degrees/sec is fast, but we’ve definitely spun robots over 200 degrees per second. Try full stick open-loop rotate: How long does it take your robot to spin in a circle?

And I should have been clearer about the NavX: Yes, it has both gyro and accelerometer inputs, but definitely no GPS or other outside-in tracking, and while the angle is pretty good despite drifting a bit, the position tracking drifts quite a lot. When I first started coding these bots and saw that the NavX had position tracking I was excited about using it to know our position on the field, but after some experimentation and reading I realized that the position tracking is quite a bit more approximate than the angle readings. (Also, important note: Remember to recalibrate the NavX if you’re mounting the circuit board vertically! We’ve had some confused robots when the builders got ahead of the coders in making THAT modification to a prototype.)

We were using rubber pneumatic wheels, partially under-inflated at the corners and partially over-inflated in the middle (worked well for six-wheel drive.) They stick like glue on the carpet. In fact, before adjusting the inflations, the robot could hardly turn at all. Too sticky.

What I really have to do one day is set aside a whole weekend to go into the shop by myself with a prototype robot and really study and implement the robot drive profiling and trajectory following/pathweaver system. I’ve had some false starts on that in the past when trying to work on it during shop time. That system for auto paths is obviously immensely powerful and takes all of the various sensing mechanisms available into account when driving the robot along the specified path. But I didn’t want to send the OP down that rabbit hole just yet, especially as a relatively new coder!

1 Like

Yes for sure! I gotcha, yea, position tracking will basically be accelerometer only. Which gets very inaccurate very quickly for any sub-$500 accelerometer. FWIW I keep poking every now and then to see if the Zebra tracking system might consider making their location data available in real-time to the FMS and (therefor) to robots. I don’t know all the technical background on what it takes to get there, but IMO timestamped position on the field would be crazy awesome to have available.

Very interesting - so we’ve actually had the opposite experience with similar wheels. I had attributed it to the fact that, with a center drop, we were rocking back and forth between the front and back sets of wheels (and, therefor, changing the center of rotation). Still, both sample points are valid, I think it just points to the idea that we don’t know full root cause on what makes it accurate or inaccurate, it’s up to the end user to get the single sample point that really matters (their robot).

1 Like

I recall working on some robots where the CG was offset to the front or back so it primarily ran on the same 4 wheels most of the time. The two other wheels only came into play when accelerating or decelerating hard.

1 Like

For 2020 we intentionally did this to help do shooting line up. In 2019 and 2016… not so much.

1 Like

How do you access the built in PID for falcons? I can’t seem to find it in the docs.

Real offtopic for this thread, but their examples show how to do pretty much anything you want to do.