Hello
I have a code for a line follower, it is intended that my robot follow the black line, but he stops at the black line! Does anyone know what that may lie: mounting the sensor, code or … Kan somebody look at my code and tell me what’s wrong is?
Thanks before
Ps: THE comment are in dutch because i am from Belgium
#include <QTRSensors.h>
#include <AFMotor.h>
AF_DCMotor motor1(1, MOTOR12_64KHZ);
AF_DCMotor motor2(2, MOTOR12_64KHZ);
#define KP .2
#define KD 5
#define M1_DEFAULT_SPEED 50
#define M2_DEFAULT_SPEED 50
#define M1_MAX_SPEED 255
#define M2_MAX_SPEED 255
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 5
#define TIMEOUT 2500
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define DEBUG 0
QTRSensorsRC qtrrc((unsigned char) { 4,5,6,7,8} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup() // voorbereiding
{
delay(1000); // 1000 miliseconden
manual_calibration(); // De methode wordt opgeroepen
set_motors(0,0); // De methode wordt opgeroepen
}
int lastError = 0;
int last_proportional = 0;
int integral = 0;
void loop()
{
unsigned int sensors[5];
// Het aantal sensors definiëren
int position = qtrrc.readLine(sensors);
// 0 tot 5000, waarbij 0 betekent direct onder de sensor of de lijn werd verloren
int error = position - 2000;
// error = positie - 2000
int motorSpeed = KP * error + KD * (error - lastError);
// De snelheid van de motor is gelijk aan 0,2 * error + 5 * (error - lastError)
lastError = error;
// De lastError = error
int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
// linkermotor = 150 + motorspeed
int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
// rechtermotor = 150 + motorspeed
set_motors(leftMotorSpeed, rightMotorSpeed);
// Zet de motor snelheid op (linkermotorsnelheid, rechtermotorsnelheid)
}
void set_motors(int motor1speed, int motor2speed)
{
if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;
// limiet van de snelheid van motor 1
if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;
// limiet van de snelheid van motor 2
if (motor1speed < 0) motor1speed = 0;
// Houd motor 1 boven de 0
if (motor2speed < 0) motor2speed = 0;
// Houd motor 2 boven de 0
motor1.setSpeed(motor1speed);
// Zet de motorsnelheid van motor 1 om in rotatie per minuut
motor2.setSpeed(motor2speed);
// Zet de motorsnelheid van motor 2 om in rotatie per minuut
motor1.run(FORWARD);
// Laat de motor 1 voorwaarts rijden
motor2.run(FORWARD);
// laat de motor 2 voorwaarts rijden
}
void manual_calibration() {
int i;
for (i = 0; i < 250; i++)
// de kalibratie neemt enkele seconden
{
qtrrc.calibrate(QTR_EMITTERS_ON);
// Het zet de emitter pin aan
delay(20);
// met een tussenstop van 20 miliseconden
}
if (DEBUG) {
// Als het juist is, zendt de sensor de data naar de seriele output
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
// verhoog i, zolang i kleiner is dan het aantal sensors
{
Serial.print(qtrrc.calibratedMinimumOn*);*
// print de kalibratie tot een minimum bij een emitter pin die on is, bij sensor i
Serial.print(’ ');
// daarna een lege print
}
Serial.println();
// print data naar de seriele output, als menselijke taal via de ASCII tabel
for (int i = 0; i < NUM_SENSORS; i++)
// verhoog i, zolang i kleiner is dan het aantal sensors
{
Serial.print(qtrrc.calibratedMaximumOn);
// print de kalibratie tot een maximum bij een emitter pin die on is, bij sensor i
Serial.print(’ ');
// daarna een lege print
}
Serial.println();
// print data naar de seriele output, als menselijke taal via de ASCII tabel
Serial.println();
// print data naar de seriele output, als menselijke taal via de ASCII tabel
}
}