Mecanum - use of gyro

[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 ?

[kamocat]

Originally Posted by Ether

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 ?

[EDIT]
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.
[/EDIT]

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]

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?

[jhersh]

Originally Posted by Ether

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.

[Ether]

Originally Posted by jhersh

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.

[Ether]

Originally Posted by jhersh

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 ?