Arduino line follower, code

Development Boards Arduino
1

Hello

Can someone explain each line of this code, please?

#include <QTRSensors.h>

#include <AFMotor.h>

AF_DCMotor motor1(1, MOTOR12_8KHZ);

AF_DCMotor motor2(2, MOTOR12_8KHZ);

#define KP .2

#define KD 5

#define M1_DEFAULT_SPEED 150

#define M2_DEFAULT_SPEED 150

#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

#define DEBUG 0

QTRSensorsRC qtrrc((unsigned char) { 8,7,6,5,4} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

void setup()

{

delay(1000);

manual_calibration();

set_motors(0,0);

}

int lastError = 0;

int last_proportional = 0;

int integral = 0;

void loop()

{

unsigned int sensors[5];

int position = qtrrc.readLine(sensors);

int error = position - 2000;

int motorSpeed = KP * error + KD * (error - lastError);

lastError = error;

int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;

int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;

set_motors(leftMotorSpeed, rightMotorSpeed);

}

void set_motors(int motor1speed, int motor2speed)

{

if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;

if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;

if (motor1speed < 0) motor1speed = 0;

if (motor2speed < 0) motor2speed = 0;

motor1.setSpeed(motor1speed);

motor2.setSpeed(motor2speed);

motor1.run(FORWARD);

motor2.run(FORWARD);

}

void manual_calibration() {

int i;

for (i = 0; i < 250; i++)

{

qtrrc.calibrate(QTR_EMITTERS_ON);

delay(20);

}

if (DEBUG) {

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();
}
}
kelly.vanloon

Berichten: 3
Geregistreerd: za nov 24, 2012 12:00 pm

2

Why don’t you tell us what lines you don’t understand. It’ll save us a lot of typing and you won’t look like a lazy person who’s asking to be spoon fed everything.

3

Mee_n_Mac:
Why don’t you tell us what lines you don’t understand. It’ll save us a lot of typing and you won’t look like a lazy person who’s asking to be spoon fed everything.

I just need to know which piece:

put the motors on,

put the line sensor on

ensures that the sensor detects the line

and which piece that ensures that the robot turns.

I do my best to speack English, because I’m from Belgium

Thanks in advance

4

See the comments I’ve added to the code (below). There is no code here that applies power to the motors or line sensor. There are some library functions which have code that does the low level job of formatting, sending and getting data to and from the sensor and setting (I GUESS) the PWM outputs that actually control the motor(s) speed(s). You don’t see those library functions here.

This code gets a position from the sensor(s). It then implements a basic control loop by computing a raw position error (how far off the line is the robot), low pass filters that error to smooth out the robot’s response and then computes a speed increment to be added or subracted to each motor. The difference between the left motor speed and the right motor speed cause the robot to turn so as to reduce the position error.

No code “ensures” the sensor sees the line. It’s assumed the line is detected and a position relative to that line (and error) is reported to this code via this command. I don’t know what happens if the robot doesn’t detect the line.

  int position = qtrrc.readLine(sensors);  //reads the sensor to determine position
  int error = position - 2000;             //computes how far off the line the 'bot is

That position error is filtered and turned into a motor speed correction by these lines.

  int motorSpeed = KP * error + KD * (error - lastError); //calculates the motor speed correction
  lastError = error;

Those speed corrections are turned into new motor speed commands here.

  int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;  //calculates the new left motor speed using the correction
  int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed; //calculates the new right motor speed using the correction

  set_motors(leftMotorSpeed, rightMotorSpeed); // calls the routine that commands the motor speed

The routine called above checks that the commands are within range (>0, < some maximum speed) and then calls a library function you don’t have listed.

void set_motors(int motor1speed, int motor2speed) //the routine checks the motor speeds aren't too low or too high
{
  if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;
  if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;
  if (motor1speed < 0) motor1speed = 0;
  if (motor2speed < 0) motor2speed = 0;
  motor1.setSpeed(motor1speed);
  motor2.setSpeed(motor2speed);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
}

Commented and indented …

#include <QTRSensors.h>
#include <AFMotor.h>

AF_DCMotor motor1(1, MOTOR12_8KHZ);
AF_DCMotor motor2(2, MOTOR12_8KHZ);

// tells the compiler the values of various constants
#define KP .2
#define KD 5
#define M1_DEFAULT_SPEED 150
#define M2_DEFAULT_SPEED 150
#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
#define DEBUG 0


QTRSensorsRC qtrrc((unsigned char[]) {8,7,6,5,4}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

void setup()
{
  delay(1000);
  manual_calibration();
  set_motors(0,0);
}

int lastError = 0;
int last_proportional = 0;
int integral = 0;


void loop()
{
  unsigned int sensors[5];
  int position = qtrrc.readLine(sensors);  //reads the sensor to determine position
  int error = position - 2000;                   //computes how far off the line the 'bot is

  int motorSpeed = KP * error + KD * (error - lastError); //calculates the motor speed correction needed
  lastError = error;

  int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;  //calculates the new left motor speed using the correction
  int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed; //calculates the new right motor speed using the correction

  set_motors(leftMotorSpeed, rightMotorSpeed); // calls the routine that commands the motor speed
}

void set_motors(int motor1speed, int motor2speed) //the routine checks the motor speeds aren't too low or too high
{
  if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;
  if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;
  if (motor1speed < 0) motor1speed = 0;
  if (motor2speed < 0) motor2speed = 0;
  motor1.setSpeed(motor1speed);
  motor2.setSpeed(motor2speed);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
}


void manual_calibration() {

  int i;
  for (i = 0; i < 250; i++)
  {
    qtrrc.calibrate(QTR_EMITTERS_ON);
    delay(20);
  }

  if (DEBUG) {
    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();
  }
}