View unanswered posts | View active topics It is currently Sat Apr 19, 2014 4:26 pm






Reply to topic  [ 10 posts ] 
PID controller 
Author Message
Rookie

Joined: Sun Jan 29, 2012 6:41 pm
Posts: 7
Post PID controller
Hi,

I have been trying to implement a PI controller for my robot to get it to move in a straight line. However I am having a lot of difficulty trying to find proper gains for the algorithm. Is there a certain way this is to be done? I have tweaked the proportional gains until i was meeting the setpoint and then increased the Integral gain to get it exactly at setpoint. However once i got the current setpoint being tracked I changed the setpoint to a different value and it was no longer being tracked.

Any help will be greatly appreciated.
Thanks


Sun Jan 29, 2012 6:46 pm
Profile
Expert

Joined: Wed Jun 30, 2010 7:15 am
Posts: 181
Post Re: PID controller
Does the setpoint have to change? PID controllers aren't my strongest point, but I've done a few and I think you tune it for one specific set point - if you change it, you have to re-tune. Also, you can usually keep the integral very low - I never had mine over 0.01 for anything.

But this is mostly about line following, where the robot follows an exact shade of gray, so it might be different for you.

PS: You might find this an interesting read: http://nxttime.wordpress.com/2011/01/07 ... netic-way/

_________________
leonoverweel.com


Sun Jan 29, 2012 10:38 pm
Profile WWW
Rookie

Joined: Sun Jan 29, 2012 6:41 pm
Posts: 7
Post Re: PID controller
Well yea my setpoint has to be changing since I need to change the speed depending on the distance from an object in the robot's path.
There are somethings that I was not clear about setting up. For instance, when I calculate the output of the controller, the Kp*err+ Ki*integrator*sampleRate, what do I do with that output? Do I convert that back into a PWM value and add it to the previous PWM value? This is the part I was unclear about.


Mon Jan 30, 2012 12:15 am
Profile
Expert

Joined: Wed Jun 30, 2010 7:15 am
Posts: 181
Post Re: PID controller
How is your robot set up? (Do you have a picture of it?)

It kind of depends on the way it drives.

_________________
leonoverweel.com


Mon Jan 30, 2012 12:34 am
Profile WWW
Rookie

Joined: Sun Jan 29, 2012 6:41 pm
Posts: 7
Post Re: PID controller
Do you know how the VEX squarebot 3.0's base is designed? I followed that design. So it's just basically on four wheels with one motor on either side. But the two front wheels are smaller than the back wheels.


Mon Jan 30, 2012 12:44 am
Profile
Expert

Joined: Wed Jun 30, 2010 7:15 am
Posts: 181
Post Re: PID controller
Ah. Well in that case I'm not really sure; can you post your code? Mmake sure you put
Code:
 [code] CODE HERE [/code]
brackets around it to keep it readable.

_________________
leonoverweel.com


Mon Jan 30, 2012 5:59 pm
Profile WWW
Rookie

Joined: Sun Jan 29, 2012 6:41 pm
Posts: 7
Post Re: PID controller
Code:
#pragma config(Sensor, dgtl1,  LeftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  RightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  SonarSensor,    sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  touchSensor,    sensorTouch)
#pragma config(Motor,  port2,           rightMotor,    tmotorNormal, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]          [Name]              [Type]                [Description]                     *|
|*    Motor Port 2        rightMotor          VEX 3-wire module     Right side motor                  *|
|*    Motor Port 3        leftMotor           VEX 3-wire module     Left side motor                   *|
|*    Digital Port 6      touchSensor         VEX Bumper Switch     Top mounted, facing forward.      *|
|*    Digital Port 7      distanceSensor      VEX range sensor      Front mounted                     *|
\*-----------------------------------------------------------------------------------------------4246-*/
//#include <speedToPwm.h>


//______________________________Declaration of Variables_______________________________________________


float SPH=150;                  //setpoint high limit
float SPL=0;                   //setpoint low limit
int y=0;
float leftCountb4=0;
float rightCountb4=0;
float leftCount1sec=0;
float rightCount1sec=0;
float ynLeft=0;                 //current output sample of encoder counts for left wheel
float ynRight=0;
float KpLeft=1.2;                   
float KpRight=1.2;
float KdLeft=0.1;
float KdRight=0.14;
float KiLeft=0.05;                         
float KiRight=0.03;                         
float sampleRate=0.1;           //sample time of 0.1secs
float enLeft=0;                 //current sample
float enRight=0;
float en1Left=0;                //last error (n-1)
float en1Right=0;
float outLeft=0;                //controller output
float outRight=0;
float integratorOutputLeft=0;
float integratorOutputRight=0;
float intFactorLeft=1;         //anti windup switch
float intFactorRight=1;
int leftSpeed;
int rightSpeed;
int pwmOutLeft1=0;
int pwmOutRight1=0;
int speedSetpoint=60;
int offset=0;

/*--------------------------------------------------------------
Function to measure encoder counts within a certain time
----------------------------------------------------------------*/
int encoderCountsInTime()
{
  ClearTimer(T1);
  leftCountb4 = SensorValue[LeftEncoder];        //taking encoders counts as soon as timer begins
  rightCountb4= SensorValue[RightEncoder];      // (offset)
  while(y==0)
  {
    if(time1[T1]==100)
    {
      leftCount1sec=SensorValue[LeftEncoder];       //sampling counts after 0.1 sec
      rightCount1sec= SensorValue[RightEncoder];
      y=1;
    }
  }
  rightSpeed= rightCount1sec-rightCountb4;
  leftSpeed=leftCount1sec-leftCountb4;

  return rightSpeed;                              //return speeds
  return leftSpeed;
}


/*--------------------------------------------------------------
PID code for both motors to move at same speed
----------------------------------------------------------------*/

void pid( float setPoint)
{
  //store previous errors
  en1Left=enLeft;
  en1Right=enRight;

  //Sample Output
  ynLeft= leftSpeed;      //measured speeds
  ynRight=rightSpeed;
 
  ynRight= (-1)*ynRight; //multiplying by -1 since encoder for right drive counts backwards

 
  //checking setpoint limits

  if (setPoint> SPH)
  {
    setPoint=SPH;
  }
  else
  {
    setPoint = setPoint;
  }

  if (setPoint< SPL)
  {
    setPoint=SPL;
  }
  else
  {
    setPoint=setPoint;
  }


  //Determining current error

  enLeft = setPoint - ynLeft;
  enRight= setPoint - ynRight;


  //_______________________Compensate for windup_______________________________

  if (enLeft==en1Left)                  //windup if last and current error are the same
  {
    intFactorLeft=0;                   //switch integrator term off
  }


  if (enRight==en1Right)                  //windup if last and current error are the same
  {
    intFactorRight=0;                     //switch integrator term off
  }



  //___________________Calculating controller output______________________

  integratorOutputLeft= integratorOutputLeft + (intFactorLeft*enLeft);
  integratorOutputRight=integratorOutputRight+ (intFactorRight*enRight);

  outLeft= KpLeft*enLeft + KiLeft*integratorOutputLeft*sampleRate + (KdLeft*(enLeft-en1Left)/sampleRate);
  outRight=(KpRight*enRight)+(KiRight*integratorOutputRight*sampleRate) + (KdRight*(enRight-en1Right)/sampleRate);

//Calculating new output PWM value
 
  pwmOutLeft1=pwmOutLeft1+ (outLeft/3);
  pwmOutRight1=pwmOutRight1+ (outRight/3);


  //______________________Checking output limits___________________________

  if (pwmOutLeft1> 127)
  {
    pwmOutLeft1=127;
  }
  if(pwmOutRight1>127)
  {
    pwmOutRight1=127;
  }

  if (pwmOutLeft1<0)
  {
    pwmOutLeft1=0;
  }
  if(pwmOutRight1<0)
  {
    pwmOutRight1=0;
  }

  motor[leftMotor] =-pwmOutLeft1+40;
  motor[rightMotor]=-pwmOutRight1-40;

}

//-----------------------------------------------------------------------------------


task main ()

  intFactorLeft=1;
  intFactorRight=1;
 
  motor[leftMotor]=pwmOutLeft1;
  motor[rightMotor]=pwmOutRight1;
  wait1Msec(1000);

  while(1)
  {
    wait1Msec(100);
    y=0;
    encoderCountsInTime();             //check encoder counts within 100ms
    pid(speedSetpoint);


  }
}


Mon Jan 30, 2012 8:42 pm
Profile
Rookie

Joined: Sun Jan 29, 2012 6:41 pm
Posts: 7
Post Re: PID controller
That's my code above. At the moment I am only implementing PI control so the derivative gains are zero.


Mon Jan 30, 2012 8:43 pm
Profile
Expert

Joined: Wed Jun 30, 2010 7:15 am
Posts: 181
Post Re: PID controller
I don't really see any problems with that but then again I mostly program NXT.

As for your problem, maybe you could tune it once for the lowest speed it's ever going to need to drive at and then again for the fastest speed, and then do the math to make it choose a speed somewhere in between - for example, if your P value would be 20.0 at 100% speed and 1.0 at 5% speed, it'd be 6 at 30% speed 14 at 70%. Once you figure out the relationship between the P value and your desired speed, you could turn it into an equation. You could then do the same for your integral.

I haven't tried this, but I'm just trying to brainstorm.. Hope this helps

_________________
leonoverweel.com


Mon Jan 30, 2012 9:12 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3105
Location: Rotterdam, The Netherlands
Post Re: PID controller
This tutorial will help you build a very good PID controller: [LINK].

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue Jan 31, 2012 1:29 am
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 10 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.