Progetto Arduino BART. Corso di Sistemi RT Prof. Davide Brugali Università degli Studi di Bergamo

Transcript

1 Progetto Arduino BART Corso di Sistemi RT Prof. Davide Brugali Università degli Studi di Bergamo

2 Ultrasonic sensor The ping sound pulse is generated when the pingpin level goes HIGH for two microseconds. The sensor will then generate a pulse that terminates when the sound returns. The width of the pulse is proportional to the distance the sound traveled The speed of sound is 340 meters per second, which is 29 microseconds per centimeter. The formula for the distance of the round trip is: RoundTrip = microseconds / 29 2

3 Ultrasonic sensor 3

4 class Ultrasonic class Ultrasonic { public: Ultrasonic(int TP, int EP); long timing(); long ranging(); private: int Trig_pin; int Echo_pin; long duration, distance; ; 4

5 Obstacle Detector #include "Ultrasonic.h" Ultrasonic left_sonar(3, 2); // left sonar connected to digital pin 3 and 2 Ultrasonic right_sonar(5, 4); // right sonar connected to digital pin 5 and 4 int d_dx, d_sx; // distances measured by the sonar sensors void setup() { Serial.begin(9600); void loop() { d_sx=left_sonar.ranging(cm); Serial.print("LX: "); Serial.print(d_sx); d_dx=right_sonar.ranging(cm); Serial.print(" RX: "); Serial.println(d_dx); delay(1000); 5

6 Line Following: seguilinea() void seguilinea() { dx = digitalread(rightpin); sx = digitalread(leftpin); if (dx == WHITE && sx == WHITE) { motore.setspeed_l(20); // 30 motore.setspeed_r(20); // 30 else if (dx == BLACK && sx == WHITE) { motore.setspeed_l(20); // 45 motore.setspeed_r(-12); // -7 else if (dx == WHITE && sx == BLACK ) { motore.setspeed_l(-12); // -7 motore.setspeed_r(20); // 45 6

7 Line Following : loop() void loop() { seguilinea(); delay(200); 7

8 Obstacle Avoidance Segui linea Segui linea Aggira ostacolo Obstacle Aggira ostacolo Aggira ostacolo Aggira ostacolo Segui linea 8

9 Line Following: macchina a stati Segui linea Strada libera Strada libera Ostacolo Aggira ostacolo Ostacolo 9

10 Line Following : macchina a stati 10 int d_dx, d_sx; // distanze misurate dai sensori int stato = 1; bool ostacolo; bool linea; void loop() { d_sx=left_sonar.ranging(cm); d_dx=right_sonar.ranging(cm); switch(stato) { case 1 : // segui linea if( d_sx > 0 d_dx > 0) : // trovato ostacolo ostacolo = true ; stato = 2; else seguilinea(ldx, Lsx); case 2 : // aggira ostacolo if(! ostacolo ) stato = 1; else ostacolo = aggiraostacolo(); delay(200);

11 Infrared radar

12 Obstacle Avoidance Obstacle 12

13 class Radar #include "Servo.h" #include "TimerOne.h" class Radar { Servo myservo; volatile static bool arrived; public: Radar(); void initialize(int servopin, int sensorpin); void goto(int angle); bool isarrived(); int getangle(); int getdistance(); 13

14 class Radar : goto(int angle) void Radar::goTo( int target) { Radar::arrived = false; current = _angle; _angle = target; current target myservo.write( target); Timer1.initialize(getPeriod(target)); Timer1.attachInterrupt(setArrived); void Radar::setArrived() { Timer1.stop(); Radar::arrived = true; bool Radar::isArrived() { return Radar::arrived; t0 t0 + period 14

15 Radar example 15 Radar radar; int stato = 1; int angolo= 0; int angolo_finale = 90; void loop() { switch(stato) { case 1 : // cambia la posizione angolare del motore radar.goto( angolo ); state = 2; case 2 : // attende che il motore sia arrivato if( radar.isarrived() ) state = 3; case 3 : // legge la distanza misurata dal sensore Serial.println( radar.getdistance() ); angolo += 10; if ( angolo > angolo_finale ) state = 4; else state = 1; case 4 : // fa altre cose delay(200);

16 class Radar #include "Servo.h" #include "TimerOne.h" class Radar { Servo myservo; volatile static bool arrived; public: Radar(); void initialize(int servopin, int sensorpin); void goto(int angle); bool isarrived(); int getangle(); int getdistance(); int measures[]; int scan(int angularstep); bool nextstep(); int getdistance(int i); NEW 16

17 Radar example Radar radar; int stato = 1; int num_steps = 0; void loop() { switch(stato) { case 1 : // inizializza la scansione num_steps = radar.scan( 15 ); // una misura ogni 15 state = 2; case 2 : // attende che la scansione sia terminata if( radar.nextstep() ) state = 3; case 3 : // legge le distanze misurate dal sensore for (int i = 0; i < num_steps; i++) { Serial.println( radar.getdistance(i) ); state = 4; case 4 : // fa altre cose delay(200); 17

18 Architettura distribuita Controllore Radar Aggira ostacolo Controllore Robot Mobile Segui linea Rileva ostacolo

19 Architettura distribuita Controllore Radar Aggira ostacolo Trasferimento controllo Comandi di velocità Controllore Robot Mobile Segui linea Rileva ostacolo

20 Architettura distribuita Controllore Robot Mobile Controllore Radar Segui linea Strada libera Idle Ricevi controllo Esegui movimenti Ostacolo / Passa controllo Comando Strada libera / Passa controllo Aggira ostacolo Ricevi controllo Ostacolo / Comando 20

21 Comunicazione seriale : Controllore Robot 21 void loop() { switch(stato) { case 1 : // Inizializzazione Serial.print( Motor-Init ); if(serial.available) { message = Serial.read(); if(message = Radar-Init ) state = 2; case 2 : // Segue la linea //... if(ostacolo) { Serial.print( Take-Control ); state = 3; case 3 : // Riceve comandi di velocità o trasferimento controllo if(serial.available) { //... delay(200);

22 Comunicazione seriale : Controllore Radar 22 void loop() { switch(stato) { case 1 : // Inizializzazione if(serial.available) { message = Serial.read(); if(message = Motor-Init ) { Serial.print( Radar-Init ); state = 2; case 2 : // Attende trasferimento di controllo if(serial.available) { message = Serial.read(); if(message = Motor-Init ) { Serial.print( Take-Control ); state = 3; case 3 : // Rileva l ostacolo e guida il robot Serial.print(velocity); delay(200);