View unanswered posts | View active topics It is currently Wed Dec 17, 2014 9:12 pm






Reply to topic  [ 3 posts ] 
ready industrial PID for line follower 
Author Message
Rookie

Joined: Sun Mar 22, 2009 5:19 am
Posts: 6
Post ready industrial PID for line follower
mayby someone finds it usefull ...

I also added dynamic antywindup mechanism for integral action, ie:

when derivative of error is smaller than "0" (errror is being reduced) - integral action is also reduced - this helps the robot not to cross the center of black line
ki_1 defines How fast (in secs) the integration is reduced.

Code:
|     ---------                    ---- I_limit -----------
|    /      ki_1  \               /
|  / ki                \         / ki
|/                        \     /                     ki < ki_1
|                            \ /
|-----------
|                \
|                  \
|                    \
|                      \  d_err<0
|                        \
|                          \
|                            \--------------------error
|
|____________________________________________

Derivative factor of PID is defined with peak height (kd) and pulse width (TM_LAG), when TM_LAG is = 0 then it looks like Dirac pulse.
Code:
|                   / -
|                 /      \
|                /           \
|              /               \
|             /                  \
|---------         < -TM+LAG->     \-------- D
|
|
|            --------------------- error
|           / 
|-----------/
|__________________________________________


PID is being put into separate task ... reasons are explained in program comments.

-----------------------------------------------
Code:
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Motor, motorB, , tmotorNormal, PIDControl)
#pragma config(Motor, motorC, , tmotorNormal, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

float Time=0;
float Sum_err = 0;
float Sum_err_End = 0;
float err = 0;
float d_err = 0; //error derivative
float last_err=0;
float d_val_0= 0;
float d_val_1= 0;
float P = 0;
float I = 0;
float D = 0;
float pid = 0;
float I_old = 0;
float rest_I = 0;
float delta_I=0;

const int sp_hist=20; //set point hystersa
const float Sample_T=10; //sampling time, also inflences Sum_err_End used for efficiency estimation

bool test = false;
bool test_passed = false;
int sp = 500; //set point for error calculation

float TM_LAG=1.0; // [sec] variables for impulse height and witdh definition
float sRueck=0;
float sRestDiff=0;
float D_anteil=0;
float RueckDiff=0;
float RueckAlt=0;

const int pid_limit = 60;
const float kp = 0.85; //1.5
const float ki = 0.12; // in [sec], integral part of pid
const float ki_1 = 0.12; // in [sec], kind od antywindup: if I reaches i_limit & error decreases, I action is dynamicaly reduced, ki_1 is the integral time in [sec] for reduction
const float kd = 0.3; // derivative
int BC_power = 28; // motors' power, for original Lego test pad (simple black cirle) BC_power might be 50-70%
const float BC_const = 1; // reduce the opposite wheel's power for BC_const fator to make the curves smoother
const int i_limit = 60; // integral limit
const bool block = false; // for tests only
//int i=1;
//+ PID - PID
// RESULTS
// BC_power BC_const kp ki ki_1 kd Sum_Err Time Sample_T
// 40 0.15 1.3 1.9 0.9 0.3 130 8.9 10 ms
// 40 0.1 1.5 1.1 0.3 0.07 135 8.5


void PID_Calc()
{
P=kp*err; // Propotional action

if (kd!=0.0) // Differential action, it calculates impulse height and width depending on TM_LAG
{
if (TM_LAG < (Sample_T/2)/1000) TM_LAG=(Sample_T/2)/1000;
if ((sRueck<-1000.0)||(sRueck<-1000.0)) sRueck=0.0;
if ((sRestDiff<-1000.0)||(sRestDiff>1000.0)) sRestDiff=0.0;
D_anteil=(P-sRueck)*kd/((Sample_T/1000*0.5)+TM_LAG);
RueckAlt=sRueck;
RueckDiff=(Sample_T/1000)/kd * D_anteil + sRestDiff;
sRueck=RueckDiff+RueckAlt;
sRestDiff=RueckAlt-sRueck+RueckDiff;
D=D_anteil;
};

if (ki!=0) // integral action
{
I_old = I;
if ((d_err>=0) & (abs(err)>7))
delta_I=(kp*(err+last_err)*0.5*(Sample_T/1000)/abs(ki));// + rest_I;
else
delta_I=-(kp*(err+last_err)*0.5*(Sample_T/1000)/abs(ki_1));// + rest_I;
if ((delta_I<0.000001)&(delta_I>-0.000001)) delta_I=0.0;
I = I_old + delta_I; //trapesoid integration
if (I > i_limit) {I = i_limit;I_old=i_limit;};
if (I < -i_limit) {I = -i_limit;I_old=-i_limit;};
}

pid = abs(P+I+D);
last_err = err;
if (pid > pid_limit) pid = pid_limit;
}

task d_light_PID() // start another task just not to interupt the main tastk by wait1Msec()-s
{
while (true)
{
for (int j=1; j<10;j++) // calculate an average Ligh val for dif err estiamtion, kinf of filter
{
wait1Msec(Sample_T/10);
d_val_0 += SensorRaw[light];
}
d_val_0 = d_val_0/10;
wait1Msec(Sample_T);
d_val_1 = SensorRaw[light];
err = (d_val_1 - sp)/10;
d_err = (d_val_1 - d_val_0)/(Sample_T/1000);
if ((0<abs(d_err))&(abs(d_err)<=100)) d_err=0;
PID_Calc();
// This is just for PID efficiency estimation after line run,
// the closer the line the smaller Sum_err value should be.
Sum_err += abs(err)/10;
}
}

//1. Test whether NXT is on the left or on the right side of the line.
//2. Find the min & max value to calculate Set Point for PID
//3. If after back motor move, sensor finds lower than Min or higher than Max values,
// min or max values are corrected respectively. This helps better SP estimation.

void Test_Line_Side_and_SP()
{
int min_val=0;
int max_val=0;
int old_val=0;

old_val = SensorRaw[light];
motor[motorB]=15;
//motor[motorC]=-15;
wait1Msec(400);
motor[motorB]=0;
if (SensorRaw[light] < old_val)
{
min_val = SensorRaw[light];
max_val=old_val;
}
else
{
max_val = SensorRaw[light];
min_val = old_val;
}
motor[motorB]=-15;
wait1Msec(400);
motor[motorB]=0;
motor[motorC]=0;
if (SensorRaw[light] < min_val) min_val = SensorRaw[light];
if (SensorRaw[light] > max_val) max_val = SensorRaw[light];
wait1Msec(600);
if ((old_val - SensorRaw[light])>0) test = true;
else test = false;
test_passed = true;
//sp = (min_val + max_val)/2;
ClearTimer(T1);
}

task main ()
{
wait1Msec(100);
StartTask(d_light_PID);

while (true)
{
if (!test_passed)Test_Line_Side_and_SP();
if (block==false)
{
//if ((d_err>5900) & (Time>23000)) // may be used to stop with Time & Sum_err_End measurment
//{ // at the end of cirle, when white tape (marker) is sticked across the black one
//motor[motorB]=0;
//motor[motorC]=0;
//err=0;
//Sum_err_End = Sum_err;
//break;
//}
if ((sp-sp_hist)<SensorRaw[light] & SensorRaw[light]<(sp+sp_hist))
{
I = 0; // if the robot is "on the line", "cancel" the pid,
err = 0;
motor[motorB]=BC_power+5;
motor[motorC]=BC_power+5;
}
else
{
if (!test)
{
if (err>0)
{
motor[motorB]=BC_power + pid;
motor[motorC]=BC_power - BC_const*pid;
}
else
{
motor[motorB]=BC_power - BC_const*pid;
motor[motorC]=BC_power + pid;
}
}
else
{
if (err>0)
{
motor[motorC]=BC_power + pid;
motor[motorB]=BC_power - BC_const*pid;
}
else
{
motor[motorC]=BC_power - BC_const*pid;
motor[motorB]=BC_power + pid;;
}
}
}
}
Time=time1(T1);
Sum_err_End = Sum_err;
}
}


Last edited by sekai on Sun Apr 05, 2009 5:50 pm, edited 20 times in total.



Sun Apr 05, 2009 6:19 am
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3297
Location: Rotterdam, The Netherlands
Post Re: ready industrial PID for line follower
Thanks for sharing the code. Would you mind editing your post and putting the code tags around it? Just select the code in the editor and click on the "Code" button above. It will make the code look a bit easier to read.

Thanks!

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]


Sun Apr 05, 2009 6:34 am
Profile WWW
Rookie

Joined: Wed Jun 22, 2011 6:50 am
Posts: 1
Post Re: ready industrial PID for line follower
Thanks for share awesome posting keep it up!

_________________
industrial products supply


Wed Jun 22, 2011 6:55 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 3 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.