gbm
Robot cartesiano e odometria con Arduino
Tutorials -
Venerdì 29 Aprile 2011 14:03
Scritto da gbm

 

Ciao a tutti ragazzi. Come avrete visto la mia partecipazione agli articoli è diminuita in questo periodo. Questo non perchè mi sia stufato, ma perchè stavo realizzando il mio primo robot cartesiano. Con questo tutorial vorrei aprire nuove possibilità ai possessori di semplici robot con servomotori, che,  avevano la sola possibilità di girare random per la casa con qualità di roving piuttosto contenute. Ho passato piu di due mesi a lavorare a un codice che permetta a qualsiasi robot con servi / microservi a rotazione continua di avere una percezione dello spazio percorso e della propria posizione rispetto a quella di partenza.

In poche parole ho realizzato un sistema informatico, che grazie all'integrazione dei volt presenti nel circuito di alimentazione dei motori, deduce lo spazio percorso, la rotazione sul posto e la propria posizione in uno spazio virtuale bidimensionale (piano cartesiano xy). 

Grazie a questo codice è per voi possibile a questo punto pilotare il robot con coordinate cartesiane. Allo stesso modo con un semplice link radio ottenere in real time la posizione del robot e degli eventuali ostacoli e renderli grafici con Processing, creare un robot plotter o un robot mazesolver pilotato a suon di xy. Diciamo che le possibilità che si aprono sono pressochè infinite e il gioco non smette subito come con i normali robot con servi a rotazione continua, che effettivamente prima di oggi erano piuttosto "limitati".

 

L'integrazione dei Voltaggi

Come spiegato nei tutorial precedenti per la modifica dei servomotori (modifica servomotori rotazione continua parte III), ho saldato due cavi ai due poli dei motori dc all'interno dei servomotori. Questo ci permetterà di percepire tramite le porte analogiche il voltaggio presente nel circuito di alimentazione. Il comportamento di questo feedback è il seguente: 

porta1 4.35v porta2 0.25v = avanti tutta

porta1 2.35v porta2 2.35v = motori fermi

porta1 0.25v porta2 4.35v = indietro tutta

Quello che bisogna fare ora è inquadrare quale coppia di 4 cavi che abbiamo saldato, avrà un voltaggio alto mentre il robot va avanti. Per fare cio' basterà richiamare i due comandi per muovere i servomotori (in uno sketch vuoto) e stampare in seriale i 4 valori dei 4 feedback saldati. 

Ecco il codice necessario, ricordatevi di includere la libreria Servo.h:

 Servo leftservo;  // create servo object to control a servo 
Servo rightservo;  // create servo object to control a servo 
 
void setup() { 
 leftservo.attach(9); 
 rightservo.attach(10); 
 Serial.begin(9600);
} 
 
void loop() { 
 leftservo.write(180);  //i due servi devono ruotare in modo che il robot vada avanti
 rightservo.write(0);                
 Serial.print("MOTORE sinistro - se alto v1Left ");
 Serial.print(analogRead(2));
 Serial.print(" se basso v2Left ");
 Serial.print(analogRead(3));
 Serial.print("MOTORE destro - se alto v1Right ");
 Serial.print(analogRead(4));
 Serial.print(" se alto v2Right ");
 Serial.println(analogRead(5));             
}

 

Come vedete basterà chiarire quale dei due poli ha un voltaggio alto durante il movimento in avanti per avere trovato il wiring corretto. Fatto questo utiliziamo l'integrazione rispetto al tempo per ottenere un valore "scalato" rispetto alla durata dell'acquisizione: 

movement = v1Right * (endTime - startTime);

Come vedete il polo con voltaggio alto, durante il movimento in avanti viene moltiplicato per la durata del loop.

 

La correzione del movimento delle ruote

Conoscendo la tensione nei due circuiti di alimentazione è possibile correggere il movimento delle ruote in modo da ottenere un moto perfettamente rettilineo o una rotazione delle ruote perfettamente coordinata. Per fare cio' Martino ha scritto le funzioni correctStop(); correctGo(); e correctRotation(); partendo dai miei vecchi sketch. Grazie a queste tre funzioni viene autoregolato il pilotaggio dei motori per ottenere movimenti fluidi e coordinati.

 

Il calcolo dello spazio percorso in mm

Sempre utilizzando il feedback di tensione del circuito di alimentazione, calcoliamo quanti mm ha percorso il robot. Quello che facciamo è integrare i voltaggi e inquadrare la somma di questi in modo da ottenere una completa e precisa rivoluzione della ruota. Per farlo ho creato una variabile:

float oneRotationValue = 1125000000; //travelled per un giro di ruota

Questa variabile contiene il valore della variabile travelled che corrisponde a un giro di ruota. Per fare debugging e setting comodo di questa variabile basterà richiamare questo comando: goForward(50, 251); dove 50 è il valore velocità tra 0 e 100 e 251 sono i mm che chiediamo al robot di percorrere (in questo caso le nostre ruote essendo diametro 8cm hanno una superficie di rotolamento di 251mm). Chiaramente è improbabile che il valore che io ho inserito corrisponda anche per voi a un giro di ruota, dovrete calibrarlo per ottenere una discreta precisione.

Il resto è semplice:

const float wheelDiameter = 80; // diametro ruota in mm
const float wheelCircumference = M_PI * wheelDiameter; // circonferenza ruota in mm
const float wheelsDistance = 134; // distanza tra le due ruote in mm
const float wheelsDistanceCircumference = M_PI * wheelsDistance;  // circ. rotazione 
const float oneDegreeTravel = wheelsDistanceCircumference / 360; // mm per ruotare 1°
const float millimeter = oneRotationValue / wheelCircumference; // trasformazione in mm

Come vedete vi basterà inserire il diametro della vostra ruota e la distanza tra le due ruote perchè il software calcoli tutto il resto (di questa cosa vado fierissimo Surprised). Grazie al diametro della ruota e al valore che abbiamo trovato precedentemente il software calcola quanti volt integrati corrispondono a 1mm, invece conoscendo la distanza tra le due ruote e il dato precedente calcola quanti volt integrati dovrà fare per ruotare di 1°.

 

La coscienza cartesiana

Chiarito il calcolo della distanza e della rotazione sul posto introduciamo il concetto di piano cartesiano e di angolo assoluto che permette al robot di calcolare la sua posizione attuale rispetto a quella di partenza (quando si accende il robot) e di conseguenza la funzione goXY(x, y);.

Per calcolare la rotazione necessaria per raggiungere il punto richiesto viene utilizzato l'arco tangente della differenza delle coordinate precedenti e quelle attuali:

 float angle = atan2(dy, dx) * (180 / M_PI);

Dopodichè utilizzando il semplicissimo teorema di pitagora viene calcolato lo spazio che bisognerà percorrere in moto rettilineo per raggiungere il punto richiesto:

 goForward(40, sqrt(square(dx) + square(dy)));

Come vedete non è cosi' complesso come sembra, anche se è stato un incubo ottenere questo risultato e devo dire devo ringraziare il mio grande amico Martino Difilippo per il debugging, l'idea di usare l'arcotangente piuttosto che seno e coseno ma sopratutto per la ripulita al codice Tongue out

Da questi due statement abbiamo sviluppato la funzione goXY(x,y); che permette di richiedere al robot di raggiungere un punto con coordinate cartesiane!!! 

Vediamo il codice completo:

(ricordate di includere le librerie Servo.h e Math.h)

// SENSORE ERER
#define irEmitter 13   // polo positivo emettitore - positive emitter pin
#define irReceiverN 3  // polo negativo ricevitore - negative receiver pin
volatile long ambient = 0;
volatile long emission = 0;
volatile long distance = 0;
volatile long lightTime = 0;
volatile int currentInit = 0;
volatile long gain = 4;       //gain crescita valore distanza
volatile long maxRangeValue = 666666;  //valore range massimo
volatile long maxRange = 2000; //distanza range massimo in mm
// CONTROLLO SERVI
Servo leftServo;  
Servo rightServo; 
//////////////////////
int v1Left = 0;  // val poloi servo sn - pole1 servo left
int v2Left = 0;  // val polo2 servo sn - pole2 servo left
int v1Right = 0;  // val polo1 servo dx - pole1 servo right
int v2Right = 0;  // val polo2 servo dx - pole2 servo right
//////////////////////
int leftThrottle = 90; // val servo sn - left servo variable
int rightThrottle = 90; // val servo dx - right servo variable
int leftStop = 90; // val stop servo sn - left servo stop calibrated value
int rightStop = 90; // val stop servo dx - right servo stop calibrated value
//////////////////////
float millimetersTravelled = 0;    // distanza percorsa in mm
float oneRotationValue = 1125000000; //travelled per un giro di ruota
//////////////////////
const float wheelDiameter = 80;// diametro ruota in mm - wheel diameter in mm
const float wheelCircumference = M_PI * wheelDiameter;// circonferenza ruota in mm
const float wheelsDistance = 134;// distanza tra le due ruote in mm
const float wheelsDistanceCircumference = M_PI * wheelsDistance;// circonferenza rotazione sul posto
const float oneDegreeTravel = wheelsDistanceCircumference / 360;// distanza in mm per ruotare di 1 grado
const float millimeter = oneRotationValue / wheelCircumference; // trasformazione in mm
//COSCIENZA CARTESIANA :P
float x = 0;  // attuale posizione sulle x
float y = 0;  // attuale posizione sulle y
float absoluteAngle = 0; // direzione attuale in gradi
////////////////////////

void setup() {
 leftServo.attach(9); 
 rightServo.attach(10); 
 pinMode(irEmitter, OUTPUT);     
 Serial.begin(9600);
 stopServos(); //diamo il comando stop ai servi
}

//////////////////////READ MOTOR POLES/////////////////////////////////////////
void readVs() { //leggiamo i 4 poli dei servi
  v1Left = analogRead(2);                       
  v2Left = analogRead(3);
  v1Right = analogRead(4);
  v2Right = analogRead(5);
}
//////////////////////CORRECT SERVO SPEEDS DURING MOVEMENTS////////////////////
void correctStop() {
 readVs();
 if(v1Left > v2Left){leftThrottle--;}
 if(v1Left < v2Left){leftThrottle++;}
 if(v1Right > v2Right){rightThrottle++;}
 if(v1Right < v2Right){rightThrottle--;}
}

void correctGo() {
 readVs();
 if(v1Left > v2Right){leftThrottle++;}
 if(v1Left < v2Right){leftThrottle--;}
}

void correctRotation(float dir) {
 readVs();
 if(dir > 0) {
  if(v1Left > v2Right){leftThrottle++;}
  if(v1Left > v2Right){leftThrottle--;}
 }
 else {
  if(v1Left > v2Right){leftThrottle--;}
  if(v1Left > v2Right){leftThrottle++;}
  }
}
////////////////////////WRITE SERVOS///////////////////////////////////////////
void writeServos() {
  // constrain servo speeds 0-180
  leftThrottle = constrain(leftThrottle, 0, 180);
  rightThrottle = constrain(rightThrottle, 0, 180);
  leftServo.write(leftThrottle);
  rightServo.write(rightThrottle);
}

void stopServos() {
  leftThrottle = leftStop;
  rightThrottle = rightStop;
  writeServos();
}
////////////////////////CALIBRAZIONE POSIZIONE DI STOP/////////////////////////
void stopCalibration() {
  for(int t = 0; t < 2000; t++) {
    correctStop();
    writeServos();
    delay(1);
  }
  // aggiorna le posizioni di stop
  leftStop = leftThrottle;  
  rightStop = rightThrottle;
}
//////////////////////FUNZIONE AVANZAMENTO/////////////////////////////////////
void goForward(int speed, float distance) {
 float travelled = 0, movement = 0;
 long startTime = 0, endTime = 0;
 int accelerating = 1;
 int leftSpeed = map(speed, 0, 100, leftStop, 180);
 int rightSpeed = map(speed, 0, 100, rightStop, 0);
 while(travelled < distance) {
   startTime = micros();
   correctGo();
   writeServos();
   if(accelerating) {
    if(leftThrottle < leftSpeed){leftThrottle += 5;}
    if(rightThrottle > rightSpeed){rightThrottle -= 5;}
    if((leftThrottle >= leftSpeed) || (rightThrottle <= rightSpeed)){accelerating = 0;}
   }
   endTime = micros();
   movement = v1Right * (endTime - startTime);
   travelled += movement / millimeter;
 }
 stopServos();
 float angle = (M_PI / 180) * absoluteAngle;
 x += travelled * cos(angle);
 y += travelled * sin(angle);
 millimetersTravelled += travelled;
}
//////////////////FUNZIONE ROTAZIONE///////////////////////////////////////////
void rotate(int angle) {
 float travelled = 0, movement = 0;
 long startTime = 0, endTime = 0;
 if(abs(angle - absoluteAngle) < 1){return;}
 if(abs(angle - absoluteAngle) > 180) {
  if(angle < 0){absoluteAngle -= 360;}
  else{absoluteAngle += 360;}
 }
 angle -= absoluteAngle;
 stopServos();
 leftThrottle = (angle > 0) ? 0 : 180;
 rightThrottle = (angle > 0) ? 0 : 180;  
 while(travelled < (wheelsDistanceCircumference / 360) * abs(angle)) {
  startTime = micros();
  correctRotation(angle);
  writeServos();  
  endTime = micros(); 
  movement = (angle > 0 ? v1Right : v2Right) * (endTime - startTime);
  travelled += movement / millimeter;
 }
 stopServos();
 absoluteAngle = absoluteAngle + angle;
}
//////////////////FUNZIONE XY//////////////////////////////////////////////////
void goXY(float x1, float y1) {
 float dx = x1 - x;
 float dy = y1 - y;
 if(abs(dx) < 2){dx = 0;}
 if(abs(dy) < 2){dy = 0;}
 if(dx == 0 && dy == 0){return;}
 float angle = atan2(dy, dx) * (180 / M_PI);
 rotate(angle);
 goForward(40, sqrt(square(dx) + square(dy)));
}
///////////////////////////SENSORE ERER////////////////////////////////////////
void focus() { 
 if(currentInit == 0){if(digitalRead(irReceiverN) == LOW){ ambientLightCheck();}}
 if(currentInit == 1){if(digitalRead(irReceiverN) == LOW){ reflectionLightCheck();}}
}

void ambientLightCheck() {
 ambient = micros() - lightTime;
 init(1);
}

void reflectionLightCheck() {
 emission = micros() - lightTime;
 distance = emission * (ambient / (ambient - emission));
 distance = map(distance, 0, maxRangeValue, 0, maxRange);
 if(distance <= 0) distance = 0;
 if(distance > 15) distance = sqrt(distance) * gain;
 Serial.println(distance); 
 emission = 0; 
 ambient = 0;
 init(0);
}

void init(int emitter) {
 if(emitter == 0)digitalWrite(irEmitter, LOW);   
 if(emitter == 1)digitalWrite(irEmitter, HIGH); 
 lightTime = micros();
 pinMode(irReceiverN, OUTPUT);
 digitalWrite(irReceiverN, HIGH); //carico ricevitore di induttanza
 pinMode(irReceiverN, INPUT);
 digitalWrite(irReceiverN, LOW);
 if(emitter == 1) currentInit = 1;
 if(emitter == 0) currentInit = 0;
}
//////////////////////////////LOOP////////////////////////////////////////////////
void loop() {
  goForward(40, 500);
  delay(5000);
  absoluteAngle = 0;
  x = 0; 
  y = 0;
  goXY(200, 0);
  goXY(200, 200);
  goXY(0, 200);
  goXY(0, 0);
  delay(5000);
}

Come vedete nel loop troviamo il comando che chiede al robot di andare al 40% della velocità per 500mm e dopo aver azzerato la sua posizione, richiediamo i 4 punti necessari per disegnare un quadrato di 20cm di lato (consiglio di seguire il video iniziale per vedere un risultato visuale di questo software).

 

Conclusione

Questo è un mero esempio, come puo' essere blink delle infinite possibilità di sviluppo di questo sistema. Con un semplice link radio / ir o anche via cavo per chi vuole risparmiare, comunicando al computer i dati ottenuti da questo software e utilizzando Processing sarà possibile per voi sviluppare sistemi odometrici e rappresentazioni grafiche del percorso effettuato, degli ostacoli trovati e di conseguenza effettuare una mappatura dell'ambiente circostante!! Ecco un esempio:

 

Creative Commons License
Continuous servo cartesian and odometry robot by Giovanni Blu Mitolo e Martino Difilippo is licensed under a Creative Commons Attribution-NonCommercial 3.0 Unported License.
Based on a work at www.gioblu.com.

 

Gioblu Robotics © 2010 - 2012 · Sitemap · privacy

gioscarab@gmail.com

Gioblu BOTServer è online dal 10 Aprile 2010 - 319.232 Visite - 1.027.175 Pagine visualizzate - 182.309 Visitatori unici - 536 utenti attivi