3FEN / Mbed 2 deprecated MotorShield_futura

programma commando motori

Dependencies:   mbed HCSR04 mbed-rtos

main.cpp@3:5b91f83642bf, 2017-05-30 (annotated)

Committer:
Wonderjack996
Date:
Tue May 30 07:30:45 2017 +0000
Revision:
3:5b91f83642bf
Parent:
2:7df0fc7d8d3c 
programma steve fine giornata 29-05

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wonderjack996 2:7df0fc7d8d3c 1 /*---LIBRERIE---*/
Wonderjack996 2:7df0fc7d8d3c 2 #include "mbed.h"
Wonderjack996 2:7df0fc7d8d3c 3 #include "rtos.h"
Wonderjack996 2:7df0fc7d8d3c 4 #include "hcsr04.h"
Wonderjack996 2:7df0fc7d8d3c 5 /*--------------*/
Wonderjack996 1:009a67d6875f 6
Wonderjack996 2:7df0fc7d8d3c 7 /*---COSTANTI---*/
Wonderjack996 2:7df0fc7d8d3c 8 #define MAX 999999 //valore intero massimo utilizzabile
Wonderjack996 0:637e2ec8d164 9
Wonderjack996 2:7df0fc7d8d3c 10 #define CTRL_SIRENA D11 //D11 - pin aggancio sensore sonoro
Wonderjack996 2:7df0fc7d8d3c 11
Wonderjack996 3:5b91f83642bf 12 #define DISTANCE_CTRL 15 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
Wonderjack996 2:7df0fc7d8d3c 13 per capire quando la macchina si deve fermare*/
Wonderjack996 0:637e2ec8d164 14
Wonderjack996 3:5b91f83642bf 15 #define NUMBER_CRC 9 //numero utilizzato per controlli nell avanzamento automatico (vedi nella funzione che lo utilizza per spiegazione approfondita)
Wonderjack996 3:5b91f83642bf 16
Wonderjack996 3:5b91f83642bf 17
Wonderjack996 3:5b91f83642bf 18 #define LED_AUTOMATIC PB_2 //attacco led controolo modalita automatica
Wonderjack996 3:5b91f83642bf 19
Wonderjack996 2:7df0fc7d8d3c 20 /*---PIN BLUETHOOT---*/
Wonderjack996 2:7df0fc7d8d3c 21 /*I pin utilizzati per TX e RX sono pin morfo, a livello hardware collegare il pin TX dell HC05 al PA_12 e quello RX a PA_11 ( a livello harware vanno invertiti )*/
Wonderjack996 2:7df0fc7d8d3c 22 #define TX PA_11
Wonderjack996 2:7df0fc7d8d3c 23 #define RX PA_12
Wonderjack996 2:7df0fc7d8d3c 24 /*-------------------*/
Wonderjack996 2:7df0fc7d8d3c 25
Wonderjack996 2:7df0fc7d8d3c 26 /*---PIN CONTROLLO MOTORI---*/
Wonderjack996 2:7df0fc7d8d3c 27 #define PWMB PC_7 //D9
Wonderjack996 2:7df0fc7d8d3c 28 #define DIRB D8 //D8
Wonderjack996 2:7df0fc7d8d3c 29
Wonderjack996 2:7df0fc7d8d3c 30 #define PWMA PB_4 //D5
Wonderjack996 2:7df0fc7d8d3c 31 #define DIRA D4 //D4
Wonderjack996 2:7df0fc7d8d3c 32 /*--------------------------*/
Wonderjack996 2:7df0fc7d8d3c 33
Wonderjack996 2:7df0fc7d8d3c 34 /*---PIN ULTRASUONI---*/
Wonderjack996 2:7df0fc7d8d3c 35 #define AVANTI_TRIGGER PA_5 //D13
Wonderjack996 2:7df0fc7d8d3c 36 #define AVANTI_ECHO PA_6 //D12
Wonderjack996 2:7df0fc7d8d3c 37 #define SINISTRA_TRIGGER PA_8 //D7
Wonderjack996 2:7df0fc7d8d3c 38 #define SINISTRA_ECHO PB_10 //D6
Wonderjack996 2:7df0fc7d8d3c 39 #define DESTRA_TRIGGER PA_10 //D2
Wonderjack996 2:7df0fc7d8d3c 40 #define DESTRA_ECHO PB_3 //D3
Wonderjack996 2:7df0fc7d8d3c 41 /*--------------------*/
Wonderjack996 2:7df0fc7d8d3c 42 /*--------------*/
Wonderjack996 1:009a67d6875f 43
Wonderjack996 2:7df0fc7d8d3c 44 /*---VARIABILI---*/
Wonderjack996 2:7df0fc7d8d3c 45 /*---CONTROLLO MOTORI---*/
Wonderjack996 2:7df0fc7d8d3c 46 PwmOut motorBpower(PWMB); //oggetti controllo motori
Wonderjack996 2:7df0fc7d8d3c 47 DigitalOut motorBdirection(DIRB);
Wonderjack996 2:7df0fc7d8d3c 48
Wonderjack996 2:7df0fc7d8d3c 49 PwmOut motorApower(PWMA);
Wonderjack996 2:7df0fc7d8d3c 50 DigitalOut motorAdirection(DIRA);
Wonderjack996 2:7df0fc7d8d3c 51 /*---------------------*/
Wonderjack996 2:7df0fc7d8d3c 52
Wonderjack996 2:7df0fc7d8d3c 53 /*---CONTROLLO ULTRASUONI---*/
Wonderjack996 2:7df0fc7d8d3c 54 HCSR04 ultrasuoniAvanti(AVANTI_TRIGGER, AVANTI_ECHO); //istanzio oggetti per utilizzo ultrasuoni
Wonderjack996 2:7df0fc7d8d3c 55 HCSR04 ultrasuoniSinistra(SINISTRA_TRIGGER, SINISTRA_ECHO);
Wonderjack996 2:7df0fc7d8d3c 56 HCSR04 ultrasuoniDestra(DESTRA_TRIGGER, DESTRA_ECHO);
Wonderjack996 2:7df0fc7d8d3c 57 /*--------------------------*/
Wonderjack996 2:7df0fc7d8d3c 58
Wonderjack996 3:5b91f83642bf 59 DigitalOut ledAutomatic(LED_AUTOMATIC); //pin in rifermineto al led controllo mod automatica
Wonderjack996 3:5b91f83642bf 60
Wonderjack996 2:7df0fc7d8d3c 61 Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale
Wonderjack996 2:7df0fc7d8d3c 62
Wonderjack996 2:7df0fc7d8d3c 63 Serial device(TX, RX); //oggetto per gestione seriale bluethoot
Wonderjack996 2:7df0fc7d8d3c 64
Wonderjack996 2:7df0fc7d8d3c 65 bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena
Wonderjack996 2:7df0fc7d8d3c 66
Wonderjack996 2:7df0fc7d8d3c 67 bool flagAutomatic; //utilizzo per sapere se utilizzare modalita automatica o meno
Wonderjack996 3:5b91f83642bf 68
Wonderjack996 3:5b91f83642bf 69 bool flagStopAvanti;
Wonderjack996 2:7df0fc7d8d3c 70 /*----------------*/
Wonderjack996 1:009a67d6875f 71
Wonderjack996 2:7df0fc7d8d3c 72 /*---PROTOTIPI---*/
Wonderjack996 2:7df0fc7d8d3c 73 void avanti();
Wonderjack996 2:7df0fc7d8d3c 74 void indietro();
Wonderjack996 2:7df0fc7d8d3c 75 void destra();
Wonderjack996 2:7df0fc7d8d3c 76 void sinistra();
Wonderjack996 2:7df0fc7d8d3c 77 void fermo();
Wonderjack996 2:7df0fc7d8d3c 78 bool checkFermo();
Wonderjack996 2:7df0fc7d8d3c 79 void automaticProgram();
Wonderjack996 2:7df0fc7d8d3c 80 void check_sirena_thread(void const *args);
Wonderjack996 3:5b91f83642bf 81 void check_stop_thread(void const *args);
Wonderjack996 3:5b91f83642bf 82 void check_stop_avanti(void const *args);
Wonderjack996 2:7df0fc7d8d3c 83 /*---------------*/
Wonderjack996 0:637e2ec8d164 84
Wonderjack996 0:637e2ec8d164 85 int main() {
Wonderjack996 0:637e2ec8d164 86
Wonderjack996 1:009a67d6875f 87 unsigned char rx;
Wonderjack996 3:5b91f83642bf 88 int distanceTemp;
Wonderjack996 1:009a67d6875f 89
Wonderjack996 2:7df0fc7d8d3c 90 motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
Wonderjack996 2:7df0fc7d8d3c 91 motorApower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
Wonderjack996 0:637e2ec8d164 92
Wonderjack996 3:5b91f83642bf 93 Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
Wonderjack996 3:5b91f83642bf 94 Thread ctrlStopThread(check_stop_thread); // start Thread controllo fine processo automatico
Wonderjack996 3:5b91f83642bf 95 Thread ctrlStopAvanti(check_stop_avanti); // start Thread controllo fine processo avanzamento
Wonderjack996 3:5b91f83642bf 96
Wonderjack996 1:009a67d6875f 97 while(true){
Wonderjack996 1:009a67d6875f 98 if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
Wonderjack996 1:009a67d6875f 99 rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
Wonderjack996 2:7df0fc7d8d3c 100 //pc.printf("recived: %c \r\n",rx); //stampa a video
Wonderjack996 1:009a67d6875f 101 switch(rx){ //in base alla lettera che riceve sa cosa deve fare
Wonderjack996 1:009a67d6875f 102 case 'a':
Wonderjack996 3:5b91f83642bf 103 flagStopAvanti = true; /*il thread check_stop_avanti viene avviato appena tale flag diventa true*/
Wonderjack996 3:5b91f83642bf 104 /*a causa di problemi riscontrati con l'utilizzo dell'ultrasuono si e' deciso di ripere l'operazione di acquisizione
Wonderjack996 3:5b91f83642bf 105 dati dall'ultrasuono per piu' volte (tante quante NUMBER_CRC) in modo da essere sicuri del valore ottenuto*/
Wonderjack996 3:5b91f83642bf 106 for(int j = 0; j < NUMBER_CRC && flagStopAvanti; j++){
Wonderjack996 3:5b91f83642bf 107 distanceTemp = MAX;
Wonderjack996 3:5b91f83642bf 108 for( int i = 0; i < 100 && ( distanceTemp < 2 || distanceTemp > 400 && flagStopAvanti ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
Wonderjack996 3:5b91f83642bf 109 ultrasuoniAvanti.start();
Wonderjack996 3:5b91f83642bf 110 distanceTemp = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 3:5b91f83642bf 111 //wait_ms(10);
Wonderjack996 3:5b91f83642bf 112 }
Wonderjack996 3:5b91f83642bf 113 while( distanceTemp > 5 && flagStopAvanti ){
Wonderjack996 3:5b91f83642bf 114 avanti();
Wonderjack996 3:5b91f83642bf 115 ultrasuoniAvanti.start();
Wonderjack996 3:5b91f83642bf 116 distanceTemp = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 3:5b91f83642bf 117 }
Wonderjack996 3:5b91f83642bf 118 fermo();
Wonderjack996 3:5b91f83642bf 119 }
Wonderjack996 1:009a67d6875f 120 break;
Wonderjack996 1:009a67d6875f 121 case 'b':
Wonderjack996 1:009a67d6875f 122 indietro();
Wonderjack996 1:009a67d6875f 123 break;
Wonderjack996 1:009a67d6875f 124 case 'c':
Wonderjack996 1:009a67d6875f 125 destra();
Wonderjack996 1:009a67d6875f 126 break;
Wonderjack996 1:009a67d6875f 127 case 'd':
Wonderjack996 1:009a67d6875f 128 sinistra();
Wonderjack996 1:009a67d6875f 129 break;
Wonderjack996 1:009a67d6875f 130 case 'e':
Wonderjack996 1:009a67d6875f 131 fermo();
Wonderjack996 1:009a67d6875f 132 break;
Wonderjack996 2:7df0fc7d8d3c 133 case 'f':
Wonderjack996 2:7df0fc7d8d3c 134 flagAutomatic = true;
Wonderjack996 3:5b91f83642bf 135 ledAutomatic = 1; //accensione led mod automatica
Wonderjack996 2:7df0fc7d8d3c 136 automaticProgram(); //funzione pilota automatico
Wonderjack996 3:5b91f83642bf 137 ledAutomatic = 0; //spegnimento led
Wonderjack996 2:7df0fc7d8d3c 138 break;
Wonderjack996 1:009a67d6875f 139 }
Wonderjack996 1:009a67d6875f 140 }
Wonderjack996 1:009a67d6875f 141 }
Wonderjack996 0:637e2ec8d164 142
Wonderjack996 0:637e2ec8d164 143 }
Wonderjack996 0:637e2ec8d164 144
Wonderjack996 2:7df0fc7d8d3c 145 void automaticProgram()
Wonderjack996 2:7df0fc7d8d3c 146 {
Wonderjack996 2:7df0fc7d8d3c 147 int avantiDis, distance; //la variabile direction contiene 0 se il robot deve andare a destra, 1 se deve andare a sinistra
Wonderjack996 2:7df0fc7d8d3c 148
Wonderjack996 2:7df0fc7d8d3c 149 int distanceDx, distanceSx;
Wonderjack996 2:7df0fc7d8d3c 150
Wonderjack996 2:7df0fc7d8d3c 151 fermo();
Wonderjack996 2:7df0fc7d8d3c 152
Wonderjack996 2:7df0fc7d8d3c 153 while(flagAutomatic){ //flag modificabile dal thread che aspetta il segnale di stop
Wonderjack996 2:7df0fc7d8d3c 154 while(flagSirena && flagAutomatic){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza
Wonderjack996 2:7df0fc7d8d3c 155 ultrasuoniAvanti.start();
Wonderjack996 2:7df0fc7d8d3c 156 avantiDis = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 157 //pc.printf("avanti: %d \r\n",avantiDis);
Wonderjack996 2:7df0fc7d8d3c 158 while( avantiDis >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere
Wonderjack996 2:7df0fc7d8d3c 159 avanti();
Wonderjack996 2:7df0fc7d8d3c 160 ultrasuoniAvanti.start();
Wonderjack996 2:7df0fc7d8d3c 161 avantiDis = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 162 //pc.printf("avanti: %d \r\n",avantiDis);
Wonderjack996 2:7df0fc7d8d3c 163 wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 164 }
Wonderjack996 2:7df0fc7d8d3c 165
Wonderjack996 2:7df0fc7d8d3c 166 fermo();
Wonderjack996 2:7df0fc7d8d3c 167
Wonderjack996 2:7df0fc7d8d3c 168 if( checkFermo() && flagSirena && flagAutomatic ){
Wonderjack996 2:7df0fc7d8d3c 169 /*funzione utilizzata per leggere un valore attendibile ( nel range dell ultrasuono ). se dopo 100 tentativi
Wonderjack996 2:7df0fc7d8d3c 170 il valore non e' ancora attendibile allora esco dal controllo in modo da non rimanere bloccato*/
Wonderjack996 3:5b91f83642bf 171 distanceDx = MAX;
Wonderjack996 3:5b91f83642bf 172 distanceSx = MAX;
Wonderjack996 2:7df0fc7d8d3c 173 for( int i = 0; i < 100 && ( distanceDx < 2 || distanceDx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
Wonderjack996 2:7df0fc7d8d3c 174 ultrasuoniDestra.start();
Wonderjack996 2:7df0fc7d8d3c 175 distanceDx = ultrasuoniDestra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 176 //wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 177 }
Wonderjack996 2:7df0fc7d8d3c 178
Wonderjack996 2:7df0fc7d8d3c 179 for( int i = 0; i < 100 && ( distanceSx < 2 || distanceSx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
Wonderjack996 2:7df0fc7d8d3c 180 ultrasuoniSinistra.start();
Wonderjack996 2:7df0fc7d8d3c 181 distanceSx = ultrasuoniSinistra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 182 //wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 183 }
Wonderjack996 2:7df0fc7d8d3c 184
Wonderjack996 2:7df0fc7d8d3c 185 /*controllo se il valori valori di uscita dalle iterazioni sono attendibili ( nel range )
Wonderjack996 2:7df0fc7d8d3c 186 se non soddisfano i criteri allora li setto al valore MAX in modo che i seguenti controlli non li prendano
Wonderjack996 2:7df0fc7d8d3c 187 in considerazione*/
Wonderjack996 2:7df0fc7d8d3c 188 if( distanceSx < 2 || distanceSx > 400 )
Wonderjack996 2:7df0fc7d8d3c 189 distanceSx = MAX;
Wonderjack996 2:7df0fc7d8d3c 190
Wonderjack996 2:7df0fc7d8d3c 191 if( distanceDx < 2 || distanceDx > 400 )
Wonderjack996 2:7df0fc7d8d3c 192 distanceDx = MAX;
Wonderjack996 2:7df0fc7d8d3c 193
Wonderjack996 3:5b91f83642bf 194 //pc.printf("distanceSx: %d distanceDx: %d\r\n",distanceSx, distanceDx);
Wonderjack996 2:7df0fc7d8d3c 195
Wonderjack996 2:7df0fc7d8d3c 196 //in base ai valori letti capisco da che lato e' piu vicino la superficie piana e in base a cio sono da che parte girare
Wonderjack996 2:7df0fc7d8d3c 197 if( distanceSx > distanceDx ){
Wonderjack996 2:7df0fc7d8d3c 198 sinistra();
Wonderjack996 2:7df0fc7d8d3c 199 wait_ms(1000);
Wonderjack996 2:7df0fc7d8d3c 200
Wonderjack996 2:7df0fc7d8d3c 201 ultrasuoniDestra.start();
Wonderjack996 2:7df0fc7d8d3c 202 distance = ultrasuoniDestra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 203 //pc.printf("sinistra: %d \r\n",sinistraDis);
Wonderjack996 2:7df0fc7d8d3c 204 while( distance >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana
Wonderjack996 2:7df0fc7d8d3c 205 ultrasuoniDestra.start();
Wonderjack996 2:7df0fc7d8d3c 206 distance = ultrasuoniDestra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 207 //pc.printf("distance: %d \r\n",distance);
Wonderjack996 2:7df0fc7d8d3c 208 }
Wonderjack996 2:7df0fc7d8d3c 209 }
Wonderjack996 2:7df0fc7d8d3c 210 else{
Wonderjack996 2:7df0fc7d8d3c 211 destra();
Wonderjack996 2:7df0fc7d8d3c 212 wait_ms(1000);
Wonderjack996 2:7df0fc7d8d3c 213
Wonderjack996 2:7df0fc7d8d3c 214 ultrasuoniSinistra.start();
Wonderjack996 2:7df0fc7d8d3c 215 distance = ultrasuoniSinistra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 216 //pc.printf("sinistra: %d \r\n",sinistraDis);
Wonderjack996 2:7df0fc7d8d3c 217 while( distance >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana
Wonderjack996 2:7df0fc7d8d3c 218 ultrasuoniSinistra.start();
Wonderjack996 2:7df0fc7d8d3c 219 distance = ultrasuoniSinistra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 220 //pc.printf("distance: %d \r\n",distance);
Wonderjack996 2:7df0fc7d8d3c 221 }
Wonderjack996 2:7df0fc7d8d3c 222 }
Wonderjack996 2:7df0fc7d8d3c 223
Wonderjack996 2:7df0fc7d8d3c 224 wait_ms(200);
Wonderjack996 2:7df0fc7d8d3c 225 fermo();
Wonderjack996 2:7df0fc7d8d3c 226 }
Wonderjack996 2:7df0fc7d8d3c 227 }
Wonderjack996 2:7df0fc7d8d3c 228 }
Wonderjack996 2:7df0fc7d8d3c 229 }
Wonderjack996 2:7df0fc7d8d3c 230
Wonderjack996 3:5b91f83642bf 231 void check_stop_avanti(void const *args)
Wonderjack996 3:5b91f83642bf 232 {
Wonderjack996 3:5b91f83642bf 233 /*questo thread ha il compito di attendere che l utente richieda lo stop del mezzo, quando cio avviene il thread setta il flag a
Wonderjack996 3:5b91f83642bf 234 false e interrompe sia se stesso che la funzione avanti*/
Wonderjack996 3:5b91f83642bf 235 unsigned char rx;
Wonderjack996 3:5b91f83642bf 236 while(true)
Wonderjack996 3:5b91f83642bf 237 while( flagStopAvanti )
Wonderjack996 3:5b91f83642bf 238 if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
Wonderjack996 3:5b91f83642bf 239 rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
Wonderjack996 3:5b91f83642bf 240 if( rx == 'e' )
Wonderjack996 3:5b91f83642bf 241 flagStopAvanti = false;
Wonderjack996 3:5b91f83642bf 242 }
Wonderjack996 3:5b91f83642bf 243 }
Wonderjack996 3:5b91f83642bf 244
Wonderjack996 2:7df0fc7d8d3c 245 void check_stop_thread(void const *args)
Wonderjack996 2:7df0fc7d8d3c 246 {
Wonderjack996 2:7df0fc7d8d3c 247 /*thread che controlla se l'utente via dispositivi ha chiesto lo stop del proamma automatico*/
Wonderjack996 2:7df0fc7d8d3c 248 unsigned char rx;
Wonderjack996 3:5b91f83642bf 249 while(true)
Wonderjack996 3:5b91f83642bf 250 while( flagAutomatic )
Wonderjack996 3:5b91f83642bf 251 if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
Wonderjack996 3:5b91f83642bf 252 rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
Wonderjack996 3:5b91f83642bf 253 if( rx == 'g' )
Wonderjack996 3:5b91f83642bf 254 flagAutomatic = false;
Wonderjack996 3:5b91f83642bf 255 }
Wonderjack996 2:7df0fc7d8d3c 256 }
Wonderjack996 2:7df0fc7d8d3c 257
Wonderjack996 2:7df0fc7d8d3c 258 void check_sirena_thread(void const *args)
Wonderjack996 2:7df0fc7d8d3c 259 {
Wonderjack996 2:7df0fc7d8d3c 260 /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento
Wonderjack996 2:7df0fc7d8d3c 261 con un flag al main che si adatta alla situazione*/
Wonderjack996 2:7df0fc7d8d3c 262 DigitalIn in(CTRL_SIRENA);
Wonderjack996 2:7df0fc7d8d3c 263 while(true){
Wonderjack996 2:7df0fc7d8d3c 264 int val = in.read();
Wonderjack996 2:7df0fc7d8d3c 265 if( val == 1 )
Wonderjack996 2:7df0fc7d8d3c 266 flagSirena = false;
Wonderjack996 2:7df0fc7d8d3c 267 else
Wonderjack996 2:7df0fc7d8d3c 268 flagSirena = true;
Wonderjack996 2:7df0fc7d8d3c 269 }
Wonderjack996 2:7df0fc7d8d3c 270 }
Wonderjack996 2:7df0fc7d8d3c 271
Wonderjack996 2:7df0fc7d8d3c 272 bool checkFermo()
Wonderjack996 2:7df0fc7d8d3c 273 {
Wonderjack996 2:7df0fc7d8d3c 274 /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili
Wonderjack996 2:7df0fc7d8d3c 275 La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/
Wonderjack996 2:7df0fc7d8d3c 276 bool ret = true;
Wonderjack996 2:7df0fc7d8d3c 277 int avantiDis, ctrl = 50;
Wonderjack996 2:7df0fc7d8d3c 278 for(int i = 0; i < ctrl; i++){
Wonderjack996 2:7df0fc7d8d3c 279 ultrasuoniAvanti.start();
Wonderjack996 2:7df0fc7d8d3c 280 avantiDis = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 281 if( avantiDis >= DISTANCE_CTRL ){
Wonderjack996 2:7df0fc7d8d3c 282 ret = false;
Wonderjack996 2:7df0fc7d8d3c 283 break;
Wonderjack996 2:7df0fc7d8d3c 284 }
Wonderjack996 2:7df0fc7d8d3c 285 wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 286 }
Wonderjack996 2:7df0fc7d8d3c 287 return ret;
Wonderjack996 2:7df0fc7d8d3c 288 }
Wonderjack996 2:7df0fc7d8d3c 289
Wonderjack996 0:637e2ec8d164 290 void avanti()
Wonderjack996 0:637e2ec8d164 291 {
Wonderjack996 0:637e2ec8d164 292 motorBdirection = 1;
Wonderjack996 0:637e2ec8d164 293 motorAdirection = 0;
Wonderjack996 0:637e2ec8d164 294 motorBpower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 295 motorApower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 296 }
Wonderjack996 0:637e2ec8d164 297
Wonderjack996 0:637e2ec8d164 298 void indietro()
Wonderjack996 0:637e2ec8d164 299 {
Wonderjack996 0:637e2ec8d164 300 motorBdirection = 0;
Wonderjack996 0:637e2ec8d164 301 motorAdirection = 1;
Wonderjack996 0:637e2ec8d164 302 motorBpower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 303 motorApower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 304 }
Wonderjack996 0:637e2ec8d164 305
Wonderjack996 0:637e2ec8d164 306 void destra()
Wonderjack996 0:637e2ec8d164 307 {
Wonderjack996 0:637e2ec8d164 308 motorBdirection = 0;
Wonderjack996 0:637e2ec8d164 309 motorAdirection = 0;
Wonderjack996 0:637e2ec8d164 310 motorBpower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 311 motorApower.pulsewidth(0.007); // 70%
Wonderjack996 0:637e2ec8d164 312 }
Wonderjack996 0:637e2ec8d164 313
Wonderjack996 0:637e2ec8d164 314 void sinistra()
Wonderjack996 0:637e2ec8d164 315 {
Wonderjack996 0:637e2ec8d164 316 motorBdirection = 1;
Wonderjack996 0:637e2ec8d164 317 motorAdirection = 1;
Wonderjack996 0:637e2ec8d164 318 motorBpower.pulsewidth(0.007); // 100%
Wonderjack996 0:637e2ec8d164 319 motorApower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 320 }
Wonderjack996 0:637e2ec8d164 321
Wonderjack996 0:637e2ec8d164 322 void fermo()
Wonderjack996 0:637e2ec8d164 323 {
Wonderjack996 0:637e2ec8d164 324 motorBpower.pulsewidth(0); // fermo
Wonderjack996 0:637e2ec8d164 325 motorApower.pulsewidth(0); // fermo
Wonderjack996 0:637e2ec8d164 326 }