I am trying to use PID algorithm in my line following robot and I don't know why but my robot isn't following the line. I am posting my code here to see if there's any problem. I sure enough that there no problem in my hardware.
/*
author: crocodile_009
date: 25/07/2013
my first code for autonomous line follower robot with arduino IDE
*/
#define rep(a,b,c) for(int a = b;a<c;a++)
int motor_left[] = {7,8} ; // ledPin 7 and 8 for turning motor left
int motor_right[] = {2,3} ; // ledPin 2 and 3 for turning motor right
int sensors[5] = {0, 1, 2, 3, 4} ; // sensor arrays which are connected to 5 available analog pins
int sensor_reading = 0 ; // count active sensor
int sen[5] = {0};
double sensors_average = 0.00 ;
int right_speed, left_speed ;
double active_sensor = 0.00 ;
double total_sensor = 0.00 ;
double max_speed ; // get from sensor
double sensors_sum = 0.00;
double position1 = 0.0;
double proportional = 0.00;
double integral = 0.00;
double derivative = 0.00;
double set_point = 4.5 ; // need to assign value, on which path robot drives, manual ;
double last_proportional = 0.00;
double error_value = 0.00;
bool flag = true ;
/* need to assign value these three variables, at first start equiling with 0,
tuning Kp first, we will try to set this value less than 10 ;
when loses the line reduce Kp, if cannot navigate a turn increase Kp;
after that try to tune Kd then Ki ;
*/
double Kp=72;
double Kd=5;
double Ki=0.00015;
int light = 4;
/* program initialized */
void setup()
{
Serial.begin(9600) ; // set up for serial port
/* set up motors */
pinMode(light, OUTPUT);
rep(i,0,2)
{
pinMode(motor_left[i], OUTPUT) ;
pinMode(motor_right[i], OUTPUT) ;
}
}
/* loop starts */
void loop()
{
digitalWrite(light, HIGH);
sensors_read() ; // Reads sensors values and computes sensors sum and weighted average
PID_Calc() ; // calculate PID values
Calc_turn() ; // select turn
Motor_drive (right_speed, left_speed) ; // drive motor
/* More logics about Threshold tuning, PID tuning, counting gap will be updated later */
}
/* read sensors */
void sensors_read()
{
total_sensor = 0;
active_sensor = 0 ;
sensors_average = 1 ;
sensors_sum = 1 ;
rep(i,0,5)
{
sensor_reading = analogRead(sensors[i]) ;
//Serial.print(sensor_reading);
//Serial.print(" ");
//delay(1000);
if(sensor_reading>33)
{
active_sensor += 1 ;
sen[i] = 1 ;
}
else
{
sen[i] = 0 ;
}
//Serial.print(sen[i]);
//Serial.print(" ");
//delay(1000);
//total_sensor = total_sensor + sensor_reading[i] * (i+1) ;// calculate the weighted mean of sensor's reading, 1000 means nothing
total_sensor += ((i+1) * sen[i]); // calculate the weighted mean of sensor's reading,
/* to debug */
/*
Serial.print(sensors_average);
Serial.print(' ');
Serial.print(sensors_sum);
Serial.print(' ');
Serial.print(position1);
Serial.println();
delay(2000) ;
*/
}
if(total_sensor == 0 && active_sensor == 0)
{
total_sensor = 1;
active_sensor = 100000000;
}
sensors_average = total_sensor ;
sensors_sum = active_sensor ;
//position1 = sensors_average / sensors_sum;
/*
Serial.println();
Serial.print(sensors_average);
Serial.print(" ");
Serial.print(sensors_sum);
Serial.print(" ");
Serial.print(position1);
Serial.print(" ");
delay(1000);
*/
}
/* PID algorithm */
void PID_Calc()
{
/*
Serial.print(position1);
Serial.print(' ');
Serial.print(set_point);
Serial.print(' ');
delay(1000);
*/
position1 = sensors_average / sensors_sum ;
last_proportional = proportional ;
proportional = position1 - set_point ;
integral = integral + proportional ;
derivative = proportional - last_proportional ;
/* calculate the formula */
error_value = proportional*Kp + integral*Ki + derivative*Kd ;
/*
Serial.print(position1);
Serial.print(' ');
Serial.print(set_point);
Serial.print(' ');
Serial.print(error_value);
Serial.print(' ');
delay(1000);
Serial.println();
*/
}
/* select turn */
void Calc_turn()
{
/* restricting error value between +/- 256 , it can be any arbitary number depending on my choice */
if(error_value < -256)
{
error_value = -256 ;
}
if(error_value > 256)
{
error_value = 256 ;
}
/* if error_value is less than zero calculate right turn value,
actually PID is an inverse relationship between motor turning and error_value,
we know analog pins were introduced from left to right in sensors arrays
so, if error_values < 0 , bot must turn right as well as increase left_turn values
*/
if(error_value < 0)
{
right_speed = max_speed + error_value ;
left_speed = max_speed ;
}
if(error_value > 0)
{
left_speed = max_speed - error_value ;
right_speed = max_speed ;
}
}
/* drive motor */
void Motor_drive(int right_speed, int left_speed)
{
/* based on the width of track, we will trace if difference is too small.
we will run the bot direct, it may help to tune bot more finely,
here we set the value 20.
*/
if((right_speed - left_speed) < 20 || (left_speed - right_speed) < 20)
{
drive_forward() ;
}
else if(right_speed > left_speed)
{
drive_left() ;
}
else
{
drive_right() ;
}
delay(50) ;
}
/* drive logics */
void drive_forward()
{
digitalWrite(motor_left[0], HIGH) ;
digitalWrite(motor_left[1], LOW) ;
digitalWrite(motor_right[0], HIGH) ;
digitalWrite(motor_right[1], LOW) ;
}
void drive_left()
{
digitalWrite(motor_left[0], HIGH) ;
digitalWrite(motor_left[1], LOW) ;
digitalWrite(motor_right[0], LOW) ;
digitalWrite(motor_right[1], HIGH) ;
}
void drive_right()
{
digitalWrite(motor_left[0], LOW) ;
digitalWrite(motor_left[1], HIGH) ;
digitalWrite(motor_right[0], HIGH) ;
digitalWrite(motor_right[1], LOW) ;
}
