PID Speed Control/Motor Synchronizing

VEX V5 General Discussion Technical Discussion
1

Hello to all you smart people out there. I have been trying to figure out PID control and think I have an OK understanding of it, however cannot seem to figure out how to use it in ROBOTC for anything because I get stuck needing an Integral function. However ROBOTC does not support calculus functions, so how do I go about working around this? Or am I missing something stupid? I have seen a few people saying that they have PID speed control and are wondering how they are accomplishing this. It would be super helpful for someone to post an example of their code that is explained or even just for me to analyze.

Thank you in advance for any help you can give me! :slight_smile:

~Jordan

2

Here: PID Controller For Lego Mindstorms Robots
This is an awesome tutorial I found about PID (in simplified form). I’ve also used ROBOTC with it - it doesn’t require calculus. I’ve used the PID control it describes a couple of times, and it works really well (but just needs patient tuning).

Hope this helps! :slight_smile:

Andrew

3

Thank you!!! I have to run now but I will read this ASAP! I glanced over it and it looks really helpful, thank you so much!

~Jordan

4

i read it over and it was REALLY helpful!
its too late to incorporate into my CURRENT robot, but for next year, its a MUST!

btw i think the tutorial title is ironic :stuck_out_tongue:

5

You’re welcome! :slight_smile:

Andrew

6

I agree it’s a little too late for this year but definitely something I’ll have everywhere in my code next year, I’m sure!

~Jordan

7

Ok here’s PID:

**Q. First, what is this PID thing and what does it do? **

A. PID is a control loop, meaning it helps you take a given value like current_temperature and get it to a desired value, say desired_temperature.

**Q. OK, how do I do it. **

A. PID has two parts, the code (which is easy) and the tuning (which can be harder).

First lets go over the code. If you Google the subject you will find a crap-ton of complicated looking math, welcome to calculus. Fortunately you don’t need to know calculus to use a PID loop but you should know that calculus is at work here.

For now, know that PID is a acronym for Proportional Integral Derivative.

It has three parts, namely the Proportional, the Integral and the Derivative, super complicated right?

The proportional is simple, it’s just your value. Essentially here, heres a number.

The integral is a sum.

And finally the derivative is a rate of change.

So what is PID? Well PID combines these functions with the use of a variable called error.

Error is clearly defined as desired value minus the actual value. Our PID loop (when tuned correctly) will fight to keep error close to zero.

Another important part of this control loop is time. Actually more specifically it is the interval between cycles, but for simplicities sake we’ll just call it time.

And finally we need a variable to hold the last calculated error, lets call this oldError.

So heres the loop:

http://polynomic3d.com/user/smith/pid.jpg

Text version here.

As for tuning, several methods exist. I haven’t yet experimented enough to find my favorite, but I will soon enough. For now I suggest reading into the subject of PID tuning.

I hope this helps you guys wrap your head around the concept. Just note that the code provided here only works with ROBOTC because only ROBOTC supports tasking.

Also, this code uses integers but I would recommend using floats. This was code that we wrote for Clean Sweep but never ended up using. You’ll want the added accuracy of floats for this.

Oink!
-Cody

8

Thank you, Cody! With the help of Andrew’s tutorial link I was able to write something similar to what you posted. :slight_smile: Now onto the fun part… synchronization along with ramp-up and ramp-down. I’m not so sure how well the PID syncing will work along with changing motor speeds… although I am still trying it. I’d be happy to post my progress with this if others would like to assist me, as many times my brain has issues and does not see stupid errors, or I’m simply not smart enough to think of a solution.

~Jordan

9

I’d love to help! (but I’ve never used PID for driving straight :o, just for line following and holding an arm in position…)

I’m curious as to how you’re going to find the “error” - so far I’ve thought of one way (I’m sure there’s more…): Sync one motor to the other motor by setting the target value of the left encoder to the current value of the right encoder, or vice versa. However, this would still allow for some drift (if the left motor was held still, the right motor would keep going…)
This assumes tank drive, not omni.

I have used PID with variable power levels controlling the arm in this robot. (BTW, I’m no longer using this robot - a claw was better for autonomous…) The PID worked well, so I think ramping up/down wont be a problem.

10

Andrew,

Yes I pretty much have what you explained. So here’s what I have so far:

#pragma config(Sensor, dgtl1,  SensorEncoderDriveLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  SensorEncoderDriveRight, sensorQuadEncoder)
#pragma config(Motor,  port1,           MotorDriveLeft1, tmotorNormal, openLoop)
#pragma config(Motor,  port2,           MotorDriveLeft2, tmotorNormal, openLoop)
#pragma config(Motor,  port9,           MotorDriveRight2, tmotorNormal, openLoop)
#pragma config(Motor,  port10,          MotorDriveRight1, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

const int Left = 1;
const int Right = 2;
int MasterSide = Left;

int MasterSideGivenPower = 0;
int SyncedTurnRatio = 0;

const int SensorEncoderDriveL = 1;
const int SensorEncoderDriveR = 2;
const int SensorEncoderDriveBoth = 3;
const int SensorTouchL = 4;
const int SensorTouchR = 5;
const int SensorTouchBoth = 6;
const int Time = 7;
int EndSensor = SensorEncoderDriveBoth;

int SensorEndValue = 0;

float DeltaTime = 0;
float CurrentTime = 0;

float M_Distance = 0;
float M_TargetSpeed = MasterSideGivenPower;
float M_CurrentSpeed = 0;
float M_LastError = 0;
float M_CurrentError = 0;
float M_Integral = 0;
float M_Derivative = 0;
float M_PGain = 1.0;
float M_IGain = 1.0;
float M_DGain = 1.0;
float M_POut = 0;
float M_IOut = 0;
float M_DOut = 0;
float MasterSidePower = 0;

float S_Distance = 0;
float S_CurrentSpeed = 0;
float S_LastError = 0;
float S_CurrentError = 0;
float S_Integral = 0;
float S_Derivative = 0;
float S_PGain = 1.0;
float S_IGain = 1.0;
float S_DGain = 1.0;
float S_POut = 0;
float S_IOut = 0;
float S_DOut = 0;
float SlaveSidePower = 0;

bool StopDrive = false;

void Drive(MasterSide, MasterSideGivenPower, SyncedTurnRatio, EndSensor, SensorEndValue)
{
  int TurnRatio = SyncedTurnRatio/100;
  
  SensorValue[SensorEncoderDriveLeft] = 0;
  time1[T1] = 0;
  
  while(StopDrive == false)
  {
    //////////////*PID control timers and sensors*//////////////
    /**/DeltaTime = time1[T1] - CurrentTime;                /**/
    /**/CurrentTime = time1[T1];                            /**/
    /**/                                                    /**/
    /**/if(MasterSide == Left)                              /**/
    /**/{                                                   /**/
    /**/  M_Distance = SensorValue[SensorEncoderDriveLeft]; /**/
    /**/  S_Distance = SensorValue[SensorEncoderDriveRight];/**/
    /**/}                                                   /**/
    /**/else                                                /**/
    /**/{                                                   /**/
    /**/  M_Distance = SensorValue[SensorEncoderDriveRight];/**/
    /**/  S_Distance = SensorValue[SensorEncoderDriveLeft]; /**/
    /**/}                                                   /**/
    /**/                                                    /**/
    /**/M_CurrentSpeed = M_Distance/CurrentTime;            /**/
    /**/M_LastError = M_CurrentError;                       /**/
    ////////////////////////////////////////////////////////////
    
    if(M_Distance > (SensorEndValue - 300)) //If it is time for the robot to ramp down...
    {
      //Ramp down... somehow. Possibly by setting more gradual P, I and D-Gains? Then I guess you'd have to somehow change the target to be the desired distance the robot is supposed to travel for...
    }
    else if(M_Distance < 150) //Otherwise if it is time for the robot to ramp up...
    {
      //Pretty much the same dilemma, I guess...
    }
    
    ////////////*PID control for master drive side speed*////////////
    /**/M_CurrentError = M_TargetSpeed - M_CurrentSpeed;         /**/
    /**/M_Integral = M_Integral + (M_CurrentError*DeltaTime);    /**/
    /**/M_Derivative = (M_CurrentError - M_LastError)/DeltaTime; /**/
    /**/                                                         /**/
    /**/M_POut = M_PGain*M_CurrentError;                         /**/
    /**/M_IOut = M_IGain*M_Integral;                             /**/
    /**/M_DOut = M_DGain*M_Derivative;                           /**/
    /**/                                                         /**/
    /**/MasterSidePower = M_POut + M_IOut + M_DOut;              /**/
    /////////////////////////////////////////////////////////////////
    
    //////////*PID control for slave drive side synchronization*//////////
    /**/S_CurrentSpeed = S_Distance/CurrentTime;                      /**/
    /**/S_LastError = S_CurrentError;                                 /**/
    /**/                                                              /**/
    /**/S_CurrentError = (M_CurrentSpeed*TurnRatio) - S_CurrentSpeed; /**/
    /**/S_Integral = S_Integral + (S_CurrentError*DeltaTime);         /**/
    /**/S_Derivative = (S_CurrentError - S_LastError)/DeltaTime;      /**/
    /**/                                                              /**/
    /**/S_POut = S_PGain*S_CurrentError;                              /**/
    /**/S_IOut = S_IGain*S_Integral;                                  /**/
    /**/S_DOut = S_DGain*S_Derivative;                                /**/
    /**/                                                              /**/
    /**/SlaveSidePower = (S_POut + S_IOut + S_DOut);                  /**/
    //////////////////////////////////////////////////////////////////////
    
    if(SlaveSidePower >= 127)
    {
      /////////*PID control for master drive side synchronization*/////////
      /**/M_CurrentSpeed = M_Distance/CurrentTime;                     /**/
      /**/M_LastError = M_CurrentError;                                /**/
      /**/                                                             /**/
      /**/M_CurrentError = (S_CurrentSpeed/TurnRatio) - M_CurrentSpeed;/**/
      /**/M_Integral = M_Integral + (M_CurrentError*DeltaTime);        /**/
      /**/M_Derivative = (M_CurrentError - M_LastError)/DeltaTime;     /**/
      /**/                                                             /**/
      /**/M_POut = S_PGain*M_CurrentError;                             /**/
      /**/M_IOut = S_IGain*M_Integral;                                 /**/
      /**/M_DOut = S_DGain*M_Derivative;                               /**/
      /**/                                                             /**/
      /**/MasterSidePower = (M_POut + M_IOut + M_DOut);                /**/
      /////////////////////////////////////////////////////////////////////
    }
    
    if(MasterSide == Left)
    {
      motor[MotorDriveLeft1] = MasterSidePower;
      motor[MotorDriveLeft2] = MasterSidePower;
      
      motor[MotorDriveRight1] = SlaveSidePower;
      motor[MotorDriveRight2] = SlaveSidePower;
    }
    else
    {
      motor[MotorDriveRight1] = MasterSidePower;
      motor[MotorDriveRight2] = MasterSidePower;
      
      motor[MotorDriveLeft1] = SlaveSidePower;
      motor[MotorDriveLeft2] = SlaveSidePower;
    }
    
    if(EndSensor == SensorEncoderDriveL)
    {
      if(abs(SensorValue[SensorEncoderDriveLeft]) > SensorEndValue)
      {
        StopDrive = true;
      }
    }
    else if(EndSensor == SensorEncoderDriveR)
    {
      if(abs(SensorValue[SensorEncoderDriveRight]) > SensorEndValue)
      {
        StopDrive = true;
      }
    }
    else if(EndSensor == SensorEncoderDriveBoth)
    {
      if((abs(SensorValue[SensorEncoderDriveLeft]) + abs(SensorValue[SensorEncoderDriveRight]))/2 > SensorEndValue)
      {
        StopDrive = true;
      }
    }
    /*else if(EndSensor == SensorTouchL)
    {
      if(SensorValue[SensorTouchLeft] == SensorEndValue)
      {
        StopDrive = true;
      }
    }
    else if(EndSensor == SensorTouchR)
    {
      if(SensorValue[SensorTouchRight] == SensorEndValue)
      {
        StopDrive = true;
      }
    }
    else if(EndSensor == SensorTouchBoth)
    {
      if((SensorValue[SensorTouchLeft] + SensorValue[SensorTouchRight])/2 == SensorEndValue)
      {
        StopDrive = true;
      }
    }*/
    else if(EndSensor == Time)
    {
      if(time1[T1] > SensorEndValue)
      {
        StopDrive = true;
      }
    }
  }
}

Sorry for the probably very annoying commented sections I have, they were mainly to help me see the different sections for editing… Also sorry for no comments yet, really…

~Jordan

P.S. Also when fooling around with ROBOTC I found you can type in the following:

nMotorPIDSpeedCtrl[port1] = mtrEncoderReg;
nMotorPIDSpeedCtrl[port1] = mtrNoReg;
nMotorPIDSpeedCtrl[port1] = mtrSpeedReg;
nMotorPIDSpeedCtrl[port1] = mtrSyncRegMaster;
nMotorPIDSpeedCtrl[port1] = mtrSyncRegSlave;

and they are all accepted by the compiler. You used to be able to assign an encoder to a motor in the “Motors and Sensors Setup” window, but you no longer can.

~Jordan

11

Helpful hint: do your calculus before-hand, and then use the end equations. It makes things so much more simpler…

12

Jordan,

One of my pet peeves is that I don’t think you are (so far) using a PID control loop. See here https://vexforum.com/showpost.php?p=163753&postcount=20

Based on a quick scan of your posted code, I think you are creating a feedback loop that measures distance (encoder counts) and feeds that measurement back to the code that decides command to send to the motor. The command sent to the motor is changed by an amount proportional to the distance.

a) I think this will be adequate and that fooling around computing the derivative or integral of the encoder measurements will introduce unnecessary complication and instability into your robot’s behavior.

b) What you posted so far would be a “P” feedback controller, not a “PID” feedback controller. PID controllers are a specific type of feedback loop. Feedback loops are not types of PID controllers.

Think carefully about whether you need to mingle the full drive-straight feedback loop routine with logic to also adjust motor speed commands based on the total distance your robot has traveled or if you can do it more simply (with a trivial calculation outside of the loop that helps you drive straight).

The same advice/caution applies to the adjusting the motor commands based on the motors’ velocities.

These are the I and D parts of the PID acronym.

Bottom Line, just because someone long ago fell in love with the PID acronym and now many STEM robotics students have it converted it into a substitute for the more fundamental term “Feedback controller”, doesn’t mean that all feedback controllers must or should be PID style controllers.

Calling all control loops “PID controllers” is akin to “Graciously accepting”. It points to a lack of understanding of the fundamental principles in play.

Finally, if you do choose to go the whole hog route, I sincerely wish you good luck tuning it to operate correctly. I haven’t studied enough control theory to help without studying/learning more first. VamFun might be a good resource in that situation. Try adjusting the most important dimension first and then using the other two just to smooth out any odd behavior that is left over.

Blake

13

Hmm… then I guess the tutorial I linked to is not really teaching PID… sorry! :o

However, I will say that it creates a very nice feedback loop (whatever kind it is).

Andrew

14

Haha, yes I saw your post there earlier. And I admit I don’t know enough about PID control loops, as I just started learning a couple days ago, really. :o

Okay yes, I changed my code quite a bit to separate the different parts of the drive function, so that those parts are tasks running constantly, and rely on the Drive function to give them specific details.

I’ve read a little over the tuning of a PID control loop, however I haven’t yet had the time to try out tuning, as I haven’t tested any of this code on my robot (1. It isn’t finished yet, and 2. I don’t much like the idea of changing all my code at this point in time. :P) but I’ll try to use your advice when I do!

Thanks so much for your post! :slight_smile:

~Jordan

15

So I’m guessing that as it is only a “P-loop” that it truly is impossible to do PID control in ROBOTC? (As you cannot do calculus? And both the I and D portions (from what I saw, at least) need calculus?)

~Jordan

16

I posted working ROBOTC code for PID!

Comon guys?

-Cody

17

Okay, so your code does do PID control?

If it indeed does… why is mine different, and only doing a P-loop control?

~Jordan

18

My code is a full PID, it has a proportional, an integral and a derivative.

Correct me if I’m wrong, but as far as I know it’s the whole package.

-Cody

19

As does mine…

Unless I’m somehow missing something. My code started out very much like Cody’s.

~Jordan

20

Hello all, I have updated my code and believe it is very nearly finished. (Meaning ready to be tested, not complete. :P) I have attached the code. (Sorry, it was much too long for a post – I reached the character limit…)

~Jordan
VEX Forum ROBOTC PID.c (11.6 KB)