Society of Robots - Robot Forum
Software => Software => Topic started by: piotr3332 on April 17, 2013, 10:48:29 AM
-
Hi
I have a question regarding my line follower. To obtain data from sensors the simple RC charge/discharge circuit was used to approximate multiple analogue to digital converter (ADC). Pulse width modulation (PWM) is used to vary the intensity of the IR TX. With a little bit of adjustment each sensor can produce an output with a 2 - 3 bit resolution i.e. 4 – 8 levels. In operation each sensor is connected to a digital IO port. Initially this port is set to an output, driving a logic ‘1’ onto the line for approximately 50us, charging the associated capacitor to VCC. The port is then switched to an input, setting the line to a high impedance state. The rate at which the capacitor discharges to zero is therefore proportional to level of reflected light i.e. the current following through the IR transistor. During this discharge period the eight inputs are polled by the processor at fixed time intervals and the zero crossing times recorded i.e. time is proportional to the reflected light level.
For example, when a sensors sees a line it gives me output of 7 on the other hand when it is off the line it gives me output 15. Could you give me the best method to use this sensor data in producing PID algoruthm?
I provide a source code for reading data.
#include "mbed.h"
DigitalOut led(LED1); // status LED
DigitalOut test(p27); // test pin
DigitalInOut ir_rx0(p11); // IR recieves
DigitalInOut ir_rx1(p12); // IR recieves
DigitalInOut ir_rx2(p15); // IR recieves
DigitalInOut ir_rx3(p16); // IR recieves
DigitalInOut ir_rx4(p17); // IR recieves
DigitalInOut ir_rx5(p18); // IR recieves
DigitalInOut ir_rx6(p25); // IR recieves
DigitalInOut ir_rx7(p26); // IR recieves
PwmOut ir_tx(p24); // IR transmitter
Serial pc(USBTX, USBRX); // USB serial port
// Black : 4.2x200us = 840us
// White : 2 x 100us = 200us
// Silver : 1.2 x 100us = 120us
int main() {
bool ir_set[8];
int ir_value[8];
int ir_buffer;
int i, j, k;
ir_tx.period_us(100); // a 20ms period
ir_tx.pulsewidth_us(100); //
led = 1;
test = 0;
while(true)
{
ir_rx0.output();
ir_rx1.output();
ir_rx2.output();
ir_rx3.output();
ir_rx4.output();
ir_rx5.output();
ir_rx6.output();
ir_rx7.output();
ir_rx0 = 1;
ir_rx1 = 1;
ir_rx2 = 1;
ir_rx3 = 1;
ir_rx4 = 1;
ir_rx5 = 1;
ir_rx6 = 1;
ir_rx7 = 1;
for(i=0; i<8; i++)
{
ir_set[i]=true;
ir_value[i]=0;
}
wait_us(50);
ir_rx0.input();
ir_rx1.input();
ir_rx2.input();
ir_rx3.input();
ir_rx4.input();
ir_rx5.input();
ir_rx6.input();
ir_rx7.input();
test = 1;
for(i=0; i<20; i++)
{
wait_us(50);
if ((ir_buffer && 0x01) == 0)
test = 0;
if((ir_rx0==0) & ir_set[0])
{
ir_set[0]=false; ir_value[0]=i;
}
if((ir_rx1==0) & ir_set[1])
{
ir_set[1]=false; ir_value[1]=i;
}
if((ir_rx2==0) & ir_set[2])
{
ir_set[2]=false; ir_value[2]=i;
}
if((ir_rx3==0) & ir_set[3])
{
ir_set[3]=false; ir_value[3]=i;
}
if((ir_rx4==0) & ir_set[4])
{
ir_set[4]=false; ir_value[4]=i;
}
if((ir_rx5==0) & ir_set[5])
{
ir_set[5]=false; ir_value[5]=i;
}
if((ir_rx6==0) & ir_set[6])
{
ir_set[6]=false; ir_value[6]=i;
}
if((ir_rx7==0) & ir_set[7])
{
ir_set[7]=false; ir_value[7]=i;
}
}
printf( "ADC conversion\n" );
printf( "chan 0 = %d\n", ir_value[0] );
printf( "chan 1 = %d\n", ir_value[1] );
printf( "chan 2 = %d\n", ir_value[2] );
printf( "chan 3 = %d\n", ir_value[3] );
printf( "chan 4 = %d\n", ir_value[4] );
printf( "chan 5 = %d\n", ir_value[5] );
printf( "chan 6 = %d\n", ir_value[6] );
printf( "chan 7 = %d\n", ir_value[7] );
wait(1);
}
}
Thanks
-
Why PID?
Have you coded for the Bot to stay on the line?
What didn't work well?
-
PID Because it is one of the requirenments of my project. I would also want to code my solution based on fuzzy logic but at the moment I dont have idea how I can do this.
Yes I have coded my robot to follow the line.
Here is my code
#include "mbed.h"
DigitalOut led(LED1); // status LED
DigitalOut test(p27); // test pin
DigitalInOut ir_rx0(p11); // IR recieves
DigitalInOut ir_rx1(p12); // IR recieves
DigitalInOut ir_rx2(p15); // IR recieves
DigitalInOut ir_rx3(p16); // IR recieves
DigitalInOut ir_rx4(p17); // IR recieves
DigitalInOut ir_rx5(p18); // IR recieves
DigitalInOut ir_rx6(p25); // IR recieves
DigitalInOut ir_rx7(p26); // IR recieves
PwmOut ir_tx(p24); // IR transmitter
Serial pc(USBTX, USBRX); // USB serial port
// Black : 4.2x200us = 840us
// White : 2 x 100us = 200us
// Silver : 1.2 x 100us = 120us
//------MOTOR----------
I2C i2c(p9, p10); // sda, scl
PwmOut pwm0(p22);
PwmOut pwm1(p23);
const int addr = 0x40; // define the I2C Address
int kp = 200;
int kd = 1;
int ki = 1;
unsigned int FORWARD = 0x66;
unsigned int RIGHT = 0x55;
unsigned int LEFT = 0xAA;
int superslow = 6000; //pwm width value
float readSensors();
int main() {
//************MOTOR INIT******************
char cmd[2];
pwm0.period_us(20);
pwm1.period_ms(20);
//**********PID INIT************************
float i_error[10]= { 0,0,0,0,0,0,0,0,0,0}; //stores error values of last 10 iterations for I control
int i_counter=0;
int sum_i_error=0;
int turn;
int error,error_old=0,zero=0,error_diff,temp;
while(true)
{
//command for robot to go straight
cmd[0] = FORWARD;
cmd[1] = 0x00;
i2c.write(addr, cmd, 1);
//returned position is the error
error = readSensors();
//****************I control*****************************//
sum_i_error =sum_i_error - i_error[i_counter];
sum_i_error =sum_i_error + error;
//printf("I ERROR: %d\n", sum_i_error);
i_error[i_counter%10]=error;
i_counter++;
if (i_counter==10)
i_counter=0;
//****************D control*****************************//
error_diff = error-error_old;
turn = error*kp +error_diff*kd + sum_i_error*ki;
error_old=error;
//adjusting the speed of motors based on turn
pwm0.pulsewidth_us(turn + superslow);
printf("left after loop: %d\n", turn + superslow);
pwm1.pulsewidth_us(turn - superslow);
printf("right after loop: %d\n", turn - superslow);
}
}
float readSensors()
{
bool ir_set[8];
int ir_value[8];
int ir_buffer;
int i, j, k;
led = 1;
test = 0;
ir_tx.period_ms(20); // a 20ms period
ir_tx.pulsewidth_ms(20); //
int position;
ir_rx0.output();
ir_rx1.output();
ir_rx2.output();
ir_rx3.output();
ir_rx4.output();
ir_rx5.output();
ir_rx6.output();
ir_rx7.output();
ir_rx0 = 1;
ir_rx1 = 1;
ir_rx2 = 1;
ir_rx3 = 1;
ir_rx4 = 1;
ir_rx5 = 1;
ir_rx6 = 1;
ir_rx7 = 1;
for(i=0; i<8; i++)
{
ir_set[i]=true;
ir_value[i]=0;
}
wait_us(50);
ir_rx0.input();
ir_rx1.input();
ir_rx2.input();
ir_rx3.input();
ir_rx4.input();
ir_rx5.input();
ir_rx6.input();
ir_rx7.input();
test = 1;
for(i=0; i<20; i++)
{
wait_us(50);
if ((ir_buffer && 0x01) == 0)
test = 0;
if((ir_rx0==0) & ir_set[0])
{
ir_set[0]=false; ir_value[0]=i;
}
if((ir_rx1==0) & ir_set[1])
{
ir_set[1]=false; ir_value[1]=i;
}
if((ir_rx2==0) & ir_set[2])
{
ir_set[2]=false; ir_value[2]=i;
}
if((ir_rx3==0) & ir_set[3])
{
ir_set[3]=false; ir_value[3]=i;
}
if((ir_rx4==0) & ir_set[4])
{
ir_set[4]=false; ir_value[4]=i;
}
if((ir_rx5==0) & ir_set[5])
{
ir_set[5]=false; ir_value[5]=i;
}
if((ir_rx6==0) & ir_set[6])
{
ir_set[6]=false; ir_value[6]=i;
}
if((ir_rx7==0) & ir_set[7])
{
ir_set[7]=false; ir_value[7]=i;
}
}
//
if(ir_value[3] == 8 && ir_value[4] == 8 )
{
position = 0;
//perfectly straight
}
if(ir_value[3] == 10 && ir_value[5] == 10)
{
//slighltly left
position = 1;
}
if(ir_value[5] < 10)
{
//left
position = 2;
}
if(ir_value[5] == 7 && ir_value[7] == 11)
{
//more left
//position = 3;
}
if(ir_value[7] < 13)
{
//hard left
position = 4;
}
if(ir_value[7] > 6 && ir_value[5] > 14)
{
//going left off line
//position = 5;
}
if(ir_value[3] == 10 && ir_value[2] == 10)
{
//slighltly right
position = -1;
}
if(ir_value[2] < 10 )
{
//right
position = -2;
}
if(ir_value[2] >= 6 && ir_value[0] == 11)
{
//more right
//position = -3;
}
if(ir_value[1] < 13)
{
//hard right
position = -4;
}
if(ir_value[0] == 8 && ir_value[1] == 8)
{
//going right off line
//position = -5;
}
return position;
printf("position: %d\n", position);
}
The problem with my solution is that it is not accurate, meaning the position value for each sensor is returned only when the if expression is satisfied. Could there be another method of programming this solution? I would reall appreciate any help.
-
Have you googled "multi sensor line follower". There are a few good articles on how to do this.
The starting basics is to combine all the sensors into one variable that is used as the input the PID (or just P) control loop.
Here is a link to a good article showing how to add the sensors together:
http://www.ikalogic.com/line-tracking-sensors-and-algorithms/ (http://www.ikalogic.com/line-tracking-sensors-and-algorithms/)