[DFTF] Closing the loop on Wheel Velocity...

This is part of a series of posts called Drinking From The Firehose on getting Dr Joe back up to speed on All Things FIRST.

Which led to this post: Jags v. Vics

Which led to this post: We eat what we CAN, and what we can’t, we CAN…

Which led to today’s topic:
Closing the loop on Wheel Velocity…

I have never done this in a FIRST robot, but my background in controls, everything I have read, and my gut tells me that wheels should be driven in closed loop mode (not open loop mode, i.e. set a PWM value) AND that the closed loop should be based on velocity not position (well, I lied. It should use position but only in autonomous mode and even then a fast acting velocity loop should be then wrapped in a relatively slow acting position loop).

Drivers should command wheel velocities not motor voltages.

So… …has anyone done this?

Barello.net has some great info on the subject (read here)

Here are my questions:
If you read the reference Barello likes (Mobile Robots, from Inspiration to Implementation) they basically only have a P (Proportional) control on velocity. They use an I (Integral) term but it is only integrating (i.e. adding up) the relative error between the right and left sides. Which I think is probably better implemented using a gyro (i.e. an angular rate sensor).

Comments welcome.

Has anyone used a Jaguar to close the velocity loop? The standard PID modes do not support this which is a shame (you can input an encoder but the PID loop is then based on position of the encoders not speed). This should be a relatively easy code change to implement but I don’t think we are allowed to muck with the firmware for the Jaguars and still be FIRST legal so…

…I have another idea. I think that the performance of the velocity control would be much enhanced by a fast PID such as the 1000hz loop time that I have heard that the Jaguars use in PID mode. So… here is my scheme…

What if I had an Arduino (or any micro, pick your favorite) monitor encoders from the RH & LH wheels and then generated with an ADC corresponding RH and LH voltages for each side that ranged between 0-3V (top speed in rev = 0 volts, top speed in fwd = 3 volts). THEN we fed these voltages to the analog input of the RH and LH Jaguars. THEN we put the Jaguar in PID mode.

In effect, as long as the Arduino provided fast enough and faithful enough updates to the Jaguars, we would be able to take advantage of that 1000hz update rate – this is a big if because we would need velocity updates at over 1000hz which rule out encoders I think because at slow robot speeds (<1 fps), you would still need over 1000 pulses per second or more than 100 pulses per inch. Not impossible but tough. I think it may be better to use a continuous rotation magnetic encoder. I think it would be possible to get updates fast enough.

The next problem I worry about if I tried this is that the Jaguars would fight each other. If each Jaguar is doing its own PID loop internal, then I can envision a world in which the Jaguars are fighting each other (imagine if the integral error terms start going in different directions - cancelling each other out in the net, but wasting energy and heating my motors all the while).

So… …after all that, I don’t really think independent PIDs are a viable solution for multimotor drive systems. Unless you can go with a PD control (leaving out the I, which has the most potential to get you into trouble – and really to we care about driving steady state error in velocity to zero? Maybe not. We would want to make corrections to go straight but that could be done by mucking with the set points without having to have an integral term… )


The other option is to do things the old fashion way, inside the cRio. With one coordinated output to multiple Jaguars (or Victors) the fighting problem is solved, but not I don’t have my 1000hz feedback loop.

From what I hear, you can get a 200hz update rate via PWMs using the cRio.

Is that fast enough? I don’t know. Do you?

Joe J.

You bring up a lot of interesting conflicts.
We tried traction control in 2009 by cascading a velocity loop with a ground speed loop providing the target speed but that was through the CRio and we were trying to keep the wheels spinning just slightly faster than the ground speed.

Pre-processing the feedback is interesting. Using something like a USDigital MA-4 to provide an analog position and processing it fast enough to push the Jaguar sample time could be useful. I tried it and failed years ago because of throughput limits of the IFI system but a dedicated processor now days could probably pull it off. I’m not sure of the analog conversion rates on an Arduino.

I have pretty well given up on using absolute wheel position for autonomous distance because of wheel slip issues. This year our carpet was enough different from the field carpet that we had large errors when we tried to go fast enough for double tube scoring. I feel like trying to increase the wheel coupling will create problems that we don’t have now with motor overloading and drive train reliability. Instead we went back to an encoder with an omni-wheel running on the floor. We have used an X axis encoder to control drift and crab angle fairly successfully too. I haven’t tried to use this for velocity feedback yet but I think it is possible. I do know that the absolute distance accuracy was repeatable even on tile and with careful control over down pressure it held up well.

The issues of synchronization using CAN are still unresolved as far as I can tell. We tried synching our two elevator motor Jaguars several ways including paralleling the encoder to both, copying the output of one to the other, and using the CAN synch commands and the two would never stop at the same position. One would still be driving and heat up the motor if stopped long enough. There is a long thread on CD from this year on the subject and I don’t think a solution ever surfaced there either. We finally had to use a cRio PID loop with the output sent to both. Eventually we went back to PWM and used a Y cable. This was because of a CAN system problem that we never could totally avoid where the bus just wouldn’t initialize occasionally and we had to manually reset if it happened on the field.

I don’t know if any of this directly addresses rate control but all these affect the ability to pull it off well.

I couldn’t find a more technical description of Nitro, but that robot was rebuilt several times and initially used the NI SoftMotion SW with proto-jags to do position and velocity control via CAN. I wasn’t involved in the early planning, but came in as part of a cleanup crew. From what I understand, the motion control was running on the cRIO at a few kHz as an interrupt routine driven by the FPGA clocking. It used true CAN module, high resolution encoders, and customized jags.

As the guy tasked with trying to get a robot to dance, it turns out that you really want more than position setpoints and an overall velocity. Whew.

If you are interested, I can ask one of the control leads on the project for a summary of it. I’m not sure quite how to map it to what is in the kit, but there is also the option of contributing to the jag SW and seeing if TI and the control team can bless the added functionality from a safety aspect.

Greg McKaksle

From an analytic perspective, I don’t necessarily believe that 1000hz operation will yield significantly better response than, say, a 100-200hz velocity loop running on the cRIO. The important thing is that your loop rate is higher (at least 10x higher is the rule of thumb my thesis advisor used) than 1/the time constant of the system you are controlling. When brushed DC motors are connected to a load, their time constant may be on the order of 100s of milliseconds or more (depends on the load’s inertia and damping). In other words, a 4 CIM drivetrain with traction wheels geared for 10 fps free speed won’t hit (63.2% of) 10 fps instantaneously when you floor the accelerator. Additionally, as you pointed out, getting good velocity feedback at 1khz rates presents a challenge in its own right (I think magnetic encoders are the best way to go here).

Your mileage may vary, and I’d welcome anyone to do empirical studies that show whether 1000hz gives them better dynamic velocity tracking performance than 200hz (for Jags) or 120hz (for Vics).*

  • 200hz and 120hz are the maximum input rates for Jags and Vics, respectively.


I am confused with your comment here. It does support this; they just call it “Speed Control Mode” See page 12 in the attached manual.

Am I missing something?


MDL-BDC24_BDC_COMM.pdf (205 KB)

MDL-BDC24_BDC_COMM.pdf (205 KB)

The reaction time of motors is faster than you might guess, especially when we are controling velocity not position.

I am not sure what it is in this case, but I am concerned that 100hz is not fast enough given some other experiences I have run into.

Joe J.

No, I guess I have been missing something. I have looked at a lot of Jaguar docs but missed that one.

Looks good.

Has anyone used two (or more) Jaguars driving 1 wheel (or more specifically set of wheels on one side of the robot), with one wheel encoder feeding multiple Jaguars… …IN VELOCITY CONTROL MODE (phew! that’s a mouth full)?

If so, did it work well? Also did you use an I term in the PID loop (i.e. a I gain != 0)?

Finally, digging even MORE into the details, “Speed Ref <val>” command takes an unsigned 8 bit number for <val>. How do you set negative velocities ? Oh… never mind, I see that this is used to tell the Jaguar what is the speed sensor (only the encoder is supported but I guess they are imagining a day when others may be)

Joe J.

I think what Joe was saying is that the encoder is inherently a position transducer and the Jag would have to differentiate the signal to get rate… and at low speeds the signal from the encoder is not changing at 1000Hz so you can’t get a 1000Hz rate signal from it.

[edit] I guess I was reading between the lines too much [/edit]


There is that problem as well. But No, I had totally missed the velocity feedback mode.

As to counts frequency, at 256 counts per output of say a CIMpleBox and a 3:1 to an 8" wheel (1ft per rev) youll get 32 counts per inch (256*3/24). At this resolution you fall below 1000hz update at roughly 2.5fps. BUT… the code may use edges instead of periods to estimate speed (4X) if it does then we’d be pretty close the range where we cover all useful FIRST robot speeds (.6 fps seems slow enough for me)

Even without this, a slower update rate may be fine as long as we don’t go crazy with the D gain.

We’ll have to try it and see.

Joe J.

*Or you could gear up the encoder a bit I suppose so it is spinning faster. Kind of kludgy but it might work since the encoder is virtually no load. Have to be careful not to exceed signal processing capability of Jag.


I’m not particularily up on the benefits of good quality control loops as our code on my old team looked something like this:

x = Joystick.x/JOYMAX
y = Joystick.y/JOYMAX


The first version was written in 10 minutes by 1114’s Programming mentor :P. But I digress… What I wanted to make sure gets said in here is that as much as stellar control has the potential to help, a lot of the issues you are referencing are pretty blown out of proportion. e.x. A simple 6-wheel drive with reasonable weight balance will drive straight all on its own. Just wanted to make sure we remember that.

  • Alex

It has a 50 MHz Cortex M-3. Unless the PID algorithm onboard is rather complex and heavy on the cycles, how could anything from a digital encoder approach the Jag’s limits?

The Stellaris microcontroller does quadrature decoding in dedicated hardware, so there is no additional processing burden for running the encoders faster. The upper limit of that circuitry is 1/4th the speed of the processor. For a part running at 50MHz*, that is roughly 750,000 RPM for a 250 count encoder. However, I doubt you’d get clean signals from the poor encoder at those speeds!

The Stellaris has more computational horsepower available than the entire IFI control system put together several times over. It is an amazing show of what Moore’s law does to the industry.

  • Edit: I can’t remember what speed they actually run on the Jag - there simply is no need to run at the full 50 or 80 MHz. Suffice it to say that it is still overkill.

Do you know if the processor “does the right thing” in terms of speed calculations, by which I mean, does it get 4 readings per nominal encoder tick (rising edge channel A, rising edge channel B, falling edge channel A, falling edge channel B) AND create the time as rising edge to (the same) rising edge and falling edge to (again, the same) falling edge?

The reason I say this is that the encoder folks do a much better job at spacing their ticks than they do at getting exactly 50-50 ON-OFF duty cycles and spacing the phasing between the sensors A and B (remember their is only 1 encoder wheel, the 90 degree shift in the signal between the two phases is done by physically spacing the encoders N+/-(1/4) encoder pulses apart.

The reason I ask is that I want to get 4X the speed readings doing it this way provides over just a single speed reading per encoder tick which is the most obvious way to do things (see the above discussion about keeping high PID loop times at slow speeds – without 1,000Hz sensor reading, it makes little sense to have a 1,000Hz update rate on the PID loop).

Joe J.

I think you are over thinking it.

While you can get better performance by closing the loop, driving using just PWM works pretty well. All 3 robots on the winning alliance this year drove using PWM. I would highly recommend downloading 254’s code from this year, or last year, and trying the drive code that we use. It is remarkably responsive, and encourages the driving style that 254 drives with. The driver will need to correct the path of the robot as he/she is driving anyways. Closed loop control will not remove this need, and also adds in a set of corner cases that need to be carefully considered (sliding next to a wall comes to mind).

On a similar note, I believe that what you really want is to control the radius of the turn that the driver is making, like a car. This results in more consistent driving across many different velocities. This can be done with either open or closed loop control. See our code for how we do this.

FYI, I believe that the Jaguar samples at around 1 MHz. I remember getting this specification out of the manual, but it has been a while. (It debounces the signal, and the decoder runs on the peripheral clock which is probably configured at 1/4 of the speed of the main clock). If your application actually cares about this number, then I will contend that you are doing something wrong.

254’s 2 tube auto mode was done with encoders to close the loop on the wheels, and a gyro to slowly update an offset between the wheels to deal with the wheels slipping relative to each other. Driving distance was done by just using the encoders, and all turns weren’t declared “done” until both the gyro and the encoder errors were below a threshold. It worked well enough, and unless Auto mode becomes more complicated in future years, we will likely reuse some variant of that method.