/*-----------------------------------------------------------------------------squadra: 8080BT creatore: Ferrari Marco - Verona Diego anno: 2010 progetto: robotcup rescue scopo: realizzare un robot

in grado di muoversi tra macerie e localizzare il ferito e portarlo al sicuro note: ------------------------------------------------------------------------------*/ //libreria #include "NXCDefs.h" // **************************** VALORI DEFINE ********************************* #define MS OUT_A #define MD OUT_B #define MANO OUT_C //nome motore sinistro (cingoli) //nome motore destro (cingoli) //nome motore mano

#define SLS IN_1 //nome sensore luce sinistra #define SLD IN_2 //nome sensore luce destra #define SC IN_3 //nome sensore colore #define SU IN_4 //nome sensore ultrasuoni //***************************************************** #define #define #define #define #define #define STRETTA 15 VEL_R_MANO 20 VEL_MANO 100 VEL 60 VEL_STERZO 80 VEL_RETRO 50 //forza della stretta della mano espressa in % //velocità con cui rilascia la vittima espressa in % //velocità della mano del robot espressa in % //velocità robot rettilineo espressa in % //velocità robot sterzo espressa in % //velocità robot retromarcia espressa in %

#define ANGOLO 500 //angolo di rotazione dei motori (90°) #define ANGOLO_B 800 //angolo di rotazione dei motori (135°) //***************************************************************** #define PAUSA 2500 //pausa espressa in ms #define PAUSA_C 1000 //pausa espressa in ms #define PAUSA_B 500 //pausa espressa in ms //********************************************************** #define DISTANZA_V 9 #define NERO 48 #define BIANCO 60 light #define ARGENTO 70 //distanza visiva del sensore ultrasuoni //intensità del colore nero visto dai sensori light //intensità del colore del pavimento visto dai sensori //intensità del colore argento visto dai sensori light

#define SC_NERO 30 //intensità del nero visto dal sensore SC #define SC_ARGENTO 55 //intensità dell'argento visto dal sensore SC //************************************************************************ #define #define #define #define TONO 800 DISTANZA 10 DISTANZA_S 5 PARETE 5 //frequenza del segnale acustico di segnalazione //distanza dei valori dal bordo dello schermo //distanza delle scritte dal bordo dello schermo //numero cicli esplorazione

// ***************************** INDICAZIONI ********************************** //motore sinistra = uscita A //motore destra = uscita B //motore mano = uscita C //Fwd indietro //Rev avanti //sensore //sensore //sensore //sensore luce sinistra = ingresso 1 luce destra = ingresso 2 luce colore = ingresso 3 ultrasuoni = ingresso 4

// ******************************* VARIABILI ********************************** //dichiarazione variabili int comand_linea; //determina l'attivazione del task linea int comand_vista; //determina l'attivazione del task vista int comand_esplora; //determina l'attivazione del task esplorazione int int int int int int int int int int int int colore; ultras; ls; ld; z_rossa; casa; vittima; fine; ost; j; parete; parete_b; //contiene i valori di ingresso di SC //contiene i valori di ingresso di SU //ontiene i valori di ingresso di SLS //ontiene i valori di ingresso di SLD //conserva la posizione del robot //indica se il robot arriva nella zona di evacuazione //indica se l'omino è preso //indica se l'omino è stato salvato //attiva task evita ostacolo //contatore per ostacolo (misura la lunghezza) //controlla i task per l'evitamento di ostacoli nella zona rossa //controlla i task per l'evitamento di ostacoli nella zona rossa //controlla se nella zona rossa gira verso destra o verso //controlla quando il robot deve cambiare direzione //contatore linea argentata

int muro; sinistra int num_parete; nell'esplorazione int i;

// ************************* SOTTOPROGRAMMI INIZIALI ************************** //setta i sensori del robot sub settaggio(){ SetSensorLight(SLS); SetSensorLight(SLD); SetSensorLight(SC); SetSensorLowspeed(SU); }

//settaggio //settaggio //settaggio //settaggio

del del del del

sensore sensore sensore sensore

di luce sinistro (SLS) di luce destro (SLD) luce colore (SC) ultrasuoni (SU)

//resetta e setta le variabili sub reset_var(){ comand_linea=1; //setto la variabile (attivo) comand_vista=1; //setto la variabile (attivo) comand_esplora=1; //setto la variabile (attivo) z_rossa=0; casa=0; vittima=0; parete=0; parete_b=0; muro=1; num_parete=0; attivo) i=0; attivo) ost=0; (non attivo) j=0; } //resetto //resetto //resetto //resetto //resetto la la la la la variabile variabile variabile variabile variabile (non (non (non (non (non attivo) attivo) attivo) attivo) attivo)

//la prima curva nella zona rossa a destra (attivo) //resetto la variabile ancora nessuna parete vista (non //resetto la variabile ancora nessuna linea argentata (non //resetto la variabile ancora nessuno ostacolo da evitare //resetto la variabile (non attivo)

//emette un suono di avviso sub suono(){ PlayTone(TONO, PAUSA_B); //emette un suono di avvertimento che dura PAUSA_B } // **************************** MOVIMENTI ROBOT *******************************

//ferma i motori del sub fermo(){ Off (MS); Off (MD); Wait(PAUSA); }

robot e fa una pausa (ferma cingoli) //spegni motore sinistro //spegni motore destro //fai una pausa di durata PAUSA

//ferma i motori del robot e fa una pausa (ferma cingoli) sub fermo_b(){ Off (MS); //spegni motore sinistro Off (MD); //spegni motore destro Wait(PAUSA_B); //fai una pausa di durata PAUSA } //movimento rettilineo in avanti del robot sub dritto(){ OnRev (MS,VEL); //il motore sinistro ruota in avanti OnRev (MD,VEL); //il motore destro ruota in avanti } //movimento rettilineo indietro del robot sub retromarcia(){ OnFwd (MS,VEL_RETRO); //il motore sinistro ruota indietro OnFwd (MD,VEL_RETRO); //il motore destro ruota indietro } //movimento a destra del robot sub destra(){ OnRev (MS,70); //il motore sinistro ruota in avanti OnFwd (MD,70); //il motore destro ruota indietro } //movimento a sinistra del robot sub sinistra(){ OnFwd (MS,70); //il motore sinistro ruota indietro OnRev (MD,70); //il motore destro ruota in avanti } //*************************************************************** //esegue i movimenti dritto > destra (evita ostacoli) sub A(){ do{ j=j-1; //decremento j dritto(); //vado dritto Wait(3550); //durata movimento }while(j==0) //ripeto finche j è uguale a zero destra(); //giro a destra Wait(1200); //durata movimento } //esegue i movimenti dritto > destra (evita ostacoli) sub B(){ do{ //eseguo quello che segue j=j-1; //decremento j dritto(); //vado dritto Wait(3550); //durata movimento }while(j==0) //ripeto finche j è uguale a zero sinistra(); //giro a destra Wait(1500); //durata movimento } //esegue i movimenti sinistra > sub G(){ RotateMotor(MS, VEL_STERZO, RotateMotor(MD, VEL_STERZO, fermo_b(); dritto(); Wait(1200); fermo_b(); RotateMotor(MS, VEL_STERZO, RotateMotor(MD, VEL_STERZO, fermo_b(); dritto > destra (evita ostacoli) ANGOLO); -ANGOLO_B); //ruoto il motore sinistro indietro //ruoto il motore destro in avanti //mi fermo //vado dritto //durata movimento //mi fermo //ruoto il motore sinistro in avanti //ruoto il motore destro indietro //mi fermo

-ANGOLO_B); ANGOLO);

} //esegue i movimenti dritto > sinistra (evita sub M(){ dritto(); Wait(1400); fermo_b(); RotateMotor(MD, VEL_STERZO, -ANGOLO_B); RotateMotor(MS, VEL_STERZO, ANGOLO); fermo_b(); } //esegue i movimenti dritto > sinistra (evita sub W(){ dritto(); Wait(1400); fermo_b(); RotateMotor(MS, VEL_STERZO, -ANGOLO_B); RotateMotor(MD, VEL_STERZO, ANGOLO); fermo_b(); } ostacoli) //vado dritto //durata movimento //mi fermo //ruoto il motore destro in avanti //ruoto il motore sinistro indietro //mi fermo ostacoli) //vado dritto //durata movimento //mi fermo //ruoto il motore destro in avanti //ruoto il motore sinistro indietro //mi fermo

//esegue i movimenti destra > dritto > sinistra (evita ostacoli) sub V(){ RotateMotor(MD, VEL_STERZO, ANGOLO_B); //ruoto il motore destro indietro RotateMotor(MS, VEL_STERZO, -ANGOLO); //ruoto il motore sinistro in avanti fermo_b(); //mi fermo dritto(); //vado dritto Wait(800); //durata movimento fermo_b(); //mi fermo RotateMotor(MS, VEL_STERZO, ANGOLO); //ruoto il motore sinistro indietro RotateMotor(MD, VEL_STERZO, -ANGOLO_B); //ruoto il motore destro in avanti fermo_b(); //mi fermo } //esegue i movimenti destra > dritto > sinistra (evita ostacoli) sub Y(){ RotateMotor(MD, VEL_STERZO, -350); //ruoto il motore destro in avanti RotateMotor(MS, VEL_STERZO, ANGOLO_B); //ruoto il motore sinistro indietro fermo_b(); //mi fermo dritto(); //vado dritto Wait(1200); //durata movimento fermo_b(); //mi fermo RotateMotor(MS, VEL_STERZO, -ANGOLO_B); //ruoto il motore sinistra in avanti RotateMotor(MD, VEL_STERZO, ANGOLO); //ruoto il motore destro indietro fermo_b(); //mi fermo } // task che controlla i movimenti per superare un ostacolo task ostacolo(){ for(;;){ //cilclo infinito if(ost==1){ //se devo evitare un ostacolo comand_vista=0; //resetto la variabile (disattivo vista) retromarcia(); //faccio retromarcia Wait(800); //durata retromarcia fermo_b(); //mi fermo RotateMotor(MD, VEL_STERZO, ANGOLO); //ruoto il motore destro indietro (90°) RotateMotor(MS, VEL_STERZO, -ANGOLO); //ruoto il motore sinistro in avanti (90°) fermo_b(); //aspetto, mi fermo if(SensorUS(SU)<=40){ //se a destra non ho spazio sufficiente RotateMotor(MS, VEL_STERZO, 350); //ruoto il motore sinistro indietro RotateMotor(MD, VEL_STERZO, -350); //ruoto il motore destro in avanti fermo_b(); //mi fermo RotateMotor(MS, VEL_STERZO, ANGOLO); //ruoto il motore sinistro indietro RotateMotor(MD, VEL_STERZO, -ANGOLO_B); //ruoto il motore destro in avanti

fermo_b(); //mi fermo W(); //dritto destra while(SensorUS(SU)<=40){ //finchè non ho superato l'ostacolo j=j+1; Y(); dritto(2350) sinistra(90+45)gay } RotateMotor(MS, VEL_STERZO, 200); motore sinistro indietro (>45°) RotateMotor(MD, VEL_STERZO, -200); motore destro in avanti (>45°) fermo_b(); dritto(); Wait(1500); movimento W(); destra(115°) while(SensorUS(SU)<=40) { //finchè Y(); //sinistra dritto destra } RotateMotor(MD, VEL_STERZO, -600); //ruoto il motore destro in avanti (135°) RotateMotor(MS, VEL_STERZO, 600); motore sinistro indietro (135°) fermo_b(); dritto(); Wait(PAUSA_C); movimento fermo_b(); RotateMotor(MS, VEL_STERZO, -ANGOLO); motore sinistro in avanti RotateMotor(MD, VEL_STERZO, ANGOLO); motore destro indietro fermo_b(); B(); sinistra } else{ //altrimenti la destra è libera M(); //dritto sinistra(90+45) while(SensorUS(SU)<=40){ //finchè non ho superato l'ostacolo j=j+1; V(); sinistra(90+45) } RotateMotor(MD, VEL_STERZO, 200); motore destro indietro (>45°) RotateMotor(MS, VEL_STERZO, -200); //ruoto il motore sinistro in avanti (>45°) fermo_b(); //mi fermo dritto(); //vado fritto Wait(PAUSA_C); //durata movimento M(); //dritto sinistra(115°) fermo_b(); //mi fermo while(SensorUS(SU)<=40) { //finchè V(); //destra(-135°) dritto(2350) sinistra(135°) } RotateMotor(MD, VEL_STERZO, 700); //ruoto il motore destro indietro (135°) RotateMotor(MS, VEL_STERZO, -700); //ruoto il motore sinistro in avanti (135°) fermo_b(); //mi fermo dritto(); //vado dritto Wait(PAUSA_B); //durata movimento fermo_b(); //mi fermo RotateMotor(MD, VEL_STERZO, -ANGOLO); //ruoto il motore destro in avanti //ruoto il //incremento j //destra(-90-45) dritto(2350) //mi fermo //dritto //ruoto il //mi fermo //ruoto il //mi fermo //vado dritto //durata //ruoto il //dritto //mi fermo //vado fritto //durata //ruoto il //ruoto il //incremento j //destra(-90-45)

RotateMotor(MS, VEL_STERZO, ANGOLO); motore sinistro indietro fermo_b(); A(); destra(90) } j=0; ost=0; comand_linea=1; comand_vista=1; } } }

//ruoto il //mi fermo //dritto

//resetto la variabile //resetto la variabile (disattivo evita ostacolo) //setto la variabile (attivo segui linea) //setto la variabile (attivo vista)

//*********************************************************** //movimento del robot per evitare le pareti (zona rossa) (motore destro) task paretea(){ for(;;){ //ciclo infinito if(parete==1){ //se parete è uguale a uno (attivo il task) if(muro==1){ //deve girare a destra retromarcia(); //faccio retromarcia Wait(PAUSA_B); //durata retromarcia RotateMotor(MD, VEL_STERZO, ANGOLO); //ruoto il motore destro indietro Wait(PAUSA_C); //aspetto che il motore giri RotateMotor(MD, VEL_STERZO, -400); //ruoto il motore destro in avanti Wait(PAUSA_C); //aspetto che il motore giri RotateMotor(MD, VEL_STERZO, ANGOLO); //ruoto il motore destro indietro muro=0; //resetto la variabile (alla prossima parete giro a sinistra) } else{ //deve girare a sinistra retromarcia(); //faccio retromarcia Wait(PAUSA_B); //durata retromarcia RotateMotor(MD, VEL_STERZO, -ANGOLO); //ruoto il motore destro in avanti Wait(PAUSA_C); //aspetto che il motore giri RotateMotor(MD, VEL_STERZO, -400); //ruoto il motore destro in avanti Wait(PAUSA_C); //aspetto che il motore giri RotateMotor(MD, VEL_STERZO, -ANGOLO); //ruoto il motore destro in avanti muro=1; //setto la variabile (alla prossima parete gira a destra) } } } } //movimento del robot per evitare le pareti (zona rossa) (motore sinistro) task pareteb(){ for(;;){ //ciclo infinito if(parete==1){ //se parete è uguale a uno (attivo il task) if(muro==1){ //devo girare a destra Wait(PAUSA_B); //durata retromarcia RotateMotor(MS, VEL_STERZO, -ANGOLO); //ruoto il motore sinistro in avanti Wait(PAUSA_C); //aspetto che il motore giri RotateMotor(MS, VEL_STERZO, -400); //ruoto il motore sinistro in avanti

Wait(PAUSA_C); motore giri RotateMotor(MS, VEL_STERZO, -ANGOLO); sinistro in avanti muro=0; variabile (alla prossima parete giro a sinistra) } else{ //devo girare a sinistra Wait(PAUSA_B); retromarcia (aspetto l'altro motore) RotateMotor(MS, VEL_STERZO, ANGOLO); sinistro indietro Wait(PAUSA_C); motore giri RotateMotor(MS, VEL_STERZO, -400); sinistro in avanti Wait(PAUSA_C); motore giri RotateMotor(MS, VEL_STERZO, ANGOLO); sinistro indietro muro=1; variabile (alla prossima parete gira a destra) } } } }

//aspetto che il //ruoto il motore //resetto la

//durata //ruoto il motore //aspetto che il //ruoto il motore //aspetto che il //ruoto il motore //setto la

//************************************* //movimento del robot per cambiare movimento di esplorazione (zona rossa) (motore destro) task paretec(){ for(;;){ //ciclo infinito if(parete_b==1){ //se parete_b è uguale a uno attivo il task Wait(PAUSA_B); //pausa RotateMotor(MD, VEL_STERZO, -600); //ruoto il motore destro in avanti Wait(PAUSA_C); //aspetto che il motore giri } } } //movimento del robot per cambiare movimento di esplorazione (zona rossa) (motore sinistro) task pareted(){ for(;;){ //ciclo infinito if(parete_b==1){ //se parete_b è uguale a uno attivo il task Wait(PAUSA_B); //pausa RotateMotor(MS, VEL_STERZO, 600); //ruoto il motore sinistro indietro Wait(PAUSA_C); //aspetto che il motore giri } } } //***************************************************************** //afferra vittima e la solleva sub mano(){ OnFwd(MANO, VEL_MANO); //chiudi la mano e sollevala Wait(PAUSA); //pasua, durata } //rilascia la vittima sub rilascio(){ dritto(); Wait(PAUSA_B); Off(MANO); OnRev(MANO, VEL_R_MANO); Wait(PAUSA_C); retromarcia(); Wait(PAUSA_C);

//mi avvicino //durata movimento //rilascio la vittima (spengo motore MANO) //apro la mano //pasua, durata movimento //faccio retromarcia //durata retromarcia

fine=1; fermo(); retromarcia(); Wait(PAUSA_C); fermo(); Off(MANO); suono(); }

//setto la variabile (compare la scritta "SALVATO") //mi fermo //faccio retromarcia //durata retromarcia //mi fermo (spengo i motori cingoli) //spengo il motore della mano //avviso

// **************************** SEGUI LINEA *********************************** //task che controlla il sensore luce di sinistra (segui linea) task s_sinistra(){ for(;;){ //ciclo infinito Wait(10); //fai una piccola pausa if(z_rossa==0){ //ripeti finchè è uguale 0 Wait(10); //fai una piccola pausa if(comand_linea==1){ //ripeti finchè è uguale 0 if((Sensor(SLS)<=NERO)){ //se il sensore di sinistra rileva la linea /*if((Sensor(SLD)<= NERO)){ //se il sensore di destra rileva la linea dritto(); Wait(300); destra(); Wait(1450); //vai leggermente avanti //durata movimento //gira a destra //durata movimento } else{ //altrimenti OnFwd(MS, VEL_RETRO); VEL_RETRO); //muovi indietro il motore sinistro } */ OnFwd(MS, //muovi indietro il motore sinistro } else{ //altrimenti OnRev(MS, VEL); //muovi avanti il motore sinistro } } } } } //task che controlla il sensore luce di destra (segui linea) task s_destra(){ for(;;){ //ciclo infinito Wait(10); //fai una piccola pausa if(z_rossa==0){ //ripeti finchè è uguale 0 Wait(10); //fai una piccola pausa if(comand_linea==1){ //ripeti finchè è uguale 0 if((Sensor(SLD)<= NERO)){ //se il sensore di destra rileva la linea OnFwd(MD, VEL_RETRO); //muovi indietro il motore destro } else{ //altrimenti OnRev(MD, VEL); //muovi avanti il motore destro } } } } } // **************************** BUSSOLA ***************************************

//task che controlla la posizione del robot nel percorso task bussola(){ for(;;){ //ciclo continuo if(ls>=ARGENTO){ //se la condizione è vera sono nella zona rossa z_rossa=1; //setto la variabile per indicare sopra comand_esplora=1; //setto la variabile per attivare l'esplorazione della stanza i=i+1; //incremento la variabile Wait(PAUSA); //aspetto per non vedere la stessa linea } } } // ********************************* VISTA ************************************ //task che controlla il sensore di ultrasuoni task vista(){ for(;;){ //ciclo infinito Wait(10); //fai una piccola pausa if(comand_vista==1){ //ripeti finchè è uguale 1 (attivo il task) if(SensorUS(SU)<=DISTANZA_V){ //se vedo un ostacolo comand_linea=0; //resetta la variabile (disattivo task) comand_esplora=0; //resetta la variabile (disattivo task) fermo(); //fermati if(z_rossa==0){ //se non sono nella zona rossa è un ostacolo ost=1; //attivo task evita ostacolo } if(z_rossa==1){ //se sono nella zona rossa if(vittima==0){ //se non ho la vittima suono(); //avviso dritto(); //mi avvicino Wait(PAUSA_C); //pausa durata fermo(); //fermati if(colore>=SC_ARGENTO){ //se è la vittima mano(); OnFwd(MANO, STRETTA); vittima=1; comand_esplora=1; ostacolo num_parete=num_parete+1; //incremento la variabile if(num_parete==PARETE){ //se ho finito di esplorare in verticale parete_b=1; Wait(2300); parete_b=0; Wait(PAUSA_C); comand_esplora=1; num_parete=-2; muro=0; //cambio direzione //aspetto che il robot si giri //il robot smette di girare (disattivo task) //pausa //setta la variabile (avvio task) //riparte il conteggio il secondo lato è più lungo //resetto la variabile //prendila e sollevala //tieni la vittima stretta //setto la variabile //ricomincio ad esplorare } else{ //altrimenti è un

} else{ //altrimenti continuo parete=1; Wait(6300); parete=0; Wait(PAUSA_C); comand_esplora=1; //setto la variabile (attivo task) //aspetto che il robot si giri //resetto la variabile (disattivo task) //pausa //setta la variabile (attivo task) } } } else{ //se ho la vittima dritto(); avvicino Wait(PAUSA_B); //pausa durata fermo(); //fermati if(Sensor(SLS)<=NERO){ //se il sensore di destra rileva nero rilascio(); //deposito la vittima } else{ //altrimenti controlla il sensore di colore (luce) frontale if(colore<=SC_NERO){ //se rileva nero (pedana di salvataggio) rilascio(); parete parete_b=1; Wait(2600); parete_b=0; Wait(PAUSA_C); comand_esplora=1; //cambio direzione //aspetto che il robot si giri //il robot smette di girare (disattivo task) //pausa //setta la variabile (avvio task) } } } } } } } } // ********************************* VARI ************************************* //sposta i valori di ingresso dei sensori in variabili task variabili(){ for(;;){ //ciclo infinito colore=Sensor(SC); //la variabile colore aquisisce i valori di ingresso di SC ultras=SensorUS(SU); //la variabile ultras aquisisce i valori di ingresso di SU ls=Sensor(SLS); //la variabile ls aquisisce i valori di ingresso di SLS ld=Sensor(SLD); //la variabile ld aquisisce i valori di ingresso di SLD } } //deposito la vittima } else{ //altrimenti è una //mi

//task che da in uscita sul display informazioni sulla posizione task controllo(){ for(;;){ //ciclo infinito TextOut(DISTANZA_S,LCD_LINE1,"....8080BT...."); //dai sul scritta di segnalazione sulla 1 linea TextOut(DISTANZA_S,LCD_LINE2,"POSIZIONE: "); //dai sul scritta di segnalazione sulla 2 linea Wait(200); //aspetta if(z_rossa==1){ //se sono nella zona rossa TextOut(DISTANZA_S,LCD_LINE3,"zona rossa"); //dai sul scritta di segnalazione sulla 3 linea Wait(PAUSA_C); //aspetta } if(z_rossa==0){ //se non sono nella zona rossa TextOut(DISTANZA_S,LCD_LINE3,"piano terra"); //dai sul scritta di segnalazione sulla 4 line Wait(PAUSA_C); //aspetta } if(casa==1){ //se ho trovato la zona di salvataggio TextOut(DISTANZA_S,LCD_LINE4,"zona salvataggio"); //dai sul scritta di segnalazione sulla 4 linea Wait(PAUSA_C); //aspetta } if(vittima==1){ //se ho la vittima TextOut(DISTANZA_S,LCD_LINE5,"vittima trovata"); //dai sul scritta di segnalazione sulla 5 line Wait(PAUSA_C); //aspetta } if(SensorUS(SU)<=DISTANZA_V){ //se vedo un ostacolo TextOut(DISTANZA_S,LCD_LINE6,"ostacolo"); //dai sul scritta di segnalazione sulla 6 line Wait(PAUSA_C); //aspetta } if(fine==1){ //se ho salvato l'omino TextOut(DISTANZA_S,LCD_LINE7,"!!! SALVATO !!!"); //dai sul scritta di segnalazione sulla 7 linea Wait(PAUSA_C); //aspetta } ClearScreen(); //pulisci schermo } }

display una display una

display una

display una

display una

display una

display una

display una

// ********************************* ESPLORAZIONE ***************************** //esplora la zona rossa in cerca della vittima task esplorazione(){ for(;;){ //ciclo infinito Wait(10); //fai una piccola pausa if(z_rossa==1){ //se sono nella zona rossa (attivo task) Wait(10); //fai una piccola pausa if(comand_esplora==1){ //se uguale a uno attivati OnRev(MS, VEL); //muovi avanti il motore sinistro OnRev(MD, VEL); //muovi avanti il motore destro if(i>1){ //se non è la prima volta che vedo la linea retromarcia(); Wait(PAUSA_C); parete=1; //cambio direzione Wait(6500); //aspetto che il robot si giri parete=0; //il robot smette di girare (disattivo task) Wait(PAUSA_C); //pausa comand_esplora=1; //setta la variabile (avvio task) i=1; //resetto la variabile come fosse dentro la zona rossa } }

} } } // **************************** TASK PRINCIPALE ******************************* //avvia i task sub avvio(){ start controllo; start s_sinistra; start s_destra; start bussola; start vista; start variabili; start esplorazione; start paretea; start pareteb; start paretec; start pareted; start ostacolo; }

//avvia //avvia //avvia //avvia //avvia //avvia //avvia //avvia //avvia //avvia //avvia //avvia

task task task task task task task task task task task task

//task principale task main(){ suono(); avvertimento TextOut(DISTANZA_S,LCD_LINE1,"....START...."); di segnalazione settaggio(); sensori reset_var(); variabili Wait(PAUSA_B); PAUSA_B ClearScreen(); avvio(); }

//emette un suono di //dai sul display una scritta //sottoprogramma che attiva i //sottoprogramma che resetta le //fai una pausa di durata //pulisci schermo display //avvia task

// **************************** FINE PROGRAMMA ********************************

Sign up to vote on this title
UsefulNot useful