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.