Zumo Line Follower

Hi everybody,

My name is léo, i’m 18 yo, i’m french and i live in Paris, so sorry if my english is not so good :laughing:

I made a robot line follower, it’s my project for my graduation :slight_smile:

So i use a Pololu Zumo kit, with a QTR-8A sensor , and an arduino Uno

here some photo :

For the code i use the PID control :

#include <Pushbutton.h>
#include <ZumoMotors.h>
#include <QTRSensors.h>

Pushbutton button(ZUMO_BUTTON);
ZumoMotors motors;

#define KP                   1
#define KD                   0
#define ML_DEFAULT_SPEED     350
#define MR_DEFAULT_SPEED     200
#define ML_MAX_SPEED         400
#define MR_MAX_SPEED         400
#define MIDDLE_SENSOR        3
#define NUM_SENSORS          6      
#define TIMEOUT              2500  
#define NO_EMITTER_PIN       2     
#define DEBUG                0 
#define FORWARD              400

QTRSensorsAnalog qtra((unsigned char[]){A0, A1, A2, A3, A4, A5}, 6);
unsigned int sensorValues[NUM_SENSORS];

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

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


void loop()
{ 
  
  unsigned int sensors[6];
  int position = qtra.readLine(sensors);
  int error = position - 3800;
  
  int motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;

  int leftMotorSpeed = ML_DEFAULT_SPEED + motorSpeed;
  int rightMotorSpeed = MR_DEFAULT_SPEED - motorSpeed;

  
  motors.setSpeeds(leftMotorSpeed, rightMotorSpeed);
  stop();
}

void set_motors(int motor1speed, int motor2speed)
{
  if (motor1speed > ML_MAX_SPEED ) motor1speed = ML_MAX_SPEED; 
  if (motor2speed > MR_MAX_SPEED ) motor2speed = MR_MAX_SPEED; 
  if (motor1speed < 0) motor1speed = 0; 
  if (motor2speed < 0) motor2speed = 0; 
  motors.setLeftSpeed(motor1speed);     
  motors.setRightSpeed(motor2speed);     
  
}


void manual_calibration() {

  
  button.waitForButton();
    delay(500);
    int i;
    pinMode(13, OUTPUT);
    digitalWrite(13, HIGH);    
  for (i = 0; i < 400; i++) 
      {
        qtra.calibrate();       
       }
  }
void stop(){
  if(button.isPressed()){
    
   motors.setSpeeds(0,0);
   
  }
}

Video :

youtu.be/C2oltmTaDuQ

here an other code without the PID control :

#include <Pushbutton.h>
#include <ZumoMotors.h>
#include <QTRSensors.h>
#define NUM_SENSORS               6  
#define NUM_SAMPLES_PER_SENSOR    4  
#define NO_EMITTER_PIN            2  
#define QTR_A_THRESHOLD           900
#define TURN_SPEED                300
#define TURN_SPEED2               150
#define FORWARD_SPEED             400

Pushbutton button(ZUMO_BUTTON);
ZumoMotors motors;


QTRSensorsAnalog qtra((unsigned char[]){A0, A1, A2, A3, A4, A5}, 6);
unsigned int sensorValues[NUM_SENSORS];


  
void setup()
{
  button.waitForButton();
    delay(500);
    int i;
    pinMode(13, OUTPUT);
    digitalWrite(13, HIGH);    
  for (i = 0; i < 400; i++) 
      {
        qtra.calibrate();       
       }
  digitalWrite(13, LOW);     
       }

void emittersOff(){
       }

void loop()
{
  unsigned int position = qtra.readLine(sensorValues);
  unsigned char i;
 if ( sensorValues[4] > QTR_A_THRESHOLD || sensorValues[1] > QTR_A_THRESHOLD){
      motors.setSpeeds(FORWARD_SPEED,FORWARD_SPEED);
      digitalWrite(13, LOW);
}
if ( sensorValues[3] > QTR_A_THRESHOLD && sensorValues[2] > QTR_A_THRESHOLD){
      motors.setSpeeds(FORWARD_SPEED,FORWARD_SPEED);
      digitalWrite(13, HIGH);
}
if ( sensorValues[5] > QTR_A_THRESHOLD ){
      motors.setSpeeds(TURN_SPEED,-TURN_SPEED2);
      delay(200);
      digitalWrite(13, LOW);
}

if ( sensorValues[0] > QTR_A_THRESHOLD ){
       motors.setSpeeds(-TURN_SPEED2,TURN_SPEED);
       delay(200);
      digitalWrite(13, LOW);
}

}

video :

youtu.be/-x3VjQHG4jA

you see that he isn’t really fast… i’m a newbie in coding.
[b]So if you have idea for code optimization or hardware optimization tell me !

thanks ! [/b]:smiley: :smiley: :smiley:

Hello.

Awesome project. It looks like you are already running the motors at full speed in your code. Could you tell me more about your setup? Which gear ratio did you choose for your Zumo? Are you using the HP version of the motors? What batteries are you using?

- Jeremy

Hi,

I use 50:1 gearmotor non-hp, and simple duracell batteries
One of the two motor doesn’t works very well, i think i damaged the motor with solder :frowning:

If one of your motors is damaged, I recommend you upgrade to the 50:1 HP version to get more power.

- Jeremy

This code would not work with the sensor I have (ZumoReflectanceSensorArray).
I modified the calibration code with some modified 3pi code to come up with this.
It is not perfect but it is functional. Feel free to change the numbers to alter the behavior (speed, turning) of Zumo while line following.
Sorry, I wanted to set up an auto calibration, but with this code you still have to do it by hand and I included the pushbutton and didn’t use it for anything.
My zumo : 100:1 micro metal gear motors
zumo shield and chasis kit
zumo reflectance sensor array
arduino UNO
Code : Compiled and running from Arduino IDE
Thanks Atmel 3pi library
Thanks Pololu Zumo library

#include <ZumoMotors.h>
#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <Pushbutton.h>


ZumoReflectanceSensorArray reflectanceSensors;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);
#define set_motors() = (setLeftSpeed,setRightSpeed);
#define NUM_SENSORS 6
unsigned int sensorValues[NUM_SENSORS];

void setup()
{
  reflectanceSensors.init();

  delay(500);
  pinMode(13, OUTPUT);
  pinMode(2, OUTPUT);
  digitalWrite(13, HIGH);        // turn on LED to indicate we are in calibration mode
  pinMode(2,HIGH);
  unsigned long startTime = millis();
  while(millis() - startTime < 10000)   // make the calibration take 10 seconds
  {
    reflectanceSensors.calibrate();
    
  }
  digitalWrite(13, LOW);         // turn off LED to indicate we are through with calibration

  // print the calibration minimum values measured when emitters were on
  
  for (byte i = 0; i < NUM_SENSORS; i++)
  {
    reflectanceSensors.calibratedMinimumOn[i];
    
  }
  
  
  // print the calibration maximum values measured when emitters were on
  for (byte i = 0; i < NUM_SENSORS; i++)
  {
    reflectanceSensors.calibratedMaximumOn[i];
   
  }

  delay(1000);
}

void loop()
{
	unsigned int sensors[6]; // an array to hold sensor values
	unsigned int last_proportional=0;
	long integral=0;

	

	
	while(1)
	{
		// Get the position of the line.  
		// the "sensors" argument to read_line even though we
		// are not interested in the individual sensor readings.
		unsigned int position = reflectanceSensors.readLine(sensorValues);

		// The "proportional" term should be 0 when we are on the line.
		int proportional = ((int)position) - 2500;

		// Compute the derivative (change) and integral (sum) of the
		// position.
		int derivative = proportional - last_proportional;
		integral += proportional;

		// Remember the last position.
		last_proportional = proportional;

		// Compute the difference between the two motor power settings,
		// m1 - m2.  If this is a positive number the zumo will turn
		// to the right.  If it is a negative number, the robot will
		// turn to the left, and the magnitude of the number determines
		// the sharpness of the turn.
		int power_difference = proportional/5 + integral/10000 + derivative*3/2;

		// Compute the actual motor settings.  We never set either motor
		// to a negative value.
		const int max = 400;
		if(power_difference > max)
			{power_difference = max;}
		else if(power_difference < -max)
			{power_difference = -max;}

		else if(power_difference < 0)
			{motors.setLeftSpeed(max+power_difference);
                        motors.setRightSpeed (max);}
		else 
			{motors.setLeftSpeed(max);
                        motors.setRightSpeed (max-power_difference);}
	}

	
}

Oh yeah, I guess this is the PID line follower.
Have fun and hope to see more Zumo Codes. :smiley:

Thanks for this guys! I was able to cobble together a line follower using the Zumo shield and Zumo line sensor based on this code in short order which I was using to demo to some middle school kids 2 weeks ago.

hello… i have tried using the example of zumo library (line following) to run the robot. however, it doesnt give an expected result. the robot doesn’t moves forward and shaking only. should i change the gain of parameters, symmetrize motor, and change unit or only the the value of PID?

In order to work perfectly, you have to change the value of PID, it depend of width of the line and the speed of the robot.

Here you have my final code that work great !

You have to change KP, KD and the speed.

#include <Pushbutton.h>
#include <ZumoMotors.h>
#include <QTRSensors.h>

Pushbutton button(ZUMO_BUTTON);
ZumoMotors motors;

#define KP                      .3
#define KD                      3
#define ML_DEFAULT_SPEED        400
#define MR_DEFAULT_SPEED        400
#define NUM_SENSORS             6      
#define NUM_SAMPLES_PER_SENSOR  4  
#define EMITTER_PIN             2 

QTRSensorsAnalog qtra((unsigned char[]){A0, A1, A2, A3, A4, A5},  NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValue[NUM_SENSORS];

void setup()
{
  
  manual_calibration(); 
  
}

int lastError = 0;

void loop()
{ 
  stop();
  unsigned int sensors[6];
  int position = qtra.readLine(sensors);
  int error = position - 2500;
  
  int motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;

  int leftMotorSpeed = ML_DEFAULT_SPEED + motorSpeed;
  int rightMotorSpeed = MR_DEFAULT_SPEED - motorSpeed;
  motors.setSpeeds(leftMotorSpeed, rightMotorSpeed);
  
  
}



void manual_calibration() {

  
  button.waitForButton();
    delay(500);
    int i;
    pinMode(13, OUTPUT);
    digitalWrite(13, HIGH);    
  for (i = 0; i < 400; i++) 
      {
        qtra.calibrate();       
       }
 digitalWrite(13, LOW); 
}

if you want to see how well my robot work, tell me, and i will upload a video !

hello,
I have your same error. Did you solve it by your own or by following the (for me not understandable) coding instructions that Ryokean indicated to you?
Thank you in advance for your reply
Francesco