View Full Version : Mecanum - use of gyro
mwsmith78
01-12-2010, 09:06
Our team is in the process of playing around with a prototype mecanum chassis. We have a gyro installed and are trying to use it to correct for rotation errors (to drive straight).
Currently we are using the raw gyro rate (g) and the input from joystick (z) to feed the following logic ...
z' = k1 * (z - (g * k2)) + z
K1 is a P constant
K2 is factor to convert deg/sec to something in the -1 to 1 ballpark
It works, but not as smooth as we would like. We still get some drifting, but other times you can see it correcting (especially when we inflate k1).
Are we on the right track with out logic? do we just need to tweak constants more?
Do we need to filter the gyro input?
Anyone want to offer up some sample Labview code?
Related to the error correction, the team saw Team 1058's awesome mecanum video - where the robot corrects its rotation after impact. Does that (high freq) need to be handled differently that rotation drift error (low freq)?
Should we build our logic using the accumulated Gyro Angle variable? have the error correction work off the difference in the desired angle and the gyro angle? Seems like using the Angle values would cover both the Drift and Impact scenarios. But, how do you go about accumulating a desired angle from the joystick inputs? you would need to know the sampling time of joystick.
Any help would be much appreciated
Thanks
Jared Russell
01-12-2010, 09:23
What you really ought to look into is PID (proportional, integral, derivative) control - there have been a ton of threads on ChiefDelphi over the years on the topic (search for "PID" and you should see them). All three of the FRC languages have facilities for making PID control fairly painless.
Right now what you have is essentially a proportional-only controller (you have feedback on the error between your desired yaw rate and your actual yaw rate) with a feed-forward element (your command gets added to the output) acting on yaw rate. As you observed, this works okay in certain situations (especially with a high P constant - k1 in your case). However, at some point, raising k1 will start to make your bot thrash from side to side as it oscillates around your desired angular rate - it is simply going too fast as it gets near its set point. The "I" and "D" parts of PID address this issue, as well as steady-state error (when your bot knows it needs to turn one way or another, but the proportional part isn't enough to get it to actually get there).
I would recommend using the accumulated gyro angle rather than the instantaneous yaw rate because usually it is yaw you actually want to control rather than yaw rate. Even more specifically, you usually want to maintain a CONSTANT yaw (keep pointing in the same direction) when you aren't moving the rotation joystick axis. Usually when you are using the rotation axis, you can get away without feedback control (the driver will take over in that case). In other words, I would suggest some logic like:
If Z is not close to zero
Just apply Z to turning
Else if Z is close to zero
Use PID to adjust Z to keep the gyro_angle at its current value
end
It works, but not as smooth as we would like. We still get some drifting, but other times you can see it correcting (especially when we inflate k1).
To address these issues, you can try a somewhat more complicated approach.
Take your Z-axis (yaw rate) command from your joystick and integrate it (with a gain tuning constant) to create a yaw angle command.
Take the accumulated yaw angle measurement from the gyro and subtract it from the yaw angle command to form the yaw angle error.
Now you can feed this yaw angle error into a PID. The output from the PID is your modified yaw rate command Z' which gets sent (along with your fwd/rev and strafe joystick commands) to your mecanum logic which converts them in to the four wheel speeds.
Attached is an example LabVIEW implementation.
I'm not an experienced LabVIEW programmer but I think this is correct.
Note that with the LabVIEW PID vi, you feed in the setpoint and process variable, not the error.
The vi also does the output range limiting for you.
The constant K1 adjusts the sensitivity of your joystick Z-axis command.
The Z' output from the PID is your controlled yaw rate command which gets fed into your mecanum wheel speed calculation along with the fwd/rev and strafe joystick commands.
To tune the PID, try starting with proportional only.
Ether is correct.
The example is excellent, but is missing a WAIT function. The input to the PID function overrides automatic delta time calculation, but does not actually slow the loop down.
Note that you are not restrained to the 50hz loop rate. If you want your PID to respond more quickly, you can increase the loop rate. 50hz is simply the maximum rate you can receive new data from your joysticks.
The example is excellent, but is missing a WAIT function. The input to the PID function overrides automatic delta time calculation, but does not actually slow the loop down.
Thanks for pointing that out Marshal. I've made a note to update and re-post the attachment.
I have a question though:
This "automatic delta time calculation" ... where in the documentation is that explained? The help file says only this:
dt (s) specifies the interval, in seconds, at which this VI is called. If dt (s) is less than or equal to zero, this VI uses an internal timer with a one millisecond resolution. The default is –1.
... nothing in there about automatic delta time calculation.
Note that you are not restrained to the 50hz loop rate. If you want your PID to respond more quickly, you can increase the loop rate. 50hz is simply the maximum rate you can receive new data from your joysticks.
The 120+ pound robot has a substantial moment of inertia so I think increasing the PID rate beyond 50Hz is probably not going to make a noticeable difference in response.
EricVanWyk
02-12-2010, 18:52
This "automatic delta time calculation" ... where in the documentation is that explained? The help file says only this:
dt (s) specifies the interval, in seconds, at which this VI is called. If dt (s) is less than or equal to zero, this VI uses an internal timer with a one millisecond resolution. The default is –1.
... nothing in there about automatic delta time calculation.
The second sentence of the documentation you quoted states that it uses a timer to calculate delta t, which sounds like "automatic delta time calculation" to me.
Because the VI's default for dt is -1, the default behavior is for it to calculate its own dt internally based on the timer. When you wire something real up to it, that will disable the timer and use that value instead.
The second sentence of the documentation you quoted states that it uses a timer to calculate delta t, which sounds like "automatic delta time calculation" to me.
OK I get it now. The internal 1ms resolution timer keeps track of the elapsed time since the PID was last executed, and uses that elapsed time for the computation (derivative & integration) for the current iteration? So the elapsed time used for the computation can (will) be different (due to jitter) from iteration to iteration?
Yes.
In this case, I would expect it to be accurate within 1 millisecond.
In this case, I would expect it to be accurate within 1 millisecond.
I'm surprised to hear you say that. Please elaborate.
Have you ever measured the realtime jitter in the Teleop loop of the FRC Framework ?
I have.
I was actually expecting this to be placed in a Periodic tasks, and estimating. I know this code is not likely to take 20ms to execute, and in looking at time-to-execute in loops, it's usually the value wired into the wait function (occasionally plus or minus a millisecond).
As for the packet jitter from the DS, I have done more detailed testing (http://www.chiefdelphi.com/forums/showthread.php?t=86266).
Packets tend to arrive in multiples of 20ms, but and are usually within 2 milliseconds. (This is assuming you're only running the DS and robot for 1 match. Strange things happen over several hours.) Most packets arrive at 40ms (that is to say, half the packets the DS sends aren't received by the robot). It's likely a packet-free period greater than 500ms will occur 10 times within 1 match.
EDIT:
I'm assuming it's inconsequential, but I measured from the loop in Robot Main, and from the DriverStation StartCommunication.vi, not from Teleop itself. The purpose of the tests was to examine the reliability of the Driver Station, not the determinacy of Teleop.vi and RobotMode.vi.
I was actually expecting this to be placed in a Periodic tasks, and estimating.
Are you saying that jitter would be less than 1ms in a periodic task in a ready-for-competition code release that has several concurrent threads competing for CPU ?
Are you saying that jitter would be less than 1ms in a periodic task in a ready-for-competition code release that has several concurrent threads competing for CPU ?
Oops, I forgot to answer your question.
Yes, that's what I'm saying.
I'll take the chance of making an untested estimate, assuming no-one hangs their life on it.
It's be interesting to do a decent test.
(I didn't use image processing last year, so that probably made a difference)
I wonder if it would be more deterministic if we made the wait function sequential (using a flat sequence structure)?
mwsmith78
03-12-2010, 13:50
Thanks for all the great feedback. We tried out the example VI last night. We inserted it in the Timed-Tasks area with a 20ms wait loop.
We had to debug some other problems that ate up much of our time last night, but once we got past that it sort of worked. PID gains were VERY sensitive. We had lots of trouble getting a stable AND responsive control loop. Tried using a P-only loop, but responses ranged from 1) robot studdering to get to desired angle or 2) robot overshooting and wildly rotating back-forth. Had to keep P<0.02 in order for it to be stable (but not really responsive). Will try tweaking the gains more next week. Have others had this much trouble tuning a PID loop?
Thanks for all the great feedback. We tried out the example VI last night. We inserted it in the Timed-Tasks area with a 20ms wait loop.
We had to debug some other problems that ate up much of our time last night, but once we got past that it sort of worked. PID gains were VERY sensitive. We had lots of trouble getting a stable AND responsive control loop. Tried using a P-only loop, but responses ranged from 1) robot studdering to get to desired angle or 2) robot overshooting and wildly rotating back-forth. Had to keep P<0.02 in order for it to be stable (but not really responsive). Will try tweaking the gains more next week. Have others had this much trouble tuning a PID loop?
Tell us a little more about your vehicle. What size and kind of mecanum wheels are you using; how are they mounted (can you post a close-up picture to go along with your words); are the rollers contoured and do they spin freely. And are you using direct-drive or chain drive, and how much free play is there at the wheels (ie if you lock the motor shaft, how much - what angle - can you rotate the wheels back and forth).
Alan Anderson
03-12-2010, 14:45
Before you do any more with the software, make absolutely sure your hardware is correct. A "stuttering" turn is sometimes a sign that you have your mecanum wheels mounted in the wrong orientation. The rollers contacting the floor should make an "O" or diamond pattern; they should look like an "X" from above.
Also make sure you understand what the inputs to the LabVIEW PID block mean. I haven't looked at it in over a year, but at that time the I and D parameters were not gains. They were time values. That scheme is just as easy to tune as a gain-based one, as long as you recognize that a smaller I or D value yields a stronger response than a larger value.
Tell us a little more about your vehicle. What size and kind of mecanum wheels are you using; how are they mounted (can you post a close-up picture to go along with your words)
With mecanum wheels, the mounting is very important, and is a common source of error:
each mecanum wheel is either right-handed or left-handed. It is important that the wheels are mounted at the correct locations (http://www.chiefdelphi.com/forums/showpost.php?p=907174&postcount=18) on the vehicle
mounting mecanum wheels at the end of a "stalk" that is not stiff enough can cause control issues
Tom Line
03-12-2010, 19:21
Before you do any more with the software, make absolutely sure your hardware is correct. A "stuttering" turn is sometimes a sign that you have your mecanum wheels mounted in the wrong orientation. The rollers contacting the floor should make an "O" or diamond pattern; they should look like an "X" from above.
Also make sure you understand what the inputs to the LabVIEW PID block mean. I haven't looked at it in over a year, but at that time the I and D parameters were not gains. They were time values. That scheme is just as easy to tune as a gain-based one, as long as you recognize that a smaller I or D value yields a stronger response than a larger value.
Oh. My. Gosh.
We've been using labview since FRC started using it, and I never even thought to check it: I just assumed it was a normal non-dimensional gain number that would act the way most PID setups expect it to act.
During this year's game we had a very robust targetting system in real time that would turn the robot and shoot after pushing the fire button. However, we removed it because we could not tune in the I and D portions to get the level of speed required: it was slightly slower than our driver's aiming and therefore wasn't effective.
I feel very, very silly right about now. :o
Are you saying that jitter would be less than 1ms in a periodic task in a ready-for-competition code release that has several concurrent threads competing for CPU ?
I would expect there to be more than 1ms jitter unless you are using a timed loop for your periodic task. Especially if you are running your controller in teleop. That loop timing is based on the incoming UDP packets from the DS, not a local timer.
Even if you aren't, though, the benefit of the automatic delta time calculation is that your controller will still behave well even in the presence of jitter. It simply measures the jitter and uses the actual time instead of the ideal time when computing the I and D terms.
I would expect there to be more than 1ms jitter unless you are using a timed loop for your periodic task.
So you're saying you'd expect jitter in a periodic task to be less than 1ms (in a full-function competition code release with many concurrent tasks)?
You know more about LabVIEW than I do, but I find that surprising. I would have thought that the way LabVIEW protects critical sections would create more software latency than that.
the benefit of the automatic delta time calculation is that your controller will still behave well even in the presence of jitter. It simply measures the jitter and uses the actual time instead of the ideal time when computing the I and D terms.
Capisco. I just misread the LabVIEW documentation.
Out of curiousity I looked at the PID block diagram to see what numerical integration algorithm it uses. Not being very adept at LabVIEW I couldn't parse it (and it wasn't commented). Is it simple Euler or something more sophisticated ?
For clarification:
http://content.screencast.com/users/kamocat/folders/Jing/media/bc158dbb-2dc5-4153-9b8d-a8078e38b8ae/timed%20loop.png
Apparentally you can get resolution finer than 1ms using a 1MHz timing source, but I've never had a time-critical task that executes that quickly.
Greg McKaskle
05-12-2010, 08:56
I've attached the context help for the PID. It is described as the simple one, but it includes integrator anti-windup, output limits, and bumpless gain changes. It also supports wiring up N values in an array, and it will manage and control all of them using the same control settings.
As for learning to read LV, that is definitely starting pretty deep in the pool, but I'll point out the big chunks.
In the lower left of the loop, it determines what the dt is. It can be user specified or if unwired, is calculated by the function.
Upper left are some comparisons to determine if the PID coefficients are the same as the previous call (to fix the state data and avoid bumps).
The code in the center and lower right calculate the terms. Note that this uses the academic form with Kc, Ti, and Td. The two icons at the upper left of the switches were used to document which term was being computed.
Other comments are sprinkled inside the cases for the exceptional cases such as reinitialize, dt=0, etc.
Note that I'm describing the PID.vi, the simple one. There are several others in the full Control portion of the palette -- Advanced, Auto-tuning, etc. I didn't write any of these, but may be able to help you read them if that is what you are asking.
Greg McKaskle
Greg McKaskle
05-12-2010, 10:46
Concerning the loops and timing jitter, the normal while loop inherits the priority of the VI it is within. Note that by default, subVIs inherit their caller's priority as well. Initially everything in the FRC framework is set to normal priority. If you decide you want to bump something, you change that subVI's priority in its properties/execution page, and it typically propagates as you intend.
The delay used in the while loop that was shown has a resolution of 1 ms, and is not considered low jitter, though it typically performs fine IMO. The low jitter and higher resolution delays and timing functions are located in the real-time palette.
The timed loop incorporates these timing functions into the loop and performs many of the common calculations for you. The timed loop lets you choose between a 1ms, 1us, or device specific clocks which may be available. It lets you choose between a few different timing policies concerning missed periods. It lets you set priority on each loop without needing to embed it in a subVI. It lets you more easily measure actual start times vs scheduled start time, actual and scheduled finish times, whether the loop met or missed the deadline and by how much, etc.
From what I've seen, common FRC tasks are fine with msec resolution and aren't bothered by a few ms of jitter, so the framework starts simple. Meanwhile, if you find that an arm or other mechanism needs it, the real-time mechanisms are there to lower the jitter. Similarly, the framework uses simple globals to share information between loops and subVIs. To gain determinism, you would trade those for RT FIFOs which are more complex, but can offer jitter or performance benefits and offer more control in corner situations.
Greg McKaskle
mwsmith78
07-12-2010, 08:43
Lots of posts since my last one ... I will try to knock off all the questions.
Using 6" Mecanum wheels with tapered rollers (from AndyMark). Wheels are correctly mounted. The robot drives correctly w/o PID loop - although it does tend to have rotation error. Given that it works well before attempting closed-loop control, I believe that points to the software.
If I understand correctly about the PID post - the I & D gain values are time constants and for tuning purposes should start HIGH and work down. Meanwhile, the proportion gain should start low and work up. That may explain why we were having trouble tuning. When we inserted I and/or D values, we started very small. If we want it to function as a P-only loop, is setting I and D to zero correct?
http://www.youtube.com/watch?v=KL7iZXCRtPw (http://www.youtube.com/watch?v=KL7iZXCRtPw)
mwsmith78
07-12-2010, 08:44
Direct-driven using CIMs and Victors.
Have another post pending that will answer other questions. It includes youtube video and requires moderator approval.
mwsmith78
07-12-2010, 11:46
Lots of posts since my last one ... I will try to knock off all the questions.
Using 6" Mecanum wheels with tapered rollers (from AndyMark). Wheels are correctly mounted. The robot drives correctly w/o PID loop - although it does tend to have rotation error. Given that it works well before attempting closed-loop control, I believe that points to the software.
If I understand correctly about the PID post - the I & D gain values are time constants and for tuning purposes should start HIGH and work down. Meanwhile, the proportion gain should start low and work up. That may explain why we were having trouble tuning. When we inserted I and/or D values, we started very small. If we want it to function as a P-only loop, is setting I and D to zero correct?
The robot drives correctly w/o PID loop - although it does tend to have rotation error.
Could you please clarify what you mean by "rotation error"? Are you saying that when you give a "straight ahead" command, the bot wants to turn?
Or are you saying that when you give a "turn" command, it doesn't respond as you think it should?
(or something else?)
Are you using the built-in vi to compute mecanum wheel speeds, or did you design and code your own algorithm ?
I can anwser the question about the rotation error. What he means is that the robot drifts when we do not expect it to and that is why we wanted a software solution to the rotation error.
If I understand correctly about the PID post - the I & D gain values are time constants and for tuning purposes should start HIGH and work down. Meanwhile, the proportion gain should start low and work up. That may explain why we were having trouble tuning. When we inserted I and/or D values, we started very small. If we want it to function as a P-only loop, is setting I and D to zero correct?
Yes, you are correct.
Greg,
I'm curious....why does the PID.vi itself use a regular while loop, rather than a timed loop?
Greg,
I'm curious....why does the PID.vi itself use a regular while loop, rather than a timed loop?
In this case, the While loop is actually not used to loop the PID function (if it were, the PID function would never get new inputs). It is instead simply for its shift registers. Note that the loop is set to end on the first iteration, and none of the shift registers are initialized (meaning they retain their values between calls).
It is also a reentrant VI, storing the shift register data separately for each instance.
The other way to accomplish this is to use feedback nodes (found in the structures palette). However, shift registers often create cleaner and more readable code, and so are more commonly used.
Greg McKaskle
15-12-2010, 10:09
Since the PID VI is a subVI, it is intended to be placed after the sensor and before the actuator. Placing this into a user's loop, timed loop, or callback lets the user control the timing and priority. If these are placed into a timed loop set at a higher priority, the PID and other VIs will inherit the higher priority.
If this were flipped and the PID were an object that owned the loop, you could do this by registering the sensor, actuator, timing, priority, and other elements with it. I believe this is how it was done with Java and C++.
I'm assuming this was useful because the PID may need to spin up a new thread and this handles it for the user. Meanwhile in LV, it is easy to draw a parallel loop if that is what you want.
Different strokes.
Greg McKaskle
bump.
@mwsmith78: haven't heard from you in almost 2 weeks. could you give us an update on where you're at with this? did you resolve it successfully, or did you move on to other issues?
keericks
10-01-2011, 15:10
Lots of posts since my last one ... I will try to knock off all the questions.
Using 6" Mecanum wheels with tapered rollers (from AndyMark). Wheels are correctly mounted. The robot drives correctly w/o PID loop - although it does tend to have rotation error. Given that it works well before attempting closed-loop control, I believe that points to the software.
If I understand correctly about the PID post - the I & D gain values are time constants and for tuning purposes should start HIGH and work down. Meanwhile, the proportion gain should start low and work up. That may explain why we were having trouble tuning. When we inserted I and/or D values, we started very small. If we want it to function as a P-only loop, is setting I and D to zero correct?
http://www.youtube.com/watch?v=KL7iZXCRtPw (http://www.youtube.com/watch?v=KL7iZXCRtPw)
In the video above, I notice the person pushing the robot and it responds to return back to a specific pointing direction. Is this based on vision camera, gyro, wheel encoders? Thanks!
vBulletin® v3.6.4, Copyright ©2000-2017, Jelsoft Enterprises Ltd.