Hi this is our first year in this competition and as a rookie programmer I’ve attempted to write a could to auto balance my robot. Now I may be completely wrong but I was wondering if I did anything right or this could even work when I’m on the bridge.
You’ve got the right idea, but the implementation needs some work.
First - never set motors inside if/then’s like that. The motor set should be outside, over to the right of everything. You should only have 1 of any particular motor set in teleop. This helps to avoid potential race issues.
Next, your case structures should be nested inside one another. This is good coding practice, and helps to avoid extra calculations. There’s no reason to check one if the other is true…
Next, you’ll find this will oscillate around your setpoint. Mechanical systems have momentum, and once it starts moving it won’t stop immediately. It will drive back and forth over your setpoint (probably quite wildly with a motor value of .75!).
I suggested reading some papers on PID’s. That’s what you need to implement here.
I have tried to read on PID’s but I simply do not understand them since they dont really provide a practical example for what I what. This would all go in my Periodic Tasks.vi and well well I just put the left and right ref get was just so you could see what I’m trying to do. I set it to .75 to limit the speed a bit but I’m not sure if thats correct.
Is the difficulty you’re having to do with understanding how a PID loop works or understanding how to implement one? Many resources you can find online—especially on this very forum—are great at explaining the former, but details on the latter can be harder to find. I haven’t programmed a robot in LabVIEW for a few years, but if I recall correctly there are PID VIs that you can put in your code that do most of the work for you; they just need tuning of the constants. Some FRC teams—including some that use LabVIEW—post their previous years’ code here; reading examples is a great way to learn.
A few other things to watch out for:
-As Tom mentioned, nest the case structures and make them manipulate numbers that feed into a single motor command (not just here but anywhere you have motor output).
-The gyro will in practice never give exactly 0; your code as above relies on that to stop the drivetrain from moving.
-If your gyro angle is, say, –20, I don’t know if the remainder operation will output –20 or 340. Unless your robot does a somersault (which for all I know might be crucial to your strategy :D), the gyro you use for bridge balancing will never be out of the range –90, 90], so you should be fine without taking the remainder.
So from what I understand is that I have to have a set point, which I assume would be zero since when the angle reaches zero thats when I want it to stop. My process variable would be the angle output of the sensor, and well the output would go to my tank drive vi. and basically from what I’ve read the only thing that I would need to tune would be the proportional gain. Heres what I came up with.
Not to discourage you or anything, but has anyone actually tried this and gotten it to work? My team tried using it at first, but we realized that automatic PID control could never be as good as human prediction. I think the problem with auto-balancing is that PID is completely reactive. In order to be able to balance successfully, a human driver needs to be able to pretty accurately predict how the bridge will react to the robot moving and adjust accordingly. If you can get this to work, great, but I feel the problem may be more complicated than just using PID and human control will always be better.
Team 135 was able to get this to work quite well with PID control. I do not have access to the code right now or I would post it. We found that the robot could balance in about half the time than the human could. You are definitely on to something and you should not be discouraged. FIRST is all about figuring out problems that you never thought you would come across. If I am in our shop in the next few days, I will try to get a screen shot of how we are doing it.
Not to discourage you or anything, but has anyone actually tried this and gotten it to work?
It’s been done repeatedly for many years on problems much more difficult than this year’s FIRST challenge. (Ask Dean Kamen about something called a “Segway”). You have a bridge that will balance if the CG is located within about a 6" band between the hinges.
I feel the problem may be more complicated than just using PID and human control will always be better.
You may want to do a little more research. The only thing the current problem requires is a quick reaction time. Human reactions will never come close to machine reactions - judgement is a different subject! 
Try something like either of these under human control if you think you can:
If other teams have done it, I stand corrected, but I guess what I really was getting at was that these robots that we’re developing in FIRST (or at least our team’s) aren’t ideal for balancing. Our robot’s center of gravity is not in the very center since we never really had time to fine-tune that. Those robots in those videos are small and light and built specifically for balancing, and the Segway especially is built to have perfect balance. I guess what I really meant was that in the time frame we had for coding and tweaking, we realized we wouldn’t be able to develop something that was better at balancing than the human driver. Although it is true that the design of the bridges makes it slightly easier than if they were just balanced on a single fulcrum. Also we had a weird issue with the gyro itself, but that’s a separate problem.
fsgond, I’d love to see your code for the PID. My team is new to PID, we only started using it this year and we don’t really know much about how to use it or especially how to tweak it to get optimal coefficients.
I always go back to the basics on controls questions.
Your control loop is only as good as your sensor feedback.
Have you tried to see what a single Gyro input does when you are using the get angle in Labview? Did you notice that the gyro drifts over time, 1-10 degrees? Because of this drift problem, your PID will always be seeking “ZERO” if that is your set point, but your gyro is reporting the wrong angle in relation to your robot.
Study the sensor first, and determine if it’s responsive enough, and “accurate” for your controls loop. Always do that first to prove that to yourself, before writing the first bit of code. Turns out, you can use labview to study the raw data from the sensor to determine it’s response, and accuracy over time. You “could” reset the gyro before you visit the ramp, but try mounting the gyro on the robot and flipping the ramp back and forth about 3-4 times, then make the ramp go back to level, does your gyro raw data still read 0 degrees after just a few iteration of tilt cycles? If not, your control loop won’t work either because your sensor data is mis-reporting due to gyro drift of integrating an angle from the gyro rate.
Spend some time on getting accurate sensor feedback, before moving to control loops is what I am suggesting.
Bottom line, the problem may not be your control loop, but because your sensor is mis-reporting, has noise, is effected by another axial movement, such as acceleration of the robot, etc…
Chris brings up a good point with this. You will need to reset your gyro to zero quite often. It was very frustrating during the initial coding to have to constantly reset it. As it stands not we will need to zero our gyro seconds before we attempt to balance to assure that it is close enough to achieve the feat.
Well we tried to balance it using human control but we realized that its to top heavy an its proned to tip. But other than the obvious points that i will need to constantly reset it, is the code sound? I know that the robot will never be exactly at zero but i assume the accerlation will be low enough that it wont move as it gets close to zero…
PID does not always need to be reactive. In general terms, there is a further thing you can add called FeedForward, which you can apply to either velocity or acceleration, as the case may be. The basic idea with Feedforward is that you know you’ll be moving in a certain direction anyway, so give a little boost to your velocity or acceleration, then let the rest of PID react once you get going.
I’ve taught PID to quite a range of people. I really ought to do a youtube video, so i don’t have to perform it as much.
The basic idea for PID is that it corrects for errors. if you make a PID loop based on position, it will move your motors such that the position is what you want. you can replace the word position with the words velocity, angle, and even acceleration or motor current. you can also loop multiple parts of your motion. (i.e. Loop position and velocity), but for FIRST, you usually only need to put a loop around one thing.
Let’s use position for discussion, which is easiest to visualize. First, you need a way to measure your position (an encoder can be used for position or velocity, the gyro or an accelerometer can measure angular data.). The difference between the position you are in, and the position you want to be in, is your error. PID acts on this error to try and make it 0.
In a typical machine tool (like a CNC machine (Hey look, FIRST applies to the real world :))), you want to position an axis. Say the axis is currently at zero, and you want to move it a meter. Your commanded position is 1m, and your actual is 0m, making your error +1m. (The sign is important!). Proportional Gain is a number we multiply by the error to get a motor command. Sometimes this is a speed (engineering unis such as m/s), or in our case, it is a motor command in the range of -1 to +1. Let’s say our proportional gain is 0.5. 0.5(gain)*1m(error) = 0.5(command), which moves our motor towards where it is supposed to go.
For the sake of discussion, let’s say our motor is geared such that a command of 1 means the mechanism in question is moving at 1m/s. That is also the top speed of the motion, since the command cannot be greater than 1.
PID loops execute repeatedly in your Periodic tasks, so they occur with a fixed time base. To demonstrate, I’ve put this together as an excel worksheet, and posted it to my blog. The sheet lets you play with some different PID settings, and lets you look at how the mechanism will move based on the commanded velocity.
I finish the discussion of PID there. (That post is based on this one)
Simulating the bridge in excel is a bit harder, since you need to keep track of the CG of the bridge and everything on it, and use that (as well as inertia) to model the bridge actual angle. The basic PID concepts follow along, but the response of the bridge is not a 1:1 relationship with what you command for motor velocity.
I believe you’ll find that once you get to competition, bridge balancing is going to be quite easier to be handled manually by the drivers to compensate for inconsistencies like the number of bots on the bridge and so on. Since the bridges are calibrated with springs to return to 0 degrees, they are pretty easy to balance on.
If you still want to try a form of software balancing, more power to you. I did mine similar to how you did yours the first time around. The way you wrote your code the first time was pretty good, but just needs a few changes.
Since the bridges are considered balanced if they are within 15 degrees either way of 0, you don’t need to be directly at 0 degrees, nor would the Gyro be accurate enough to bring you to 0 degrees exactly. I’d say < 3.5 and > 3.5 to give a 7 degree drift.
I’m not sure how your bot’s drive train works, but 0.75 is quite speedy for ours. You might consider telling your drivers to drive up onto the ramp fully THEN engage the auto-balancing code, which may allow you to use slower motor outputs, leading to a more accurate balance.
As someone else suggested, you should nestle your case structures. If the < 3.5 case structure is false, check to see if it’s > 3.5. If that’s false too, set the motor outputs to 0.
Dom
I was not aware that the competition bridges are calibrated with springs. I was under the impression that they use their own considerable weight and the off-center position of their hinges to effect their self centering. Can you please point me to the documentation that led you to this conclusion?
In addition, I believed that the bridges needed to be within 5 degrees of level to be balanced. Where did the 15 degrees come from? That is a considerable angle.
Team 1124 has gotten autonomous bridge balancing to work very well. Here’s a video of our previous year’s robot with the balancing code on it (this was before this year’s robot was completed) You’ll notice the bridge in this video is easily tipped. Our bridge was too light, which made balancing harder to achieve. The code worked even better when we tried it with a competition bridge.
If anyone’s interested in our method:
We use both a gyro and accelerometer to balance our robot. The gyro is good at giving an accurate angular rate, but not absolute angle, while the accelerometer is good at giving an accurate absolute angle, but not angular rate. We decided to combine the best features of both these sensors to get the best control.
I don’t want to release the entire code yet, but here’s essentially how it works.
It uses a state machine to decide how to behave. The driver drives the robot slowly up the bridge until the gyro rate is greater than a certain amount in either direction, at which point the code takes control of driving. The robot then drives in the opposite direction until a second significant angular rate is detected. After this sequence, the accelerometer is read for an accurate angle reading. If it is determined that balancing failed, it restarts. If it determines it is balanced, it holds its position and waits for the gyro to return a significant rate allowing it to re-balance.
You should be able to see all of these steps from the video above.
We use closed loop control on the drive (with PID) to control the rate at which the robot drives up the ramp in order to keep consistency.
Our drivers have been very pleased with this feature. It has yet to fail. It almost always balances the robot on the first try. When it doesn’t, it gets it on its second, as seen in the video. This method is much faster than when our drivers attempted it manually.
I’d be happy to answer any questions related to our method of balancing, PID, or the sensors we used. Good luck to anyone else attempting this!
Hmm, maybe I’m assuming too much. I presumed springs were used, but I was wrong 
It is, in fact, a double hinge.
2.2.5 The Bridges
Robots traverse the center of the Court by crossing over either one of three Bridges or the 4 in. tall by 6 in. wide, smooth steel Barriers running between them. Each Alliance has one dedicated Bridge for their use at end of their Alley. An additional white Coopertition Bridge is located at the center of the Court. Each Bridge is 48 in. wide, 88 in. long (outside dimensions), and sits with the top platform 12 in. high off the ground when level. Each Bridge is mounted on a double-hinge that allows the Bridge to tip towards either end of Court. The top surface of each bridge includes an array of 15 small holes, details of which are included in the official field drawings.A Bridge will count as Balanced if it is within 5° of horizontal and all Robots touching it are fully supported by it.
There’s the rule stating when the bridge is balanced. It was 5, not 15. Memory fails me again! I’ll be doing more research next time I try to ‘help’ someone out
Did I end up getting the PID structure correct…
Yes, but it’s a bit more complicated than that. Keep in mind that the gyro measures yaw (change in angle) around an axis through the gyro itself, but your robot does not balance around this axis; it balances on the bridge. You may have issues because the yaw rate is not directly proportional to the bridge angle.
As mentioned earlier, mechanical systems have momentum; for this reason, a proportional gain will almost always result in oscillation around the setpoint. Because of the double hinge setup for the bridge, however, it has a noticeable resistance to tipping, so an incredibly small P may allow you to successfully balance without tuning I or D, but your robot will be moving very slowly to the setpoint, which could possibly, depending on your wheels/drivetrain, be ineffective due to stalling or slipping. For a time-effective balancing mechanism, you will likely want to tune both P and I.
Searching the forums will yield a wealth of discussion on how to tune a PID loop; there are also a number of discussions about how to tune PI loops and ignore the D.
Well my plan was to just get the robot on the bridge on regular control, then press the button to begin the balancing then just let go of the button when its sufficiently balanced. Now for mounting the gyro it would be mounted vertically, not laying flat parallel to the floor right?