Speed PID loop


So i am trying to program a PID control loop so my motors will reach a certain speed more accurately. How it works is on our shooter, we have two axles with wheels around them that spin and shoot the ball. These axles are spun by 2 motors coupled together on each axle. So I have in total 4 motors that need to be powered, but each pair will be powered the same.

My problem is, is I don’t know alot about PID loops or how to make one to control speed. Could someone please explain in depth how to make one and if anyone has any source code they would be willing to share, it would be greatly appreciated.


First, read some background material: this year’s discussion [1], past year’s [2] and PID tuning [3]. You can ask other questions here if you need help understanding.

The general formula for PID velocity control is in [2] under the Jared341 post. In WPILIB, you need to copy PIDController.java as PIDVelocityController.java, and make some changes to the calculate() method (lines 141-171). You can rebuild this into your WPILIB or add it into your robot code, whichever you are more comfortable with. WPILIB comes with a Netbeans project to open/change/build your updated classes.jar used to link with your robot code.

Your output will become a sum of the previous output and the new calculated change. This isn’t too difficult of a change, so would be good programming practice for you.

Modifying the PIDController/PIDVelocityController to use two motor outputs is also not too difficult of a code change. Need another constructor to accept 2 motor inputs, add a new internal variable for it, and then you have an additional pidOutput.pidWrite() call near line 171 in PIDController.java. You may need to set the output as -1 * result if your motors are not physically oriented in the same direction to avoid the motors rotating in the opposite direction one another.

[1] http://www.chiefdelphi.com/forums/showthread.php?t=101244

[2] http://www.chiefdelphi.com/forums/showthread.php?t=62888

[3] http://www.chiefdelphi.com/forums/showthread.php?p=1107680#post1107680

One thing that I don’t see in your post and that TravSatEE did not mention is that you probably need to add encoders to each pair of wheels. PID loops are closed loop systems where some device measures the error and then the PID values estimate how best to change the drive signal to achieve the desired results. For position control we typically use potentiometers for the feedback device because they have mechanical memory of where “zero” is. For velocity we typically use encoders (can also sense direction) or simpler optical sensors. Encoders mount on a shaft, preferably the same one the wheels are on. Since this may not be possible with your system and because you probably don’t need to run both directions you could make a simple encoder by using the spokes of the wheel and optical reflective sensor and counting how many spokes per second you see.

My team considered that last part for a rough estimate of RPM. But I think you would need a custom counter circuit, or you’d be trying to poll your sensor constantly and hope you don’t miss a changed state.

You don’t need to poll the sensor. The FPGA can do the counting for you in hardware. Then your software just asks the FPGA for the count when it needs it.

Yes, the output of the optical sensor would need to be a digital output. The digital sidecar would not be able to tell if the signal came from a $2 optical sensor or a $35 encoder.

Thanks for all your replys.

Does, any one have any code for this that they already did? Because I looked over the three links and I still am having trouble understanding what to do,

The previous post [2] reference has Equation (1):

output += Kpe_vel + Kie_vel_sum + Kd*e_vel_delta;

PIDController.java line 159 has:

m_result = (m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError));

You want this line 159 to look more like the Equation (1). I’m not talking about changing the variable names, just that defining m_result becomes a += assignment instead of merely the = assignment.

This is the difference to the PID velocity controller and PID position controller. Note that your constants Kp, Ki, and Kd will be different compared to a position PID controller. To experiment, Set Ki, Kd to zero and find a good Kp that gets you “close enough” to your desired speeds. Then add in a Ki to see the result converge. You may want to find a way to output the encoder speed to verify your results.

Remember to make this change in a copy of the PIDController.java and not the original file itself. You can change some of the javadoc comments to reflect your work and post it to the WPILIB developers to get it included in the official WPILIB.

I would suggest you look through the lines of code 141-156 to see how it implements a PID controller. Looking at an equation is one thing, implementing in code is another.

Everyone please read the references again and follow up with questions. Copy quotes that confuse you to ask for clarification. I am seeing a lot of teams struggling with PID controllers, so the more you ask the more it will also help others.

Thanks gpetilli for pointing out the need for an encoder for the PID controller.

So I fixed the equation in the code, can I implement it like a normal pid loop now?

You can implement it using an Encoder that has setPIDSourceParameter(kRate); That is to say the encoder has to output its rate, not its distance (or position).

Also remember changing/adding files in the WPILIB requires you to rebuild the classes.jar, otherwise your change is not reflected.

do I need to rebuild the wpilib if I added the code to my robot code instead of the wpilib

Adding it to your robot means no need to rebuild the WPILIB. Your robot’s Netbeans project will compile it when it builds your robot code.