Encoder with PIDController Help

Technical C/C++
1

Maybe I am blind because I know all the powerhouse teams are probably using encoders on their drive system with the PIDController but I can not find a good example anywhere on how to get setup doing this.

I have a quad encoder on each of my transmissions, and have them calibrated so that they give me the proper distance when I query encoder->GetDistance(). That works great.

What I am trying to do is to connect my RobotDrive->TankDrive class in the proper place of the PIDController to get my robot to go a certain distance. I have a klugey code that powers the motor at a set speed until the encoders have reached a certain distance and then I just turn the motors off. This works, but I really want to graduate to the next level of control. PID.

I cannot find a good starting place or sample code. I assume PIDSource would be the encoder, but how do I set it up in PIDController so that it reads distance as the value I want to control? And in PIDOutput the RobotDrive would be in it somehow, but I don’t know how to get it setup to use TankDrive either.

Can someone give me some sample code or point me in the direction of some sample code? The sample code in the user’s guide doesn’t discuss RobotDrive or Encoders. Once I get it setup right I will experiment with the P,I,and D values but I don’t know how to get it setup correctly first.

2

Your understanding of PIDController seems to be correct - it takes PIDSources, crunches the numbers in a separate thread, and asynchronously writes to PIDOutputs.

What makes this tricky is that the WPILib PIDController writes its output to a PIDOutput, but RobotDrive does not implement PIDOutput. Likewise, Encoder doesn’t implement PIDSource (because it would be ambiguous to do so - do you want to measure speed or distance?).

There are several possible solutions to this problem.

First, we need to turn the encoder’s distance measurement into a PIDSource. You could re-write the WPILib Encoder class, but in general I try not to mess with a library unless I absolutely need to. Instead, how about a wrapper class?


class DistanceEncoder extends PIDSource
{
public:
  DistanceEncoder(Encoder *baseEncoder)
  {
    m_baseEncoder = baseEncoder;
  {

  double pidGet()
  {
    return m_baseEncoder->GetDistance();
  }

private:
  Encoder* m_baseEncoder;
};

Similarly, you can wrap RobotDrive in another PIDOutput class:


class RobotDriveOutput extends PIDOutput
{
public:
  RobotDriveOutput(RobotDrive* baseDrive)
  {
    m_baseDrive = baseDrive;
  }

  void pidWrite(double output)
  {
    m_baseDrive->Drive(output, 0.0);
  }

private:
  RobotDrive* m_baseDrive;
};

Then you can put it all together like you said.

*Note: there are many ways to do this; this is only one. Also, I banged out the above code from memory, so there might be syntax and/or API errors, but you should get the point.

3

First thing I’m a java programmer and I know the classes translate generally between Java and C++ but I’m not sure of it, also syntax differences apply.

you would probably want to make a wrapper class for TankDrive that implements PIDOutput. The PIDWrite method would probably look something like this

public void PIDWrite(double d){
super.TankDrive(d,0);
}

if I remember the method correctly( We didn’t use the FIRST class this year, we wrote our own drive system class) this should work. The PID loop is a P only loop, there is no need for the I and D in this instance(i.e. set them both to 0 in the code). You’d also need to make a wrapper class for the encoder to make it into a PIDSource. When you create the loop use your encoder class as the source the drive class you created as the output and try different values. One last thing is to remember to limit output to (-1.0,1.0) so you nothing bad happens to your speed controllers. One

4

Yikes, I thought for sure with 1800+ teams all wanting encoder driven robots there would have been an easier way. I have the basics of C down but everytime someone starts breaking out wrappers and stuff my brain just shuts down. I can handle the canned libraries and subroutines I write myself using old school subroutine and function practices but all these additional wrappers and classes and stuff is a different way of thinking. I will dig into the above code and see what I can do with it and see if I can understand it, but while thinking of this problem I realized another problem.

Seeing as I have both a left and a right encoder and RobotDrive controls all motors how do I keep two PIDControllers based on the different encoders from getting into a fist fight with each other? One will be telling RobotDrive to do one thing and the other another and I suspect only the last controller will actually have the power. I guess I will have to tie the two left motors controls and the two right motors controls into some sort of combination and use separate controllers for each side. Can I somehow wrap two Victors or Jaguars to be controlled by the same PIDController?

If I have to add all this code anyway instead of being able to just call a simple library function I may be better off just writing my own PID subroutine that is specific to my needs. At least then I might understand what everything is doing when it comes time to debug.

I think that I am not the only one that wants this information so thanks everyone and keep the info coming. I will continue to post my progress as I try to get this stuff working in the hopes it helps someone as much as me.

5

One way to deal with left and right encoders is as follows:


class AverageDistanceEncoder extends PIDSource
{
public:
  AverageDistanceEncoder(Encoder *leftEncoder, Encoder *rightEncoder)
  {
    m_leftEncoder = leftEncoder;
    m_rightEncoder = rightEncoder;
  {

  double pidGet()
  {
    return (m_leftEncoder->GetDistance() + m_rightEncoder->GetDistance())/2.0;
  }

private:
  Encoder* m_leftEncoder;
  Encoder* m_rightEncoder;
};

This averages the left and right readings and would then command both sides of the drive to do the same thing As long as the robot drives relatively straight on its own, that should be all you need.

(In general, it is easiest to tell your robot just to drive in straight lines and turn in place in autonomous mode - fancy arcs can be quite a bit trickier).

6

Jared’s example shows how PID controller is generally used but there is more to it. First, I assume you are doing the normal left and right motor drive instead of the mecanum 4-wheel drive so you only have left and right encoders. The PIDController class in the WPI library is generic so it only understands one input variable (PIDGet) and one output variable (PIDWrite). The general idea is:

Call SetSetpoint to set your target. It doesn’t care if the target is distance or speed or heading.

The PIDController thread will periodically call PIDGet (provided by your code) to get the current reading (e.g. distance, speed or angle), whatever it is. It calculates how much output force to apply depending on how far to the target (i.e. error). The greater the error, the greater the output force.

Therefore, the PIDController is agnostic on what the control variables are. It just calculates the difference between the set target and the current actual value and apply PID control to calculate the resulting force to apply.

With this understanding, you must provide a PIDGet that will read whatever sensor you have (whether it is a gyro, an accelerometer or multiple encoders) to give you the distance. In your case, averaging the left and right encoders is the appropriate way. You must also provide a PIDOutput to take the output force calculated by the PID controller to apply to the drive motors (that depends on your drive train: 2-wheel drive, 4-wheel drive etc.).

But that’s not the end of it. A major part of PID control is to tune the PID constants (Kp, Ki and Kd). This can be a whole new subject. Basically, if the PID controller calculated too large of the output force, it will cause ringing (e.g. the robot will drive to the target very quickly but overshoot the target and then it will attempt to reverse course to correct itself). This may go on for a number of iteratiions (back and forth a few times) before it stops within the tolerance range of the “target”. If ringing occurs, you generally need to decrease the amount of ringing by reducing Kp but you don’t want to reduce Kp too much that the robot never gets to the target (i.e. the output force is too weak). So you want to tune Kp to have a little bit ringing, then you move on to tuning Kd and possibly Ki to reduce the last bit of ringing so that the Robot will drive as fast as possible towards the target but will reduce speed as it approaches the target. If PID is tuned correctly, it will then stop dead on to the target.

I am fairly new to tuning PID. I would love to hear tips from experience teams on how they tune PID. We have two PID controller objects, one for distance PID and one for turn PID. Can we use one set of PID constants for both PID controllers? Since the weight and drive motors are the same on the robot, I have a theory that the two sets of PID constants should be fairly identical but since I did not study control theory in depth, I am not sure if that’s true.

BTW, we implemented mecanum drive on our bot this year so we have 4-wheel drive. This makes PID control with encoders very challenging. Since this is our first experience with mecanum drive train and because of other mechanical limitations, we chose not to use encoders and use the accelerometer as the “distance” sensor instead. Boy, that was a challenge. We found out the accelerometer is bad choice because we have to double integrate acceleration to get distance. Because of sensor noise, the double integration will cause error cumulation that eventaully cause the robot to accelerate out of control. But at the end, knowing the limitation of the sensor and some software workaround those issues, we have a decent distance PID control using the accelerometer.

7

Would anyone know how to make the wrapper for the PIDSource in java? I’m stumped as to the syntax.

8

Sorry, don’t know Java that well. :frowning:

This year, we have improved using PID by developing a generic PID drive library for C++. The main methods of the PID drive library are:


TrcPIDDrive::TrcPIDDrive(RobotDrive *drive,
                                  TrcPIDCtrl *pidCtrlDrive,
                                  TrcPIDCtrl *pidCtrlTurn,
                                  PIDDriveNotify *notify = NULL);
void TrcPIDDrive::SetTarget(float distSetPt,
                                       float distTolerance,
                                       float angleSetPt,
                                       float angleTolerance,
                                       bool fStopOnTarget);

So when creating the PIDDrive object, you provide two PID controller objects, one for driving straight and one for turning. You also optionally provide a notify object so that you get notified when PID drive is done.
So in autonomous, all we need to do is something like this:


// Drive forward 5 ft to within 1 inch tolerance and then stop.
pidDrive->SetTarget(60.0, 1.0, 0.0, 0.0, true);
WaitForEvents(pidDriveEvent);
 
// Turn right 90 degrees to within 1 degree tolerance and then stop.
pidDrive->SetTarget(0.0, 0.0, 90.0, 1.0, true);
WaitForEvents(pidDriveEvent);
 
...

With this library, once we have an autonomous strategy down, we can usually code it in one day.

9

Which encoders are you using? The encoders are the shaft encoders from the KOP and they’re wired into the DSC? Any chance you’d be willing to share your library?

TIA,

Mike

10

We are not using any encoders this year because we are using Mecanum wheels and our build head said the axle of the gearbox is too big to fit with the KOP encoders. But the beauty of the PID library is that it doesn’t care what sensor you use. Since this year is pretty much line following. So we have a library module for line following drive which takes one PID controller that controls the heading while following the line. This PID controller for line following uses the three light sensors as input. In fact, we are planning to translate the light sensor readings into an integer with the following possible values: -2, -1, 0, 1, 2. Zero being right on the center of the line. 2 being too far to the right. Then this input value can be used as PIDGet() to calculate the error. So we can do the equivalent of:


lineFollower->SetTarget(float setPoint);

So we will call it with 0.0, telling it to maintain a reading of 0 (center of the line).
You can access the library here:
http://proj.titanrobotics.net/hg/Frc/2011/code/file/348ca03ad30f/trclib

11

Our programming mentor was able to answer my previous question and help us do this, but in Java.


public class SuperEncoder implements PIDSource {
//Initialize the encoder
    SuperEncoder(int a, int b) {

        Encoder enc = new Encoder(a, b);

    }
//Return the distance as pidGet
    public double pidGet() {

        return enc.getDistance();

    }

}
12

Thanks! I’ll take a look.

Mike

13