Mecanum - use of gyro

[mwsmith78]

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]

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

[Ether]

Originally Posted by mwsmith78

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.

[Ether]

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.

[kamocat]

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.

[Ether]

Originally Posted by kamocat

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]

Originally Posted by Ether

This "automatic delta time calculation" ... where in the documentation is that explained? The help file says only this:

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

[Ether]

Originally Posted by EricVanWyk

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?

[kamocat]

Yes.

In this case, I would expect it to be accurate within 1 millisecond.

[Ether]

Originally Posted by kamocat

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 ?

[kamocat]

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

[Ether]

Originally Posted by kamocat

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 ?