Hi…
I am trying to make a linefollowing robot and I am using the Pololu QTR-8Rc.
The materials I use:
- Arduino Nano
- QTR 8 RC (6 sensors used)
- Pololu L298N Motor Control
- 2 robotus profast 12v 3000 rpm Motors
This code is working.
but it does not follow line when the speed is increased (M1_MAX_SPEED,M2_MAX_SPEED)
Is this problem due to incorrect KP and KD values?
#include <QTRSensors.h>
#define KP 0.5
#define KD 0
#define M1_MAX_SPEED 80
#define M2_MAX_SPEED 80
#define M1_DEFAULT_SPEED 50
#define M2_DEFAULT_SPEED 50
#define MIDDLE_SENSOR 4, 5
#define NUM_SENSORS 6 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define DEBUG 1 // set to 1 if serial debug output needed
#define rightMotor1 8
#define rightMotor2 9
#define rightMotorPWM 3
#define leftMotor1 4
#define leftMotor2 5
#define leftMotorPWM 6
#define motorPower 8
QTRSensorsRC qtrrc((unsigned char[]) { 14, 15, 16, 17, 18, 19} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(rightMotorPWM, OUTPUT);
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(leftMotorPWM, OUTPUT);
pinMode(motorPower, OUTPUT);
manual_calibration();
}
int lastError = 0;
int last_proportional = 0;
int integral = 0;
void loop()
{
unsigned int sensors[6];
int position = qtrrc.readLine(sensors);
int error = position - 2500;
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
int rightMotorSpeed = M2_DEFAULT_SPEED + motorSpeed;
int leftMotorSpeed = M1_DEFAULT_SPEED - motorSpeed;
if (rightMotorSpeed > M1_MAX_SPEED ) rightMotorSpeed = M1_MAX_SPEED; // limit top speed
if (leftMotorSpeed > M2_MAX_SPEED ) leftMotorSpeed = M2_MAX_SPEED; // limit top speed
if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep motor above 0
if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep motor speed above 0
{
digitalWrite(motorPower, HIGH);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(motorPower, HIGH);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
}
}
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[i]);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
}
}