WPILib PID controller object

[Phoenix Spud]

Has anyone used the PIDController object? How well did it work? We are looking at implementing the PIDController object and were wondering if anyone had any words of wisdom before we start.

Thanks!

[wt200999]

http://www.chiefdelphi.com/forums/sh...ad.php?t=71781

no example has been posted as far as I know though

[Jared Russell]

I'm not a huge fan of the design paradigm WPI used for their PIDController class (requiring you to implement the PIDSource and PIDOutput interfaces) so I rolled my own. I made it so you call a "calculate()" method synchronously inside your Teleop_Periodic() function, passing in sensor readings and getting output values as well. It just makes more sense to me.

[BradAMiller]

Here's an excerpt from the Users Guide that will publish in an update in the next day or so (the formatting will be better in the document)...

PID controllers are a powerful and widely used implementation of closed loop control. The PIDController class allows for a PID control loop to be created easily and runs the control loop in a separate thread at consistent intervals. The PIDController automatically checks a PIDSource for feedback and writes to a PIDOutput every loop. Sensors suitable for use with PIDController in WPILib are already subclasses of PIDSource. Additional sensors and custom feedback methods are supported through creating new subclasses of PIDSource. Jaguars and Victors are already configured as subclasses of PIDOutput, and custom outputs may also be created by sub-classing PIDOutput.
The following example shows how to create a PIDController to set the position of a turret to a position related to the x-axis on a joystick using a single motor on a Jaguar and a potentiometer for angle feedback. As the joystick X value changes, the motor should drive to a position related to that new value. The PIDController class will ensure that the motion is smooth and stops at the right point.
A potentiometer that turns with the turret will provide feedback of the turret angle. The potentiometer is connected to an analog input and will return values ranging from 0-5V from full clockwise to full counterclockwise motion of the turret. The joystick X-axis returns values from -1.0 to 1.0 for full left to full right. We need to scale the joystick values to match the 0-5V values from the potentiometer. This can be done with the following expression:

(turretStick.GetX() + 1.0) * 2.5

The scaled value can then be used to change the setpoint of the control loop as the joystick is moved.
The 0.1, 0.001, and 0.0 values are the Proportional, Integral, and Differential coefficients respectively. The AnalogChannel object is already a subclass of PIDSource and returns the voltage as the control value and the Jaguar object is a subclass of PIDOutput.

	Joystick turretStick(1);
	Jaguar turretMotor(1);
	AnalogChannel turretPot(1);
	PIDController turretControl(0.1, 0.001, 0.0, &turretPot, &turretMotor);
	
	turretControl.Enable();  // start calculating PIDOutput values

	while(IsOperator())
   {
		turretControl.SetSetpoint((turretStick.GetX() + 1.0) * 2.5);
		Wait(.02);		// wait for new joystick values
	}

The PIDController object will automatically (in the background):
• Read the PIDSource object, in this case the turretPot analog input
• Compute the new result value
• Set the PIDOutput object, in this case the turretMotor
This will be repeated periodically in the background by the PIDController. The default repeat rate is 50ms although this can be changed by adding a parameter with the time to the end of the PIDController argument list. See the reference document for details.

[Kruuzr]

Hi Brad...

This approach works great if your source is already pre-defined to be a PID source, but what about the situation ( such as an autotracking turret) where the source value will be an X offset from the middle of the image? Do we have to derive a new PIDSource object and implement any/all needed functions? The effort is compounded by the fact that the image offset is a relative number and would be hard to relate to an absolute turret positioning value.

Steve C

[sircedric4]

Originally Posted by Kruuzr

Hi Brad...

This approach works great if your source is already pre-defined to be a PID source, but what about the situation ( such as an autotracking turret) where the source value will be an X offset from the middle of the image? Do we have to derive a new PIDSource object and implement any/all needed functions? The effort is compounded by the fact that the image offset is a relative number and would be hard to relate to an absolute turret positioning value.

Steve C

I guess Steve was ahead of the game by a week or so. Did you get your question answered because I have the same one now? How do I make the X position from center of the camera a new PIDSource so that I can then PID my turret to the center of the image?

I am using the variable name horizontalDestination = par1.centered_mass_x_normalized which gives me a nice -1 to 1 for the position of the target on the camera from centered. How do I make horizontalDestination a PIDSource so that the PIDController will drive the turret motor until horizontalDestination = 0?

[Kruuzr]

We've just coded this last night and hope to try it tonight. You need to derive your own class from the PIDSource class. In PIDSource.h, notice that there is one method defined:

virtual double PIDGet() = 0;

This is what's known as a pure virtual method; There is no default implementation This means you can't create a PIDSource object, only one derived from it with this method overridden with your own. The code looks something like this:

// .h file
#include <WPILib.h>
class MyPIDSource : public PIDSource
{
public:

double PIDGet();
}

// .cpp file
#include "MyPIDSource.h"

double MyPIDSource:IDGet()
{
double pidSourceValue;

// calculate 'double' value that you want compared to the set point
// feel free to format it the same as you expect with your set point
pidSourceValue = ...

// return value
return pidSourceValue;
}

Before you create your controller object, you must create an instance of your class. Pass this pointer as a parameter to PIDController for the PIDSource arg.

Now, every 50 ms (assuming you've started it going), the PIDController will read the PID source, compare the value to the set point, do it's PID calculations, and write the new value to the PID output device.

With our setup, we are turning a turret that has a camera mounted on it to track the vision target. My PID Source uses the X component of the Particle report to get the error value. So our set point is permanently set to 0 (centered in the camera). But we have to take into account the position of the turret with respect to our two end stops. As we get closer to one of our stops (as read from our 10 turn pot attached to the turret gears) we have to supply a smaller number until it gets to 0 at the end, assuming we're driving it in that direction. We can also disable our 'auto tracking' by just returning a 0, and allow manual control by just passing back the z component of a joystick to represent the value. It's actually quite powerful.

I'll let you know how it works.

Hope this helped...

Steve

[sircedric4]

Originally Posted by Kruuzr

We've just coded this last night and hope to try it tonight. You need to derive your own class from the PIDSource class. In PIDSource.h, notice that there is one method defined:

virtual double PIDGet() = 0;

This is what's known as a pure virtual method; There is no default implementation This means you can't create a PIDSource object, only one derived from it with this method overridden with your own. The code looks something like this:

// .h file
#include <WPILib.h>
class MyPIDSource : public PIDSource
{
public:

double PIDGet();
}

// .cpp file
#include "MyPIDSource.h"

double MyPIDSource:IDGet()
{
double pidSourceValue;

// calculate 'double' value that you want compared to the set point
// feel free to format it the same as you expect with your set point
pidSourceValue = ...

// return value
return pidSourceValue;
}

Before you create your controller object, you must create an instance of your class. Pass this pointer as a parameter to PIDController for the PIDSource arg.

Now, every 50 ms (assuming you've started it going), the PIDController will read the PID source, compare the value to the set point, do it's PID calculations, and write the new value to the PID output device.

With our setup, we are turning a turret that has a camera mounted on it to track the vision target. My PID Source uses the X component of the Particle report to get the error value. So our set point is permanently set to 0 (centered in the camera). But we have to take into account the position of the turret with respect to our two end stops. As we get closer to one of our stops (as read from our 10 turn pot attached to the turret gears) we have to supply a smaller number until it gets to 0 at the end, assuming we're driving it in that direction. We can also disable our 'auto tracking' by just returning a 0, and allow manual control by just passing back the z component of a joystick to represent the value. It's actually quite powerful.

I'll let you know how it works.

Hope this helped...

Steve

I have coded something up as well, but won't get a chance to test it till tonight either. I think this will work, but if any code guru's see something wrong let me know. I am also using a 10 turn pot on a turret gear and want to do exactly what you are talking about.

----------------------------------------
I have a CamSource.h which is:
----------------------------------------
#include "WPILib.h"

class CamSource : public PIDSource
{
public:
CamSource();

double PIDGet();
void SetSource(double);
private:
double source;

};

------------------------------------------------
My CamSource.cpp file is:
----------------------------------------
#include "CamSource.h"

CamSource :: CamSource()
{}

void CamSource :: SetSource (double sourcedum)
{
source = sourcedum;
}

double CamSource :: PIDGet()
{
return source;
}

----------------------------------------
Then in my main code I do the following to use it. Basically I set the CamSource to what the X difference on the camera is, and then feed it to my PIDController.

RobotCode.h:
----------------------------------------
PIDController *turretCamControl;
CamSource *CameraBrains;

RobotCode.cpp
----------------------------------------
CameraBrains = new CamSource();
turretCamControl = new PIDController(CamP, CamI, CamD, CameraBrains, TurretPIDMotor);

Then in my teleoperated periodic:
turretCamControl->Enable();
CameraBrains->SetSource(horizontalDestination);
turretCamControl->SetSetpoint(0);

-----------------------------------------
I think it will work, at least it compiles fine. horizontalDestination is the normalized X parameter from the camera. Thanks for your help, what you have outlined is pretty much the route I went as well. I don't know why, WPI couldn't just write a controller that allows us to just input a dang variable. It would be much easier.

[sircedric4]

All that code I posted above seemed to work fine. I haven't got the camera on the actual robot yet, that'll be tonight or tomorrow but the motor I had it on separately seemed to track correctly with the camera as the PIDSource.

Thanks again for the help.

[Kruuzr]

Glad to hear you have some initial success. What I noticed (and you may have accounted for this in your real code) is that it does not take into account any stops you may have. Can the turret turn 360 deg? We have two stops and a pot tied to the gears to give us an absolute position. We have to use that info to slow down/stop near the ends.

Steve C.

[sircedric4]

Originally Posted by Kruuzr

Glad to hear you have some initial success. What I noticed (and you may have accounted for this in your real code) is that it does not take into account any stops you may have. Can the turret turn 360 deg? We have two stops and a pot tied to the gears to give us an absolute position. We have to use that info to slow down/stop near the ends.

Steve C.

We have limit switches to stop our turret when it gets to either end along with hard stops. One thing I have noticed is that the way the code works on the machine is different this year it seems. I was playing around with another motor on limit switches today and it seems that the second you Set the Jaguar class the command is sent. I was handling limit switches with blanket if-then statements at the end of the teleop periodic that set the motors to zero if it would drive past the limit switches. What happened in real life was that my motors would "jitter" when the limit switch was depressed instead of just stopping.

I troubleshot this for a few hours before removing the various Set Jaguar statements throughout my code. Now I have a variable that I modify throughout the code, and only sent the actual Set statement once. I was used to nothing actually being commanded till the end of each loop and so I was used to being able to modify stuff after I had set it once earlier, but it looks like I can't get away with this now. I have no idea how this is gonna work with myPIDControllers yet but it doesn't bode well. Right now I am trying to figure out how I am gonna make sure the turret doesn't "jitter" when it gets to the limit switches. I am still using blanket if-thens at the end of my teleop loop for the turret until I can figure out some way to control my PID loop.

[Kruuzr]

We have our PIDController now up and running with either the z axis of a joystick or the x offset of a camera (same ranges) to give it its input (PIDSource). Last night, we had the turret tracking the ball pretty well. Time to tweak the PID constants

If you don't have a turret position sensor device of some sort, you can still incorporate the limit switches into the PID controller setup. In your PIDSource object, check the limit switch for the direction you are turning toward. If closed, return a 0.0 to stop the motor. If you're still getting some 'jitter', you can set a flag for the direction you were travelling to keep the output 0 until you start trying to track toward the other direction. The problem with just limit switches is that you can't start slowing down before the end stops.

One more thing that just struck me... If you're trying to use Set statements for the same Jaguar that the PIDController is using, you will be fighting it. The PID loop runs every 50ms. You can set it to 0, but the PIDController will shortly overwrite it.

Steve C.

[Mr. Lim]

Originally Posted by sircedric4

void CamSource :: SetSource (double sourcedum)
{
	source = sourcedum;
}

double CamSource :: PIDGet()
{
	return source;
}

We've done pretty much exactly the same thing as you did here. I think we're running into a small concurrency problem though, and would like to hear from you as well.

As once in a blue moon your camera "spaz" while it is tracking?

Very rarely, but often enough to make it a pain, our camera tracker will move out of position quickly then recover for no apparent reason.

I think it's because of a classic concurrency issue where source is being read and written at the same time.

The PID controller runs in a separate task, and so potentially both SetSource and PIDGet could be called at the same time.

Would any more experienced programmers out there recommend locks? And if so, how would you implement them in the quoted code?

[Kruuzr]

Originally Posted by SlimBoJones

We've done pretty much exactly the same thing as you did here. I think we're running into a small concurrency problem though, and would like to hear from you as well.

As once in a blue moon your camera "spaz" while it is tracking?

Very rarely, but often enough to make it a pain, our camera tracker will move out of position quickly then recover for no apparent reason.

I think it's because of a classic concurrency issue where source is being read and written at the same time.

The PID controller runs in a separate task, and so potentially both SetSource and PIDGet could be called at the same time.

Would any more experienced programmers out there recommend locks? And if so, how would you implement them in the quoted code?

If your code in SetSource consists of just an assignment statement, it's probably not a concurrency issue. Although it's poor style to not protect it with a semaphore, a simple assignment of an int or float is usually what's called an 'atomic' operation, which means that it would not be interrupted in the middle by a task switch. (It happens in one machine language operation) I would look elsewhere for the cause.
If you have more than one line in SetSource (or if you want to get used to how to do multithreaded programming) you need to protect the function with a semaphore. The 'C programming guide for FRC.pdf' page 78 discusses how to do this. It's not hard, just a matter of adding a line at the beginning and end of your function.

Steve C

[Daniel Jones]

The update posted last night has many of the sensors in WPILib defined as PIDSources, and Victors and Jaguars as PIDOutputs. Make sure to re-image the cRIO with the latest image before using the update.