First I'd like to thank So ChikMan From Hong kong HKU , he is a pioneer in control engineering besides, he has repersented his country many times in Robocon contest, he has a great impact on teaching me the theory and operation of PID Control.
Hi, In robocon 2008 , i've tried to implement the well-known algorithm in Control engineering the PID ( Proportional -integeral -derivative) , I was having a great experience using the microcontroller from ATMEL 89S52 , plus I've already designed a complete board using it at the past years of the contests , and due to low fund , I've used it again. The problem is that uC doesnt have a PWM Hardware module so you've to write your own Software PWM , also it doesnt have an ADC ( anlogue to digital converter) , and i've solved this by using a crude way of doing an ADC which is using a compartor like LM324 .
The idea is there are 6 sensors (some people uses 8 or 16,or 32! depends on your complexity of design and speed you want to achieve) , These sensors will give you the position of the robot from the line which it follows considring that the output from the sensors are Digital , so you've to define some numeric values to enumrate for the posssibilites of the position of the robot on the line to be tracked .
Consider that 1 is the value on white which means your on the line , and 0 is the value when your away from the line . Consider The sensor arrangement as this xxxxxx , let it be L1L2L3R3R2R1
Consider that L3,R3 have been positioned to be on the line , and the other sensors are seprated by 10mm .
The Pid algorithm calculates the error from the feedback element which is our 6 line sensors , when L3,R3 are on the white line , which means the robot is going straight and it also means the the Error is 0 , which is our desired value or what's always called in control engineering text books ( Set Point ) .
pseudo code :
error = setpoint- measured value .
If you've a control engineering textbook , you will see many figures of the well-known control loop which is familiar to many engineers .
The measured value should be a predfined numeric values because we dont have an ADC here.
We will define it like in the code.
Code List using 89S52
void init_timer0(void)
{
EA=0; // disable gloabl interrupts
TMOD=0x02; // 0000 0010
TH0=0x9B; // Timer values
TL0=0x9B; //
PWM=0;
ET0=1; // Enable timer interrupt 0
EA=1;
}
The Software PWM Code
// Motor1_2 , is the enable pin of the motor for PWM
// pwm_l , r are the values of the PWM which be sent to the motor , from 0 > 11
void timer0(void) interrupt 1 using 0
{
TR0=0;
PWM++;
TL0=0x9B;TF0=0;
if (PWM==11) PWM=0
if (PWM <= pwm_l) Motor1_2=1; else Motor1_2=0; if (PWM <= pwm_r) Motor2_2=1; else Motor2_2=0; TR0=1; }
// Do this in a 20ms (sampling time) control loop
void Line_Track(void)
{
int mpos;
float error,Rate,Deriv,previous_error;
pwm_r_=11;
pwm_l_=11;
TR0=1;
Walk(1); // move forward
if((Sensor_1==0)&&(Sensor_2==0)&&(Sensor_3==0)&&(Sensor_4==0)&&(Sensor_5==0)&&(Sensor_6==0))
{
mpos=0;
}
if ((Sensor_1==1)&&(Sensor_2==1)&&(Sensor_3==1)&&(Sensor_4==1)&&(Sensor_5==1)&&(Sensor_6==1))
{
mpos=0;
}
if ((Sensor_1==0)&&(Sensor_2==0)&&(Sensor_3==1)&&(Sensor_4==0)&&(Sensor_5==0)&&(Sensor_6==0))
{
mpos=-1;
}
if((Sensor_1==0)&&(Sensor_2==0)&&(Sensor_3==0)&&(Sensor_4==1)&&(Sensor_5==0)&&(Sensor_6==0))
{
mpos=1;
}
if((Sensor_1==0)&&(Sensor_2==1)&&(Sensor_3==0)&&(Sensor_4==0)&&(Sensor_5==0)&&(Sensor_6==0))
{
mpos=-2;
}
if((Sensor_1==0)&&(Sensor_2==0)&&(Sensor_3==0)&&(Sensor_4==0)&&(Sensor_5==1)&&(Sensor_6==0))
{
mpos=2;
}
if((Sensor_1==1)&&(Sensor_2==0)&&(Sensor_3==0)&&(Sensor_4==0)&&(Sensor_5==0)&&(Sensor_6==0))
{
mpos=-5;
}
if((Sensor_1==0)&&(Sensor_2==0)&&(Sensor_3==0)&&(Sensor_4==0)&&(Sensor_5==0)&&(Sensor_6==1))
{
mpos=5;
}
if((Sensor_1==0)&&(Sensor_2==1)&&(Sensor_3==1)&&(Sensor_4==0)&&(Sensor_5==0)&&(Sensor_6==0))
{
mpos=-4;
}
if((Sensor_1==0)&&(Sensor_2==0)&&(Sensor_3==0)&&(Sensor_4==1)&&(Sensor_5==1)&&(Sensor_6==0))
{
mpos=-4;
}
if((Sensor_1==0)&&(Sensor_2==0)&&(Sensor_3==0)&&(Sensor_4==1)&&(Sensor_5==1)&&(Sensor_6==1))
{
mpos=3;
}
if((Sensor_1==1)&&(Sensor_2==1)&&(Sensor_3==1)&&(Sensor_4==0)&&(Sensor_5==0)&&(Sensor_6==0))
{
mpos=-3;
}
// PID Equations
error=-mpos; // error = 0 - feedback , 0 is the center
error=error*2.2; // KP*ERROR , Kp=2.2
Rate = error-previous_error;
Deriv = Rate * 2.0; // KD = 2.0
previous_error=error;
error=error+Deriv; // PD Terms
// CUT Terms
// error is negative , robot is going to right
if (error < pwm_r =" plus"> 0){
pwm_r_+= ABS(error);
pwm_l_-=ABS(error);
}
if(pwm_r_>11)pwm_r_=11;
if(pwm_l_>11)pwm_l_=11;
}
Videos
Fast line follower
http://www.youtube.com/watch?v=g2_9nDmEmPo
Doing a task
http://www.youtube.com/watch?v=kiqopmLjFjE
Eng.Ahmed Saleh Mohammed
El Shorouk Acadmey
Electronics and communications engineering department
I want to use my analog line sensor for my trackbot. Then how can I change this program correspondingly ?
ReplyDeleteusing analog line sensors will give you more accurate results , and you don't need to use more IFs to check for every condition , high analog value means that the sensor is on line or vice versa , so you can just use the equations directly with the output analog values from the sensors . make sure to use an integer value for the output PWM modules.
ReplyDelete