Hi…
I am trying to make a linefollowing robot and I am using the Pololu QTR-8Rc.
The materials I use:
- QTR 8 RC (6 sensors used)
- Pololu L298N Motor Control
- 2 robotus profast 12v 3000 rpm Motors
Screen Preview attached… Does not follow the line Please Help
My Code :
#include <QTRSensors.h>
#define Kp .2
#define Kd 6.1
#define Ki 0.0015
#define maxRightSpeed 100
#define maxLeftSpeed 100
#define defaultRightSpeed 50
#define defaultLeftSpeed 50
#define numberOfSensors 6
#define TIMEOUT 2500
#define EMITTER_PIN 2
#define rightMotorForward 8
#define rightMotorBack 9
#define PWM_RIGHT 3
#define leftMotorForward 4
#define leftMotorBack 5
#define PWM_LEFT 6
#define STBY 13
QTRSensorsRC qtrrc((unsigned char[]) { 19,18,17,16,15,14} ,numberOfSensors, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[numberOfSensors];
/* ...... S E T U P ...... */
void setup()
{
/* OUTPORT PORT DEFINE. */
pinMode(rightMotorForward, OUTPUT);
pinMode(rightMotorBack, OUTPUT);
pinMode(PWM_RIGHT, OUTPUT);
pinMode(leftMotorForward, OUTPUT);
pinMode(leftMotorBack, OUTPUT);
pinMode(PWM_LEFT, OUTPUT);
pinMode(STBY, OUTPUT);
/* CALIBRATE START. */
delay(500);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
for (int i = 0; i < 100; i++)
{
qtrrc.calibrate();
}
digitalWrite(13, LOW);
Serial.begin(9600);
}
/* ...... S E T U P END...... */
int previousError = 0;
int previousIntegral = 0;
/* WHITE/BLACK DEFINE.. */
bool isWhite=0;
bool isBlack=1;
/* ...... L O O P CODE START ...... */
void loop()
{
/* PRINT SENSOR VALUES... */
unsigned int position1 = qtrrc.readLine(sensorValues);
for (unsigned char sayac = 0; sayac < numberOfSensors; sayac++)
{
Serial.print(sensorValues[sayac]);
Serial.print('\t');
}
Serial.print(position1); //
Serial.print('\t');
/* END PRINT. */
/* PID... */
unsigned int sensors[6];
int position =qtrrc.readLine(sensors,QTR_EMITTERS_ON,isWhite); /* <<<<<<< WHITE ROAD BLACK LINE */
int error = 2500- position;
int Integral = error+previousIntegral;
int speedOfMotor = (Kp * error)/100 + (Kd * (error - previousError))/100 + Ki * Integral;
previousError = error;
previousIntegral=Integral;
/* PID END... */
/* NEW MOTOR SPEED... */
int speedOfRight = defaultRightSpeed + speedOfMotor;
int speedOfLeft = defaultLeftSpeed - speedOfMotor;
/*OPTIMIZED MOTOR SPEED... */
if (speedOfRight > maxRightSpeed ) speedOfRight = maxRightSpeed;
if (speedOfLeft > maxLeftSpeed ) speedOfLeft = maxLeftSpeed;
if (speedOfRight < 0) speedOfRight = 0;
if (speedOfLeft < 0) speedOfLeft = 0;
/* PRINT NEW VALUES... */
Serial.print("pid Motor Speed: ");
Serial.print(speedOfMotor);
Serial.print('\t');
Serial.print("Position: ");
Serial.print(position);
Serial.print('\t');
Serial.print("Error: ");
Serial.print(error);
Serial.print('\t');
Serial.print("Right New Speed: ");
Serial.print(speedOfRight);
Serial.print('\t');
Serial.print("Left New Speed: ");
Serial.println(speedOfLeft);
delay(500);
/* NEW SPEED WRITE... */
digitalWrite(rightMotorForward, HIGH);
digitalWrite(rightMotorBack, LOW);
analogWrite(PWM_RIGHT, speedOfRight);
digitalWrite(leftMotorForward, HIGH);
digitalWrite(leftMotorBack, LOW);
analogWrite(PWM_LEFT, speedOfLeft);
}
/* ...... L O O P E N D ...... */