R2 - RoboRoma La prima comunità romana di sviluppo di progetti elettronici
basati su piattaforma Arduino. |
Robot Line Follower
Un line follower è un robot che segue una linea, sia essa bianca o nera.
La realizzazione di un Line Follower è alla portata di un principiante. Infatti il line follower è forse il robot più semplice da realizzare e
per portarlo a termine sono necessari pochissimi componenti:
-
Sensori
a raggi infrarossi che sappiano distinguere le differenze di colore (bianco e nero)
-
Due motori (servomotori o motoriduttori)
-
Un Arduino per pilotare i motori e implementare la logica di controllo
-
Batterie per far funzionare il tutto
Nel caso in cui si utilizzino i motoriduttori è necessario utilizzare anche un circuito di alimentazione e pilotaggio dei motori.
I SENSORI
I sensori che si utilizzano nei line follower sono a infrarossi e sono formati da due componenti: un emettitore e un ricevitore di luce.
Quando l’emettitore emette luce, questa viene riflessa dal pavimento. Se la linea è ad esempio bianca e il sensore si trova proprio sopra la linea bianca, questa rifletterà tutta la luce, permettendo di capire che si è proprio sopra la linea. Invece se il sensore si trova sopra la superficie nera, la luce riflessa sarà molto poca.
Sapendo sotto quale sensore è la linea, si può capire se il robot la sta seguendo correttamente oppure se sta derivando a destra o a sinistra. Per poter capire di quanto il robot deriva è meglio utilizzare più sensori. Nella figura sopra riportata vi sono 5 sensori, tuttavia non è inusuale trovarne 8 o anche di più. Il minimo sindacale sono 3 sensori.
IL CONTROLLO
Il robot, soprattutto se la linea che si deve seguire non è dritta, tenderà a derivare rispetto ad essa. E’ pertanto importante istruire il robot affinchè cerchi di mantenersi sempre sopra alla linea. Per far questo solitamente si utilizza un regolatore PID (proporzionale-integrale-derivativo) che cercherà di compensare l’errore, cioè la deriva del robot rispetto al centro della linea da seguire. Qualche dettaglio sui PID sulla solita
wikipedia.
Questa immagine spiega quale è il criterio utilizzato per il controllo:
Sostanzialmente viene utilizzata l’azione combinata dei sensori per comprendere se la correzione del controllo deve essere più o meno lieve.
IL ROBOT LINE FOLLOWER
Il robot presentato all’Arduino day è costituito da un sensore da 8 celle IR della Pololu (
link) dal costo contenuto (circa 14 euro). E’ corredato di una libreria per Arduino già resa disponibile da Pololu che risolve tutti i problemi di interfacciamento.
La base del robot è costituita da un modulo a basso costo della DFRobot (meno di 30 euro) che offre anche i motori, le staffe dei motori e le ruote:
Questo è il link a
DFRobot.
Il driver che pilota i motori è un driver della sparkfun a bassissimo costo (8 euro) che pilota motori fino a 1.2 ampere. Supporta due motori in contemporanea e permette il controllo PWM della velocità.
Per comodità ho montato il driver dei motori su una breadboard shield, tuttavia è sufficiente una normalissima breadboard. I restanti componenti (parte superiore del telaio, ruotino, supporto per il sensore, batterie) sono auto costruiti o comprati da Leroy Marlin.
Ecco il robottino:
Ecco un breve video che mostra il robottino in azione:
http://www.youtube.com/watch?v=GCxc6LML5wo
Ecco il sw che pilota il robot line follower:
#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
PololuQTRSensorsRC qtrrc((unsigned char[]) {2, 12, 11, 10, 17, 16, 15, 14} , NUM_SENSORS, TIMEOUT, 255);
unsigned int sensors[NUM_SENSORS];
{
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;
// 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);
}
}
// ------------------ 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);
}
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);
}
}
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);
}
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
for (i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
stopMotors();
delay(2000);
}
Creato da AleGiaco (2011)
Ultimo aggiornamento: 26 aprile 2011