Crea sito

Archive for June, 2011

Linea – Line follower

 

Linea is my first line follower based on Arduino.

A line follower is a robot for beginners. In fact it is very simple to build a line follower and only few components are required :

– a robot chassis

– IR sensors for line tracking (black or white line)

– two motors (servos or gear motors)

– an Arduino to drive the motors and to implement control logic

– Batteries, jumpers, screws and other hardware

If you use  gear motors it is important to use also a motors driver, because Arduino can’t feed the motors with the high current required.

 

IR SENSORS

The sensors used in line follower robot are Infrared sensors and they are made with 2 different components:

– a light emitter

– a light receiver

When the emitter emits light, the light is reflected by the floor. If the line is white and the sensor is above the line, the white line reflect all the light, so it permits to understand that the sensor is effectively above the line. Instead if the sensor is over the black surface, the sensor doesn’t reflect the light, so we can understand that the sensor isn’t above the line.
If we have many IR sensors, knowing which sensor is above the line, lets to understand if the robot is going out from the middle of the line,  at the right or the left side. In the picture above are shown 5 sensors, but is not unusual to use 8 or 10 sensors. The minimum required is 3 sensor (left, straight, right).

 

The CONTROL

The robot, mostly if the line is curved, will go out of the path. It is important to teach to the robot how to maintain above the line. Usually, it is possible to do this with a motion control and the PID (Proportional-derivative-integral) control is the most famous. This control will try to compensate the error, i.e. the misalignment from the line. You can find some details about PID in wikipedia.

This picture shows the criteria used for the control:

 

The combined readings from the sensors are used to understand if the correction done by the control have to be hard or soft.

 

Linea – The line follower robot

This robot was shown on the Arduino Day, in Rome (Italy), in April 2011 and  use a Pololu sensor with 8 IR leds ( link), very cheap (14 euros). It is offered with an Arduino library, made by Pololu. This library resolves all the interfacing problems with Arduino (yeah!).

 

 

The robot chassis is build using a cheap DFRobot module (link) that for less than 30 euros, offers also 2 gear motors, 2 motor supports and 2 wheels:

The motor driver is a Sparkfun module (link) , very very cheap (8 euros), that can feed up to 1.2A. It supports 2 gear motors and lets the PWM usage.

For my convenience I assembled the motor drivers on a breadboard shield, but it is enough a normal breadboard. Others components (ball caster, sensor support, batteries) are self made or bought in a hardware shop.

This is Linea:

 

 

 

Here Linea in action:

 

This is the code used:

#include <PololuQTRSensors.h>
// motor driver pin
#define out_STBY 6
#define out_B_PWM 9
#define out_A_PWM 3
#define out_A_IN2 4
#define out_A_IN1 5
#define out_B_IN1 7
#define out_B_IN2 8
#define motor_A 0
#define motor_B 1
#define motor_AB 2

#define NUM_SENSORS 8 // number of sensors used - numero di sensori usati
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low - aspetta 2500 us prima di mandare low i sensori
  //define QTR pololu sensor
PololuQTRSensorsRC qtrrc((unsigned char[]) {2, 12, 11, 10, 17, 16, 15, 14}
 , NUM_SENSORS, TIMEOUT, 255);
unsigned int sensors[NUM_SENSORS];
  void setup()
{
  Serial.begin(115200);
  // define motor drive output pin - definisci i pin di output del motore
  pinMode(out_STBY,OUTPUT);
  pinMode(out_A_PWM,OUTPUT);
  pinMode(out_A_IN1,OUTPUT);
  pinMode(out_A_IN2,OUTPUT);
  pinMode(out_B_PWM,OUTPUT);
  pinMode(out_B_IN1,OUTPUT);
  pinMode(out_B_IN2,OUTPUT);
  motor_standby(false);// motor stand-by off and stop - togli i motori dallo stand-by e fermali
  stopMotors();
  delay(2000);
  SensorCalibration(); // sensor calibration - calibra i sensori
  delay(2000);
}

void loop()
{
  follow_segment(); // follow the black line - segui la linea nera
}
// PID parameters
int Kp=120;
int Ki=100;
int Kd=100;
// follow the black line - segui la linea nera 
void follow_segment()
{
  int last_proportional = 0;
  long integral=0;
  int primo=0;
  int posizione = qtrrc.readLine(sensors); // read the IR sensors - leggi i sensori IR 
  long int proportional = posizione;
  int P = proportional * Kp/1000; // Proportional parameter 
  long int derivative = proportional - last_proportional; //Derivative parameter
  if (primo==0) // firt cycle set derivative to zero - al primo giro imposta il derivativo a zero
     {
      primo=1;
      derivative=0;
    }
  int D=derivative*Kd/1000;
  integral += proportional;
  int I=integral*Ki/1000; //integral parameter 
  // Remember the last position - ricorda l'ultima posizione
  last_proportional = proportional;
  int PID =P + I + D; //calculate the PID 
  // print some values on the serial port - scrivi qualche valore sulla seriale
  unsigned char zz;
   for (zz = 0; zz < NUM_SENSORS; zz++)
    {
      Serial.print(sensors[zz]);
      Serial.print(' ');
    }
    Serial.print(" position: ");
    Serial.print(posizione);
    Serial.print(" P:");
    Serial.print(P);
    Serial.print(" I:");
    Serial.print(I);
    Serial.print(" D:");
    Serial.print(D);
    Serial.print(" PID:");
    Serial.print(PID);
  // set the values for the motors based on the PID gain - setta i valori del motore basati sul PID
    const int maximum = 40; // the maximum speed
     if (PID > 0){
       if (PID > maximum) PID=maximum;
       motor_speed2(motor_B,maximum);
       motor_speed2(motor_A,maximum-PID);
      Serial.print(" Motore_B:");     Serial.print(maximum);
    Serial.print(" Motore_A:");      Serial.print(maximum-PID);
      }
      else {
       if (PID < -maximum) PID=-maximum;
       motor_speed2(motor_B,maximum+PID);
        motor_speed2(motor_A,maximum);
      Serial.print(" Motore_B:");     Serial.print(maximum+PID);
    Serial.print(" Motore_A:");      Serial.print(maximum);
      }
      Serial.println("");
}
// ------------------ stop the robot - ferma il robottino ----------------------- 
void stopMotors(){
  motor_brake(motor_A);
  motor_brake(motor_B);
}
// ------------------ Motor driver mgmt routines - routine per gestire i motori ----------------------- 
void motor_speed2(boolean motor, char speed) { //speed from 0 to 100
  if (motor == motor_A)
   speed = -speed;
  byte PWMvalue=0;
  PWMvalue = map(abs(speed),0,100,50,255); //anything below 50 is very weak
  if (speed > 0)
    motor_speed(motor,0,PWMvalue);
  else if (speed < 0)
    motor_speed(motor,1,PWMvalue);
  else {
    motor_coast(motor);
  }
}
void motor_speed(boolean motor, boolean direction, byte speed) { //speed from 0 to 255
  if (motor == motor_A) {
    if (direction == 0) {
      digitalWrite(out_A_IN1,HIGH);
      digitalWrite(out_A_IN2,LOW);
    } else {
      digitalWrite(out_A_IN1,LOW);
      digitalWrite(out_A_IN2,HIGH);
    }
    analogWrite(out_A_PWM,speed);
  } else {
    if (direction == 0) {
      digitalWrite(out_B_IN1,HIGH);
      digitalWrite(out_B_IN2,LOW);
    } else {
       digitalWrite(out_B_IN1,LOW);
      digitalWrite(out_B_IN2,HIGH);
    }
    analogWrite(out_B_PWM,speed);
  }
}
// stand-by motor
void motor_standby(boolean state) { //low power mode
  if (state == true)
    digitalWrite(out_STBY,LOW);
  else
    digitalWrite(out_STBY,HIGH);
}
// stop the motors
void motor_brake(boolean motor) {
  if (motor == motor_A) {
    digitalWrite(out_A_IN1,HIGH);
    digitalWrite(out_A_IN2,HIGH);
  } else {
    digitalWrite(out_B_IN1,HIGH);
    digitalWrite(out_B_IN2,HIGH);
  }
}
// motors coast - fai girare i motori in folle
void motor_coast(boolean motor) {
  if (motor == motor_A) {
    digitalWrite(out_A_IN1,LOW);
    digitalWrite(out_A_IN2,LOW);
    digitalWrite(out_A_PWM,HIGH);
  } else {
    digitalWrite(out_B_IN1,LOW);
     digitalWrite(out_B_IN2,LOW);
    digitalWrite(out_B_PWM,HIGH);
  }
}
// IR sensor calibration 
void SensorCalibration() {
  int counter, i;
  for (counter=0; counter<40; counter++)
  {
    if (counter < 10 || counter >= 30) {
      motor_speed2(motor_A,10);
      motor_speed2(motor_B,-10);
    }
    else   {
      motor_speed2(motor_A,-10);
      motor_speed2(motor_B,10);
    }
    qtrrc.calibrate();
    // Since our counter runs to 80, the total delay will be
    // 80*20 = 1600 ms.
    delay(20);
  }
     // print the calibration minimum values measured when emitters were on
    for (i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
    // print the calibration maximum values measured when emitters were on
  for (i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMaximumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
  Serial.println();
  stopMotors();
  delay(2000);
}

Tags: , ,

Geronimo – Robot with obstacles avoidance


The first robot presented is called “Geronimo” and it is a simple autonomous robot with an ultrasonic sensor for obstacles avoindance.
This robot can be built by a newbie of robotics.

 

 

Components:

The robot is assembled starting from these components:

arduino duemilanove
robot chassis (link)
‘L’ support for servos   (link)
servos Hitec HRS
1422CR with continuous rotation (link)
Hitec wheels for servos (link)
ball caster  (link)
ultrasonic sensor DFRobot URM37 V 3.2 (link)
servo for ultrasonic sensor (the cheapest in ebay)
400 contacts breadboard (link)
batteries, jumpers, screw and other simple hardware

 

Functionalities:

The robot, very basic and simple, goes straight and verifies if there are obstacles. If it found some obstacle, it turns the ‘head’ and sees at the right and at the left in order to verify where the street is more free.

Then it goes where the street is more free. Nothing of special. This is a short video to see how the robot goes in a  path with obstacles:

Another video:

 

I liked very much the ultrasonic URM37. Libraries (link) that drive the sensor are available and they are easy to use. The sensor is advanced: in fact it can offer these functionalities:

– distance measurement between 4 cm and 3 meters

– temperature measurement

– sensor servo driving and control

This is the sensor:

At the beginning it seemed very interesting the functionality of servo driving offered out-of-the-box by the sensor,  however making some test I couldn’t to drive well the servo, that it went from a part to another part at maximum speed. So, I used directly Arduino to drive also the sensor servo. I think that spending more time, it is sure possible to make something better.

For other components, some comments:

  • Chassis and ‘L’ supports. Perfect, nothing to say
  • Servos Hitec: not very well. Unfortunely they don’t have a pot to regulate the stall (I mean to stop them), so I have tried with some tests. Finally I discovered the stall value. Another issue consist in turning the servos to a precise angle. The only method that I could use is many, many tentatives. I know that is a common problem for continuous rotation servos.
  • Batteries and Arduino: I used two batteries, because the Arduino hasn’t enough current to feed the servos.  When I divided the power, I hadn’t problems.

Some photo of robot process building:

 

 

 


 

This is the Arduino Code used for robot programming:
#include "URMSerial.h"
#include <Servo.h>
Servo sservo;
Servo dservo;
Servo uservo;
int ang=0;
int angolomax=0;
int distmax=0;
int value=100;
int ind=0;
int rotazione=0;

// Definizione delle misure - Measures definition
#define DISTANCE 1
#define TEMPERATURE 2
#define ERROR 3
#define NOTREADY 4

//angolo di riposo dei servi delle ruote - wheels stop angle
#define RIPOSOS 94
#define RIPOSOD 93

// inizializza l'oggetto della libreria urm del sensore ultrasuoni - initialize urm library 
URMSerial urm;

void setup()
{
  Serial.begin(9600);  // baud rate at 9600 
  urm.begin(9,10,9600); // RX Pin, TX Pin, Baud Rate
  sservo.attach(6);   // left servo to pin 10
  dservo.attach(5);    // Right servo to pin 9
  uservo.attach(3);    // Ultrasonic servo to pin 3
  uservo.write(90);    // put ultrasonic servo straight
  delay(100);
}

void loop()
{
//   sservo.write(RIPOSOS); // stop the robot
//   dservo.write(RIPOSOD); // stop the robot
  sservo.write(RIPOSOS + 90); // go straight
  dservo.write(RIPOSOD - 90); // go straight
 // Richiedi la distanza al sensore ad ultrasuoni - ask ultrasonic sensor for distance
 urm.requestMeasurement(DISTANCE);
 delay(50); // attendi allineamento dei servi - wait for servos alignement
 if(urm.hasReading())
  {
     switch(urm.getMeasurement(value)) // Verifica che il valore letto sia una distanza 
     {
      case DISTANCE: // Ulteriore verifica che sia una distanza - verify that read is a distance
        if (value < 20 & value > 0)  // se il valore e' < 20 cm stiamo per sbattere - if < 20 cm turn
        {
          angolomax=0; // imposta l'angolo di migliore uscita a zero - set best exit angle to zero
          distmax=0; // imposta la distanza di uscita migliore a zero - set best distance exit to zero
          sservo.write(RIPOSOS);  // ferma il servo in posizione di riposo - stop servo
          dservo.write(RIPOSOD);  // ferma il servo in posizione di riposo - stop servo
          delay(100);

          for (ang=0; ang<180; ang +=5) // obstacles control
            {
              uservo.write(ang); // set ultrasonic sensor on ang value
              urm.requestMeasurement(DISTANCE); //ask for new reading
              delay(20);
              if(urm.hasReading()) // verifica se ha letto - verify the reading
                {
                  switch(urm.getMeasurement(value))
                    {
                      case DISTANCE:
                        if (value > distmax) // verify if distance is > max
                          {
                             angolomax=ang;  // set new max angle
                             distmax=value;  // set new distance max
                           } //end if (value > distmax)
                      break;
                    } // end switch
                 } // end if(urm.hasReading())
            } //end for (ang=0; ang<180; ang +=5)         

        uservo.write(90); //set the URM servo straight
        delay(135);
        // write angle read in the serial port
        Serial.print("Angolo max \t");
        Serial.println(angolomax);

        //set servos to max distance

        if (angolomax > 180)
          angolomax=180;
        if (angolomax < 0)
          angolomax=0;

        rotazione=abs(angolomax-90); 

        // write value to serial port
        Serial.print("Angolo inviato ai servi \t");
        Serial.println(RIPOSOS+angolomax-90);
        Serial.print("Valore di rotazione \t");
        Serial.println(rotazione / 9);
        // go servos to max angle and max distance
        for (ind=1; ind < rotazione / 4; ind+=1)
         {
           sservo.write(RIPOSOS+angolomax-90);
           dservo.write(RIPOSOD+angolomax-90);
           delay (40);
         } // end for (ind=1; ind < rotazione / 9; ind+=1)
        } //  end if (value < 20 & value > 0)
        break;
     } // end switch
  } // end if(urm.hasReading())

} //end loop

Tags: , , ,