/*------------------------------------------------------------------------------

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 //nome motore sinistro (cingoli)

#define MD OUT_B //nome motore destro (cingoli)
#define MANO OUT_C //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 STRETTA 15 //forza della stretta della mano espressa in %

#define VEL_R_MANO 20 //velocità con cui rilascia la vittima espressa in %
#define VEL_MANO 100 //velocità della mano del robot espressa in %
#define VEL 60 //velocità robot rettilineo espressa in %
#define VEL_STERZO 80 //velocità robot sterzo espressa in %
#define VEL_RETRO 50 //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 //distanza visiva del sensore ultrasuoni

#define NERO 48 //intensità del colore nero visto dai sensori light
#define BIANCO 60 //intensità del colore del pavimento visto dai sensori
light
#define ARGENTO 70 //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 TONO 800 //frequenza del segnale acustico di segnalazione

#define DISTANZA 10 //distanza dei valori dal bordo dello schermo
#define DISTANZA_S 5 //distanza delle scritte dal bordo dello schermo
#define PARETE 5 //numero cicli esplorazione

// ***************************** INDICAZIONI **********************************

//motore sinistra = uscita A

//motore destra = uscita B
//motore mano = uscita C

//Fwd indietro
//Rev avanti

//sensore luce sinistra = ingresso 1

//sensore luce destra = ingresso 2
//sensore luce colore = ingresso 3
//sensore 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 colore; //contiene i valori di ingresso di SC

int ultras; //contiene i valori di ingresso di SU
int ls; //ontiene i valori di ingresso di SLS
int ld; //ontiene i valori di ingresso di SLD

int z_rossa; //conserva la posizione del robot

int casa; //indica se il robot arriva nella zona di evacuazione
int vittima; //indica se l'omino è preso
int fine; //indica se l'omino è stato salvato

int ost; //attiva task evita ostacolo

int j; //contatore per ostacolo (misura la lunghezza)
int parete; //controlla i task per l'evitamento di ostacoli nella zona rossa
int parete_b; //controlla i task per l'evitamento di ostacoli nella zona rossa

int muro; //controlla se nella zona rossa gira verso destra o verso
sinistra
int num_parete; //controlla quando il robot deve cambiare direzione
nell'esplorazione
int i; //contatore linea argentata

// ************************* SOTTOPROGRAMMI INIZIALI **************************

//setta i sensori del robot

sub settaggio(){
SetSensorLight(SLS); //settaggio del sensore di luce sinistro (SLS)
SetSensorLight(SLD); //settaggio del sensore di luce destro (SLD)
SetSensorLight(SC); //settaggio del sensore luce colore (SC)
SetSensorLowspeed(SU); //settaggio del sensore 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; //resetto la variabile (non attivo)

casa=0; //resetto la variabile (non attivo)
vittima=0; //resetto la variabile (non attivo)
parete=0; //resetto la variabile (non attivo)
parete_b=0; //resetto la variabile (non attivo)

muro=1; //la prima curva nella zona rossa a destra (attivo)

num_parete=0; //resetto la variabile ancora nessuna parete vista (non

attivo)
i=0; //resetto la variabile ancora nessuna linea argentata (non
attivo)
ost=0; //resetto la variabile ancora nessuno ostacolo da evitare
(non attivo)
j=0; //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 robot e fa una pausa (ferma cingoli)
sub fermo(){
Off (MS); //spegni motore sinistro
Off (MD); //spegni motore destro
Wait(PAUSA); //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 > dritto > destra (evita ostacoli)

sub G(){
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
dritto(); //vado dritto
Wait(1200); //durata movimento
fermo_b(); //mi fermo
RotateMotor(MS, VEL_STERZO, -ANGOLO_B); //ruoto il motore sinistro in avanti
RotateMotor(MD, VEL_STERZO, ANGOLO); //ruoto il motore destro indietro
fermo_b(); //mi fermo
}

//esegue i movimenti dritto > sinistra (evita ostacoli)

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

//esegue i movimenti dritto > sinistra (evita ostacoli)

sub W(){
dritto(); //vado dritto
Wait(1400); //durata movimento
fermo_b(); //mi fermo
RotateMotor(MS, VEL_STERZO, -ANGOLO_B); //ruoto il motore destro in avanti
RotateMotor(MD, VEL_STERZO, ANGOLO); //ruoto il motore sinistro indietro
fermo_b(); //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; //incremento j
Y(); //destra(-90-45)
dritto(2350) sinistra(90+45)gay
}
RotateMotor(MS, VEL_STERZO, 200); //ruoto il
motore sinistro indietro (>45°)
RotateMotor(MD, VEL_STERZO, -200); //ruoto il
motore destro in avanti (>45°)
fermo_b(); //mi fermo
dritto(); //vado fritto
Wait(1500); //durata
movimento
W(); //dritto
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); //ruoto il
motore sinistro indietro (135°)
fermo_b(); //mi fermo
dritto(); //vado dritto
Wait(PAUSA_C); //durata
movimento
fermo_b(); //mi fermo
RotateMotor(MS, VEL_STERZO, -ANGOLO); //ruoto il
motore sinistro in avanti
RotateMotor(MD, VEL_STERZO, ANGOLO); //ruoto il
motore destro indietro
fermo_b(); //mi fermo
B(); //dritto
sinistra
}
else{ //altrimenti la destra è libera
M(); //dritto sinistra(90+45)
while(SensorUS(SU)<=40){ //finchè non ho superato
l'ostacolo
j=j+1; //incremento j
V(); //destra(-90-45) dritto(2350)
sinistra(90+45)
}
RotateMotor(MD, VEL_STERZO, 200); //ruoto il
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
 RotateMotor(MS, VEL_STERZO, ANGOLO); //ruoto il
motore sinistro indietro
fermo_b(); //mi fermo
A(); //dritto
destra(90)
}
j=0; //resetto la variabile
ost=0; //resetto la variabile (disattivo evita ostacolo)
comand_linea=1; //setto la variabile (attivo segui linea)
comand_vista=1; //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); //aspetto che il
motore giri
RotateMotor(MS, VEL_STERZO, -ANGOLO); //ruoto il motore
sinistro in avanti
muro=0; //resetto la
variabile (alla prossima parete giro a sinistra)
}
else{ //devo girare a sinistra
Wait(PAUSA_B); //durata
retromarcia (aspetto l'altro motore)
RotateMotor(MS, VEL_STERZO, ANGOLO); //ruoto il motore
sinistro indietro
Wait(PAUSA_C); //aspetto che il
motore giri
RotateMotor(MS, VEL_STERZO, -400); //ruoto il motore
sinistro in avanti
Wait(PAUSA_C); //aspetto che il
motore giri
RotateMotor(MS, VEL_STERZO, ANGOLO); //ruoto il motore
sinistro indietro
muro=1; //setto la
variabile (alla prossima parete gira a destra)
}

}
}
}

//*************************************
//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(); //mi avvicino
Wait(PAUSA_B); //durata movimento
Off(MANO); //rilascio la vittima (spengo motore MANO)
OnRev(MANO, VEL_R_MANO); //apro la mano
Wait(PAUSA_C); //pasua, durata movimento
retromarcia(); //faccio retromarcia
Wait(PAUSA_C); //durata retromarcia
 fine=1; //setto la variabile (compare la scritta "SALVATO")
fermo(); //mi fermo
retromarcia(); //faccio retromarcia
Wait(PAUSA_C); //durata retromarcia
fermo(); //mi fermo (spengo i motori cingoli)
Off(MANO); //spengo il motore della mano
suono(); //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(); //vai leggermente avanti

Wait(300); //durata movimento

destra(); //gira a destra

Wait(1450); //durata movimento

}
else{ //altrimenti
OnFwd(MS,
VEL_RETRO); //muovi indietro il motore sinistro
} */
OnFwd(MS,
VEL_RETRO); //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(); //prendila e sollevala

OnFwd(MANO, STRETTA); //tieni la vittima stretta

vittima=1; //setto la variabile

comand_esplora=1; //ricomincio ad esplorare

}
else{ //altrimenti è un
ostacolo

num_parete=num_parete+1; //incremento la variabile

if(num_parete==PARETE){ //se ho finito di esplorare in verticale

parete_b=1; //cambio direzione

Wait(2300); //aspetto che il robot si giri

parete_b=0; //il robot smette di girare (disattivo task)

Wait(PAUSA_C); //pausa

comand_esplora=1; //setta la variabile (avvio task)

num_parete=-2; //riparte il conteggio il secondo lato è più lungo

muro=0; //resetto la variabile

 }
else{
//altrimenti continuo

parete=1; //setto la variabile (attivo task)

Wait(6300); //aspetto che il robot si giri

parete=0; //resetto la variabile (disattivo task)

Wait(PAUSA_C); //pausa

comand_esplora=1; //setta la variabile (attivo task)

}
}
}
else{ //se ho la vittima
dritto(); //mi
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(); //deposito la vittima

}
else{ //altrimenti è una
parete

parete_b=1; //cambio direzione

Wait(2600); //aspetto che il robot si giri

parete_b=0; //il robot smette di girare (disattivo task)

Wait(PAUSA_C); //pausa

comand_esplora=1; //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
}
}
//task che da in uscita sul display informazioni sulla posizione
task controllo(){
for(;;){ //ciclo infinito
TextOut(DISTANZA_S,LCD_LINE1,"....8080BT...."); //dai sul display una
scritta di segnalazione sulla 1 linea
TextOut(DISTANZA_S,LCD_LINE2,"POSIZIONE: "); //dai sul display una
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 display una
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 display una
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 display una
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 display una
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 display una
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 display una
scritta di segnalazione sulla 7 linea
Wait(PAUSA_C); //aspetta
}
ClearScreen(); //pulisci schermo
}
}

// ********************************* 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; //avvia task
start s_sinistra; //avvia task
start s_destra; //avvia task
start bussola; //avvia task
start vista; //avvia task
start variabili; //avvia task
start esplorazione; //avvia task
start paretea; //avvia task
start pareteb; //avvia task
start paretec; //avvia task
start pareted; //avvia task
start ostacolo; //avvia task
}

//task principale
task main(){
suono(); //emette un suono di
avvertimento
TextOut(DISTANZA_S,LCD_LINE1,"....START...."); //dai sul display una scritta
di segnalazione
settaggio(); //sottoprogramma che attiva i
sensori
reset_var(); //sottoprogramma che resetta le
variabili
Wait(PAUSA_B); //fai una pausa di durata
PAUSA_B
ClearScreen(); //pulisci schermo display
avvio(); //avvia task
}

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