Robot slalom ostacoli

4
//***** ROBOT slalom ostacoli ***** //***** librerie da includere #include <IRremote.h> // carica libreria IR - Ken Shirriff http://arcfn.com //***** variabili usate per lo slalom // velocità valide con batterie ben cariche #define GIRA_SX 190 // velocità di sterzata #define GIRA_DX 185 // velocità di sterzata #define DRITTO_A 230 // velocità motore sinistro dritto #define DRITTO_B 240 // velocità motore destro dritto byte t0, t1, t_tot; // memorizza i tempi di percorrenza byte fase = 0; // memorizza in che fase si trova il robot //***** variabili usate dal ricevitore IR ***** int RECV_PIN = 10; // Arduino pin D10 collegato a IR OUT IRrecv irrecv(RECV_PIN); // imposta il pin di ricezione dei dati decode_results results; // unsigned long code; // memorizza il codice ricevuto // i codic sono del telecomando marca LG // ai codici ricevuti aggiungere 0x + codice #define STOP 0x00 // comando stop #define KEY_STOP 0xCA9582FA // codice del tasto STOP #define SLALOM 0x11 // comando calcola perimetro #define KEY_1 0xDB2CD85E // codice del tasto 1 #define CERCHIO_SX 0x22 // comando stampa risultato #define KEY_2 0x72C8CD04 // codice del tasto 2 #define CERCHIO_DX 0x33 // comando stampa risultato #define KEY_3 0xBFA12A36 // codice del tasto 3 //***** variabili usate dal sonar #define ECHO 12 // Arduino pin D12 - SRF04 pin 4 #define TRIG 11 // Arduino pin D11 - SRF04 pin 3 unsigned long echo_time = 0; // tempo di ritorno dell'echo sonar in us #define DISTANCE_CM echo_time / 48 // calcola la distanza in centimetri unsigned long go_sonar = 0; // timer per accensione a tempo boolean obstacle = 0; // memorizza se è stato avvicinato un ostacolo unsigned long max_distance = 30; // distanza massima a cui può avvicinarsi il robot in cm //***** variabili usate dai motori #define PIN_PWMA 5 // Arduino pin D3 PWM motore A #define PIN_DIRA 2 // Arduino pin D2 comando direzione motore A #define PIN_PWMB 9 // Arduino pin D3 PWM motore A #define PIN_DIRB 8 // Arduino pin D2 comando direzione motore A int velo_A = 0; // imposta la velocità del motore A int velo_B = 0; // imposta la velocità del motore B int dir_A = LOW; // imposta la direzione del motore A

Transcript of Robot slalom ostacoli

Page 1: Robot slalom ostacoli

//***** ROBOT slalom ostacoli *****//***** librerie da includere #include <IRremote.h> // carica libreria IR - Ken Shirriff http://arcfn.com//***** variabili usate per lo slalom// velocità valide con batterie ben cariche #define GIRA_SX 190 // velocità di sterzata #define GIRA_DX 185 // velocità di sterzata #define DRITTO_A 230 // velocità motore sinistro dritto #define DRITTO_B 240 // velocità motore destro dritto byte t0, t1, t_tot; // memorizza i tempi di percorrenza byte fase = 0; // memorizza in che fase si trova il robot//***** variabili usate dal ricevitore IR ***** int RECV_PIN = 10; // Arduino pin D10 collegato a IR OUT IRrecv irrecv(RECV_PIN); // imposta il pin di ricezione dei dati decode_results results; // unsigned long code; // memorizza il codice ricevuto // i codic sono del telecomando marca LG // ai codici ricevuti aggiungere 0x + codice #define STOP 0x00 // comando stop #define KEY_STOP 0xCA9582FA // codice del tasto STOP #define SLALOM 0x11 // comando calcola perimetro #define KEY_1 0xDB2CD85E // codice del tasto 1 #define CERCHIO_SX 0x22 // comando stampa risultato #define KEY_2 0x72C8CD04 // codice del tasto 2 #define CERCHIO_DX 0x33 // comando stampa risultato #define KEY_3 0xBFA12A36 // codice del tasto 3 //***** variabili usate dal sonar #define ECHO 12 // Arduino pin D12 - SRF04 pin 4 #define TRIG 11 // Arduino pin D11 - SRF04 pin 3 unsigned long echo_time = 0; // tempo di ritorno dell'echo sonar in us #define DISTANCE_CM echo_time / 48 // calcola la distanza in centimetri unsigned long go_sonar = 0; // timer per accensione a tempo boolean obstacle = 0; // memorizza se è stato avvicinato un ostacolo unsigned long max_distance = 30; // distanza massima a cui può avvicinarsi il robot in cm//***** variabili usate dai motori #define PIN_PWMA 5 // Arduino pin D3 PWM motore A #define PIN_DIRA 2 // Arduino pin D2 comando direzione motore A #define PIN_PWMB 9 // Arduino pin D3 PWM motore A #define PIN_DIRB 8 // Arduino pin D2 comando direzione motore A int velo_A = 0; // imposta la velocità del motore A int velo_B = 0; // imposta la velocità del motore B int dir_A = LOW; // imposta la direzione del motore A

Page 2: Robot slalom ostacoli

int dir_B = LOW; // imposta la direzione del motore B

void setup() { irrecv.enableIRIn(); // attiva il ricevitore pinMode(ECHO, INPUT); // Arduino attiva ingresso pinMode(TRIG, OUTPUT); // Arduino attiva uscita pinMode(13, OUTPUT); // Arduino attiva uscita pin D13 led L pinMode(PIN_PWMA, OUTPUT); // Arduino attiva uscita pinMode(PIN_PWMB, OUTPUT); // Arduino attiva uscita pinMode(PIN_DIRA, OUTPUT); // Arduino attiva uscita pinMode(PIN_DIRB, OUTPUT); // Arduino attiva uscita Serial.begin(9600); // attiva la comunicazione seriale-USB }void loop() {//***** ricevi segnali dal telecomando ***** if (irrecv.decode(&results)) { // è stato ricevuto un comando digitalWrite(13, HIGH); // accende il led L di Arduino if(results.value == KEY_1) code = SLALOM; // se premuto 1 calcola il perimetro if(results.value == KEY_2) code = CERCHIO_SX; // se premuto 2 gira a sinistra if(results.value == KEY_3) code = CERCHIO_DX; // se premuto 3 gira a destra if(results.value == KEY_STOP) code = STOP; // se premuto tasto STOP Serial.println(results.value, HEX); // stampa il codice ricevuto irrecv.resume(); // riceve il prossimo codice delay(5); // aspetta 5ms digitalWrite(13, LOW); // spegne il led L di Arduino } //***** misura la distanza con il sonar ***** delay(1); // conta fino ad 1ms if(go_sonar++ >= 250) { // attiva il sonar ogni 250 cicli del loop digitalWrite(TRIG, HIGH); // attiva l'emissione sonar delayMicroseconds(10); // aspetta 10 us digitalWrite(TRIG, LOW); // termina l'emissione sonar echo_time = pulseIn(ECHO, HIGH); // misura la durata dell'echo if(DISTANCE_CM <= max_distance) { obstacle = 1; // memorizza l'incontro di un ostacolo digitalWrite(13, HIGH); // accendi il led L } else { digitalWrite(13, LOW); // spegni il led L } if(code == SLALOM){

Page 3: Robot slalom ostacoli

t_tot++; // incrementa il tempo di percorrenza // il robot viaggia dritto e attende il primo ostacolo if(fase == 0) { Serial.print("fase "); Serial.println(fase); // dir_A = LOW motore_A avanti // dir_B = HIGH motore_B avanti velo_A = DRITTO_A; velo_B = DRITTO_B; dir_A = LOW; dir_B = HIGH; if(obstacle) { fase = 1; // passa alla fase 1 } }

// ruota a sinistra finchè non vede più l'ostacolo if(fase == 1) { Serial.print("fase "); Serial.println(fase); velo_A = DRITTO_A; velo_B = DRITTO_B; dir_A = HIGH; dir_B = HIGH; if(obstacle) { obstacle = 0; // azzera il flag ostacolo } else{ t0 = t_tot; // prendi nota del tempo t0 fase = 2; // passa alla fase 2 } } // gira destra finchè vede il secondo ostacolo if(fase == 2) { Serial.print("fase "); Serial.println(fase); velo_A = DRITTO_A; velo_B = GIRA_DX; dir_A = LOW; dir_B = HIGH; if(obstacle) { fase = 3; // passa alla fase 3 } } // ruota a destra finchè non vede più l'ostacolo if(fase == 3) { Serial.print("fase "); Serial.println(fase); velo_A = DRITTO_A; velo_B = DRITTO_B; dir_A = LOW; dir_B = LOW; if(obstacle) { obstacle = 0; } else{ t1 = t_tot; // prendi nota del tempo t0 fase = 4; // passa alla fase 2 } } // gira sinistra finchè non finisce il tempo totale if(fase == 4) { Serial.print("fase "); Serial.println(fase); velo_A = GIRA_SX; velo_B = DRITTO_B; dir_A = LOW; dir_B = HIGH;

Page 4: Robot slalom ostacoli

if(t_tot-t0 >= 2*(t1-t0)) { // prosegui finchè non è finito il tempo fase = 0; // riparte da zero t_tot = 0; code = STOP; // ferma il robot } } } if(code == CERCHIO_SX) { // il robot gira in cerchio velo_A = GIRA_SX; velo_B = DRITTO_B; dir_A = LOW; dir_B = HIGH; } if(code == CERCHIO_DX) { // il robot gira in cerchio velo_A = DRITTO_A; velo_B = GIRA_DX; dir_A = LOW; dir_B = HIGH; } go_sonar = 0; // azzera il timer }//***** comanda i motori ***** // regola la velocità dei motori // dir_A = LOW motore_A avanti // dir_B = HIGH motore_B avanti if(code == STOP) { velo_A = 0; velo_B = 0; dir_A = LOW; dir_B = LOW; } // ferma il robot analogWrite(PIN_PWMA, velo_A); // imposta velocità motore A analogWrite(PIN_PWMB, velo_B); // imposta velocità motore B digitalWrite(PIN_DIRA, dir_A); // imposta la direzione del motore A digitalWrite(PIN_DIRB, dir_B); // imposta la direzione del motore B }