Hi,
I’m currently working on a fast line follower but I can’t get it to follow the line correctly.
It feels like it is too slow to respond. It comes to a turn and it starts turning after all sensors have passed the line (then it starts turning(fast) till it sees the line again and usually loses control). The line nearly never stays on the middle of the sensors (except for the straight parts).
It follows the line ok if I hold it and slow it down (there’s a little wobble, I believe it’s because of un-calibrated Kp/Kd).
I’ve been trying to calibrate Kp and Kd values for a month now but no luck (of course some values give better results). I also tried playing with Default_Speed, Max_Speed values (code below).
It’s components are:
Arduino Uno R3
QTR-8RC (8 sensors, all being used)
4x 3000rpm 12V motors
11.1v 1A Li-Po
Old Adafruit Motor Shield
(Motors are 10-15 cm appart from the sensors.)
Code:
#include <QTRSensors.h>
#include <AFMotor.h>
AF_DCMotor motor1(1, MOTOR12_8KHZ ); // create motor #1 pwm
AF_DCMotor motor2(2, MOTOR12_8KHZ ); // create motor #2 pwm
AF_DCMotor motor3(3, MOTOR12_8KHZ ); // create motor #3 pwm
AF_DCMotor motor4(4, MOTOR12_8KHZ ); // create motor #4 pwm
//motor 1,4 are right side motors. motor 2,3 are left side motors
#define KP 4 //no idea about Kp and Kd values. tried Kp= 0.2 and Kd=5 to Kp= 1 and Kd= 20 and many possibilities inbetween
#define KD 60
#define M1_DEFAULT_SPEED 80 //also no idea about motor speeds
#define M4_DEFAULT_SPEED 80
#define M2_DEFAULT_SPEED 80
#define M3_DEFAULT_SPEED 80
#define M1_MAX_SPEED 95
#define M4_MAX_SPEED 95
#define M2_MAX_SPEED 95
#define M3_MAX_SPEED 95
//#define MIDDLE_SENSOR 4 //comemmented this because it doesn't look like that it makes a difference and since I have 8 sensors I don't know how to get the middle one
#define NUM_SENSORS 8 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN QTR_NO_EMITTER_PIN //not sure if I need this
#define DEBUG 0 // set to 1 if serial debug output needed
QTRSensorsRC qtrrc((unsigned char[]) {13,2,19,18,17,16,15,14} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN); //sensor pins defined
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
delay(1000);
manual_calibration();
set_motors(0,0,0,0);
}
int lastError = 0;
int last_proportional = 0;
int integral = 0; //Ki isn't used at all
void loop()
{
unsigned int sensors[8];
int position = qtrrc.readLine(sensors);
int error = position - 3500; //line stays on the middle of the sensors if I hold the robot (and slow it down) and on straight paths, I think there is no problem here
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
int rightMotorSpeed = M1_DEFAULT_SPEED - motorSpeed; //right motors </v
int rightMotorSpeedy = M4_DEFAULT_SPEED - motorSpeed; //added "-y" to make variable name difference
int leftMotorSpeed = M2_DEFAULT_SPEED + motorSpeed; //left motors </v
int leftMotorSpeedy = M3_DEFAULT_SPEED + motorSpeed; //added "-y" to make variable name difference
// set motor speeds using the two motor speed variables above
set_motors(rightMotorSpeed,leftMotorSpeed, leftMotorSpeedy, rightMotorSpeedy);
}
void set_motors(int motor1speed, int motor2speed, int motor3speed, int motor4speed)
{
if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;// limit top speed
if (motor4speed > M4_MAX_SPEED ) motor4speed = M4_MAX_SPEED;// limit top speed
if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;// limit top speed
if (motor3speed > M3_MAX_SPEED ) motor3speed = M3_MAX_SPEED;// limit top speed
if (motor1speed < 0) motor1speed = 0;// keep motor speed above 0
if (motor4speed < 0) motor4speed = 0;// keep motor speed above 0
if (motor2speed < 0) motor2speed = 0;// keep motor speed above 0
if (motor3speed < 0) motor3speed = 0;// keep motor speed above 0
motor1.setSpeed(motor1speed);// set motor speed
motor4.setSpeed(motor1speed);// set motor speed
motor2.setSpeed(motor2speed);// set motor speed
motor3.setSpeed(motor2speed);// set motor speed
motor1.run(FORWARD);
motor4.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
}
void manual_calibration() {
int i;
for (i = 0; i < 250; i++) // the calibration will take a few seconds
{
qtrrc.calibrate(QTR_EMITTERS_ON);
delay(20);
}
if (DEBUG) { // if true, generate sensor dats via serial output
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn);
Serial.print(' ');
}
Serial.println();
Serial.println();
}
}