I have built a line follower using the Pololu - QTR-8RC Reflectance Sensor Array. It runs pretty well enough on curves and straight lines, but is unable to precisely navigate 90 degree turns. I mean that is does turn around the 90 degree turns, but wobbs a lot.
The program is below. Please help me out with these 90 degree turns with the pololu qtr-8rc sensor array.
#include <QTRSensors.h>
#include <AFMotor.h>
AF_DCMotor m_left(2, MOTOR12_1KHZ );
AF_DCMotor m_right(3, MOTOR34_1KHZ );
float Kp = 0.1; //0.2
float Kd = 0.28;
int base_speed = 255; // vary base motor speed according to voltage
int bot_position = 0;
int error = 0;
unsigned int sensors[8];
int proportional = 0;
int derivative = 0;
int last_proportional = 0;
#define NUM_SENSORS 8
#define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2
#define DEBUG 0
QTRSensorsRC qtrrc((unsigned char[]) {A0,A1,A2,A3,A4,A5,9,10} ,NUM_SENSORS, TIMEOUT,
EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup() {
Serial.begin(9600);
auto_calibration();
bot_stop();
delay(1000);
}
void loop() {
pid_calc();
motor_drive();
}
void auto_calibration() {
for (int i = 0; i < 170; i++){
qtrrc.calibrate(QTR_EMITTERS_ON);
delay(20);
if(i < 50) left();
else if (i > 50 && i < 120) right();
else if (i > 120 && i < 170) left();
}
if (DEBUG) {
for (int i = 0; i < NUM_SENSORS; i++){
qtrrc.calibratedMinimumOn[i];
}
for (int i = 0; i < NUM_SENSORS; i++){
qtrrc.calibratedMaximumOn[i];
}
}
}
void pid_calc(){
bot_position = qtrrc.readLine(sensors);
//Serial.println(bot_position);
proportional = bot_position - 3400;
derivative = proportional - last_proportional;
last_proportional = proportional;
error = (Kp * proportional) + (Kd * derivative);
//Serial.println(error);
}
void motor_drive(){
int left_motor_speed;
int right_motor_speed;
left_motor_speed = base_speed - error;
right_motor_speed = base_speed + error;
if(left_motor_speed < 0) left_motor_speed = 0;
else if(left_motor_speed > 255) left_motor_speed = 255;
if(right_motor_speed < 0) right_motor_speed = 0;
else if(right_motor_speed > 255) right_motor_speed = 255;
Serial.print(left_motor_speed);
Serial.print(" ");
Serial.println(right_motor_speed);
m_left.setSpeed(left_motor_speed);
m_right.setSpeed(right_motor_speed);
m_left.run(FORWARD);
m_right.run(FORWARD);
}
void left(){
m_left.setSpeed(55);
m_right.setSpeed(55);
m_left.run(BACKWARD);
m_right.run(FORWARD);
}
void right(){
m_left.setSpeed(55);
m_right.setSpeed(55);
m_left.run(FORWARD);
m_right.run(BACKWARD);
}
void bot_stop(){
m_left.setSpeed(0);
m_right.setSpeed(0);
m_left.run(FORWARD);
m_right.run(FORWARD);
}