Siren / Mbed 2 deprecated STEVEcomplete_SIRENA9-09

thread

Dependencies:   HCSR04 mbed-rtos mbed

Fork of MotorShield_futura by 3FEN

main.cpp@3:2816fec3acee, 2018-05-03 (annotated)

Committer:
Wonderjack996
Date:
Thu May 03 14:52:49 2018 +0000
Revision:
3:2816fec3acee
Parent:
2:7df0fc7d8d3c 
thread

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