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