3FEN / Mbed 2 deprecated MotorShield_futura

programma commando motori

Dependencies:   mbed HCSR04 mbed-rtos

main.cpp@4:088a0fad279a, 2017-06-09 (annotated)

Committer:
Wonderjack996
Date:
Fri Jun 09 10:57:05 2017 +0000
Revision:
4:088a0fad279a
Parent:
2:7df0fc7d8d3c
Child:
5:f4923705f199
programma STEVE completo del 8-06

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 4:088a0fad279a 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 4:088a0fad279a 15 #define NUMBER_CRC 9 //numero utilizzato per controlli nell avanzamento automatico (vedi nella funzione che lo utilizza per spiegazione approfondita)
Wonderjack996 4:088a0fad279a 16
Wonderjack996 4:088a0fad279a 17
Wonderjack996 4:088a0fad279a 18 #define LED_AUTOMATIC PB_2 //attacco led controllo modalita automatica
Wonderjack996 4:088a0fad279a 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 4:088a0fad279a 59 DigitalOut ledAutomatic(LED_AUTOMATIC); //pin in rifermineto al led controllo mod automatica
Wonderjack996 4:088a0fad279a 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 4:088a0fad279a 68
Wonderjack996 4:088a0fad279a 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 4:088a0fad279a 80 void setup();
Wonderjack996 2:7df0fc7d8d3c 81 void check_sirena_thread(void const *args);
Wonderjack996 4:088a0fad279a 82 void check_stop_thread(void const *args);
Wonderjack996 4:088a0fad279a 83 void check_stop_avanti(void const *args);
Wonderjack996 2:7df0fc7d8d3c 84 /*---------------*/
Wonderjack996 0:637e2ec8d164 85
Wonderjack996 0:637e2ec8d164 86 int main() {
Wonderjack996 0:637e2ec8d164 87
Wonderjack996 1:009a67d6875f 88 unsigned char rx;
Wonderjack996 4:088a0fad279a 89 int distanceTemp;
Wonderjack996 1:009a67d6875f 90
Wonderjack996 4:088a0fad279a 91 setup(); //settaggio iniziale del robot
Wonderjack996 4:088a0fad279a 92
Wonderjack996 4:088a0fad279a 93 Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
Wonderjack996 4:088a0fad279a 94 Thread ctrlStopThread(check_stop_thread); // start Thread controllo fine processo automatico
Wonderjack996 4:088a0fad279a 95 Thread ctrlStopAvanti(check_stop_avanti); // start Thread controllo fine processo avanzamentofunn
Wonderjack996 0:637e2ec8d164 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 4:088a0fad279a 103 flagStopAvanti = true; /*il thread check_stop_avanti viene avviato appena tale flag diventa true*/
Wonderjack996 4:088a0fad279a 104 /*a causa di problemi riscontrati con l'utilizzo dell'ultrasuono si e' deciso di ripere l'operazione di acquisizione
Wonderjack996 4:088a0fad279a 105 dati dall'ultrasuono per piu' volte (tante quante NUMBER_CRC) in modo da essere sicuri del valore ottenuto*/
Wonderjack996 4:088a0fad279a 106 for(int j = 0; j < NUMBER_CRC && flagStopAvanti; j++){
Wonderjack996 4:088a0fad279a 107 distanceTemp = MAX;
Wonderjack996 4:088a0fad279a 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 4:088a0fad279a 109 ultrasuoniAvanti.start();
Wonderjack996 4:088a0fad279a 110 distanceTemp = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 4:088a0fad279a 111 //wait_ms(10);
Wonderjack996 4:088a0fad279a 112 }
Wonderjack996 4:088a0fad279a 113 while( distanceTemp > 8 && flagStopAvanti ){
Wonderjack996 4:088a0fad279a 114 avanti();
Wonderjack996 4:088a0fad279a 115 ultrasuoniAvanti.start();
Wonderjack996 4:088a0fad279a 116 distanceTemp = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 4:088a0fad279a 117 }
Wonderjack996 4:088a0fad279a 118 fermo();
Wonderjack996 4:088a0fad279a 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 4:088a0fad279a 135 ledAutomatic = 1; //accensione led mod automatica
Wonderjack996 2:7df0fc7d8d3c 136 automaticProgram(); //funzione pilota automatico
Wonderjack996 4:088a0fad279a 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 4:088a0fad279a 145 void setup()
Wonderjack996 4:088a0fad279a 146 {
Wonderjack996 4:088a0fad279a 147 motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
Wonderjack996 4:088a0fad279a 148 motorApower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
Wonderjack996 4:088a0fad279a 149 fermo();
Wonderjack996 4:088a0fad279a 150 }
Wonderjack996 4:088a0fad279a 151
Wonderjack996 2:7df0fc7d8d3c 152 void automaticProgram()
Wonderjack996 2:7df0fc7d8d3c 153 {
Wonderjack996 2:7df0fc7d8d3c 154 int avantiDis, distance; //la variabile direction contiene 0 se il robot deve andare a destra, 1 se deve andare a sinistra
Wonderjack996 2:7df0fc7d8d3c 155
Wonderjack996 2:7df0fc7d8d3c 156 int distanceDx, distanceSx;
Wonderjack996 2:7df0fc7d8d3c 157
Wonderjack996 2:7df0fc7d8d3c 158 fermo();
Wonderjack996 2:7df0fc7d8d3c 159
Wonderjack996 2:7df0fc7d8d3c 160 while(flagAutomatic){ //flag modificabile dal thread che aspetta il segnale di stop
Wonderjack996 2:7df0fc7d8d3c 161 while(flagSirena && flagAutomatic){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza
Wonderjack996 2:7df0fc7d8d3c 162 ultrasuoniAvanti.start();
Wonderjack996 2:7df0fc7d8d3c 163 avantiDis = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 164 //pc.printf("avanti: %d \r\n",avantiDis);
Wonderjack996 2:7df0fc7d8d3c 165 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 166 avanti();
Wonderjack996 2:7df0fc7d8d3c 167 ultrasuoniAvanti.start();
Wonderjack996 2:7df0fc7d8d3c 168 avantiDis = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 169 //pc.printf("avanti: %d \r\n",avantiDis);
Wonderjack996 2:7df0fc7d8d3c 170 wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 171 }
Wonderjack996 2:7df0fc7d8d3c 172
Wonderjack996 2:7df0fc7d8d3c 173 fermo();
Wonderjack996 2:7df0fc7d8d3c 174
Wonderjack996 2:7df0fc7d8d3c 175 if( checkFermo() && flagSirena && flagAutomatic ){
Wonderjack996 2:7df0fc7d8d3c 176 /*funzione utilizzata per leggere un valore attendibile ( nel range dell ultrasuono ). se dopo 100 tentativi
Wonderjack996 2:7df0fc7d8d3c 177 il valore non e' ancora attendibile allora esco dal controllo in modo da non rimanere bloccato*/
Wonderjack996 4:088a0fad279a 178 distanceDx = MAX;
Wonderjack996 4:088a0fad279a 179 distanceSx = MAX;
Wonderjack996 2:7df0fc7d8d3c 180 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 181 ultrasuoniDestra.start();
Wonderjack996 2:7df0fc7d8d3c 182 distanceDx = ultrasuoniDestra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 183 //wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 184 }
Wonderjack996 2:7df0fc7d8d3c 185
Wonderjack996 2:7df0fc7d8d3c 186 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 187 ultrasuoniSinistra.start();
Wonderjack996 2:7df0fc7d8d3c 188 distanceSx = ultrasuoniSinistra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 189 //wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 190 }
Wonderjack996 2:7df0fc7d8d3c 191
Wonderjack996 2:7df0fc7d8d3c 192 /*controllo se il valori valori di uscita dalle iterazioni sono attendibili ( nel range )
Wonderjack996 2:7df0fc7d8d3c 193 se non soddisfano i criteri allora li setto al valore MAX in modo che i seguenti controlli non li prendano
Wonderjack996 2:7df0fc7d8d3c 194 in considerazione*/
Wonderjack996 2:7df0fc7d8d3c 195 if( distanceSx < 2 || distanceSx > 400 )
Wonderjack996 2:7df0fc7d8d3c 196 distanceSx = MAX;
Wonderjack996 2:7df0fc7d8d3c 197
Wonderjack996 2:7df0fc7d8d3c 198 if( distanceDx < 2 || distanceDx > 400 )
Wonderjack996 2:7df0fc7d8d3c 199 distanceDx = MAX;
Wonderjack996 2:7df0fc7d8d3c 200
Wonderjack996 4:088a0fad279a 201 //pc.printf("distanceSx: %d distanceDx: %d\r\n",distanceSx, distanceDx);
Wonderjack996 2:7df0fc7d8d3c 202
Wonderjack996 2:7df0fc7d8d3c 203 //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 204 if( distanceSx > distanceDx ){
Wonderjack996 2:7df0fc7d8d3c 205 sinistra();
Wonderjack996 2:7df0fc7d8d3c 206 wait_ms(1000);
Wonderjack996 2:7df0fc7d8d3c 207
Wonderjack996 2:7df0fc7d8d3c 208 ultrasuoniDestra.start();
Wonderjack996 2:7df0fc7d8d3c 209 distance = ultrasuoniDestra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 210 //pc.printf("sinistra: %d \r\n",sinistraDis);
Wonderjack996 2:7df0fc7d8d3c 211 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 212 ultrasuoniDestra.start();
Wonderjack996 2:7df0fc7d8d3c 213 distance = ultrasuoniDestra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 214 //pc.printf("distance: %d \r\n",distance);
Wonderjack996 2:7df0fc7d8d3c 215 }
Wonderjack996 2:7df0fc7d8d3c 216 }
Wonderjack996 2:7df0fc7d8d3c 217 else{
Wonderjack996 2:7df0fc7d8d3c 218 destra();
Wonderjack996 2:7df0fc7d8d3c 219 wait_ms(1000);
Wonderjack996 2:7df0fc7d8d3c 220
Wonderjack996 2:7df0fc7d8d3c 221 ultrasuoniSinistra.start();
Wonderjack996 2:7df0fc7d8d3c 222 distance = ultrasuoniSinistra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 223 //pc.printf("sinistra: %d \r\n",sinistraDis);
Wonderjack996 2:7df0fc7d8d3c 224 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 225 ultrasuoniSinistra.start();
Wonderjack996 2:7df0fc7d8d3c 226 distance = ultrasuoniSinistra.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 227 //pc.printf("distance: %d \r\n",distance);
Wonderjack996 2:7df0fc7d8d3c 228 }
Wonderjack996 2:7df0fc7d8d3c 229 }
Wonderjack996 2:7df0fc7d8d3c 230
Wonderjack996 4:088a0fad279a 231 wait_ms(150);
Wonderjack996 2:7df0fc7d8d3c 232 fermo();
Wonderjack996 2:7df0fc7d8d3c 233 }
Wonderjack996 2:7df0fc7d8d3c 234 }
Wonderjack996 2:7df0fc7d8d3c 235 }
Wonderjack996 2:7df0fc7d8d3c 236 }
Wonderjack996 2:7df0fc7d8d3c 237
Wonderjack996 4:088a0fad279a 238 void check_stop_avanti(void const *args)
Wonderjack996 4:088a0fad279a 239 {
Wonderjack996 4:088a0fad279a 240 /*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 4:088a0fad279a 241 false e interrompe sia se stesso che la funzione avanti*/
Wonderjack996 4:088a0fad279a 242 unsigned char rx;
Wonderjack996 4:088a0fad279a 243 while(true)
Wonderjack996 4:088a0fad279a 244 while( flagStopAvanti )
Wonderjack996 4:088a0fad279a 245 if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
Wonderjack996 4:088a0fad279a 246 rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
Wonderjack996 4:088a0fad279a 247 if( rx == 'e' )
Wonderjack996 4:088a0fad279a 248 flagStopAvanti = false;
Wonderjack996 4:088a0fad279a 249 }
Wonderjack996 4:088a0fad279a 250 }
Wonderjack996 4:088a0fad279a 251
Wonderjack996 2:7df0fc7d8d3c 252 void check_stop_thread(void const *args)
Wonderjack996 2:7df0fc7d8d3c 253 {
Wonderjack996 2:7df0fc7d8d3c 254 /*thread che controlla se l'utente via dispositivi ha chiesto lo stop del proamma automatico*/
Wonderjack996 2:7df0fc7d8d3c 255 unsigned char rx;
Wonderjack996 4:088a0fad279a 256 while(true)
Wonderjack996 4:088a0fad279a 257 while( flagAutomatic )
Wonderjack996 4:088a0fad279a 258 if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
Wonderjack996 4:088a0fad279a 259 rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
Wonderjack996 4:088a0fad279a 260 if( rx == 'g' )
Wonderjack996 4:088a0fad279a 261 flagAutomatic = false;
Wonderjack996 4:088a0fad279a 262 }
Wonderjack996 2:7df0fc7d8d3c 263 }
Wonderjack996 2:7df0fc7d8d3c 264
Wonderjack996 2:7df0fc7d8d3c 265 void check_sirena_thread(void const *args)
Wonderjack996 2:7df0fc7d8d3c 266 {
Wonderjack996 2:7df0fc7d8d3c 267 /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento
Wonderjack996 2:7df0fc7d8d3c 268 con un flag al main che si adatta alla situazione*/
Wonderjack996 2:7df0fc7d8d3c 269 DigitalIn in(CTRL_SIRENA);
Wonderjack996 2:7df0fc7d8d3c 270 while(true){
Wonderjack996 2:7df0fc7d8d3c 271 int val = in.read();
Wonderjack996 2:7df0fc7d8d3c 272 if( val == 1 )
Wonderjack996 2:7df0fc7d8d3c 273 flagSirena = false;
Wonderjack996 2:7df0fc7d8d3c 274 else
Wonderjack996 2:7df0fc7d8d3c 275 flagSirena = true;
Wonderjack996 2:7df0fc7d8d3c 276 }
Wonderjack996 2:7df0fc7d8d3c 277 }
Wonderjack996 2:7df0fc7d8d3c 278
Wonderjack996 2:7df0fc7d8d3c 279 bool checkFermo()
Wonderjack996 2:7df0fc7d8d3c 280 {
Wonderjack996 2:7df0fc7d8d3c 281 /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili
Wonderjack996 2:7df0fc7d8d3c 282 La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/
Wonderjack996 2:7df0fc7d8d3c 283 bool ret = true;
Wonderjack996 2:7df0fc7d8d3c 284 int avantiDis, ctrl = 50;
Wonderjack996 2:7df0fc7d8d3c 285 for(int i = 0; i < ctrl; i++){
Wonderjack996 2:7df0fc7d8d3c 286 ultrasuoniAvanti.start();
Wonderjack996 2:7df0fc7d8d3c 287 avantiDis = ultrasuoniAvanti.get_dist_cm();
Wonderjack996 2:7df0fc7d8d3c 288 if( avantiDis >= DISTANCE_CTRL ){
Wonderjack996 2:7df0fc7d8d3c 289 ret = false;
Wonderjack996 2:7df0fc7d8d3c 290 break;
Wonderjack996 2:7df0fc7d8d3c 291 }
Wonderjack996 2:7df0fc7d8d3c 292 wait_ms(10);
Wonderjack996 2:7df0fc7d8d3c 293 }
Wonderjack996 2:7df0fc7d8d3c 294 return ret;
Wonderjack996 2:7df0fc7d8d3c 295 }
Wonderjack996 2:7df0fc7d8d3c 296
Wonderjack996 0:637e2ec8d164 297 void avanti()
Wonderjack996 0:637e2ec8d164 298 {
Wonderjack996 0:637e2ec8d164 299 motorBdirection = 1;
Wonderjack996 0:637e2ec8d164 300 motorAdirection = 0;
Wonderjack996 0:637e2ec8d164 301 motorBpower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 302 motorApower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 303 }
Wonderjack996 0:637e2ec8d164 304
Wonderjack996 0:637e2ec8d164 305 void indietro()
Wonderjack996 0:637e2ec8d164 306 {
Wonderjack996 0:637e2ec8d164 307 motorBdirection = 0;
Wonderjack996 0:637e2ec8d164 308 motorAdirection = 1;
Wonderjack996 0:637e2ec8d164 309 motorBpower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 310 motorApower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 311 }
Wonderjack996 0:637e2ec8d164 312
Wonderjack996 0:637e2ec8d164 313 void destra()
Wonderjack996 0:637e2ec8d164 314 {
Wonderjack996 0:637e2ec8d164 315 motorBdirection = 0;
Wonderjack996 0:637e2ec8d164 316 motorAdirection = 0;
Wonderjack996 0:637e2ec8d164 317 motorBpower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 318 motorApower.pulsewidth(0.007); // 70%
Wonderjack996 0:637e2ec8d164 319 }
Wonderjack996 0:637e2ec8d164 320
Wonderjack996 0:637e2ec8d164 321 void sinistra()
Wonderjack996 0:637e2ec8d164 322 {
Wonderjack996 0:637e2ec8d164 323 motorBdirection = 1;
Wonderjack996 0:637e2ec8d164 324 motorAdirection = 1;
Wonderjack996 0:637e2ec8d164 325 motorBpower.pulsewidth(0.007); // 100%
Wonderjack996 0:637e2ec8d164 326 motorApower.pulsewidth(0.01); // 100%
Wonderjack996 0:637e2ec8d164 327 }
Wonderjack996 0:637e2ec8d164 328
Wonderjack996 0:637e2ec8d164 329 void fermo()
Wonderjack996 0:637e2ec8d164 330 {
Wonderjack996 0:637e2ec8d164 331 motorBpower.pulsewidth(0); // fermo
Wonderjack996 0:637e2ec8d164 332 motorApower.pulsewidth(0); // fermo
Wonderjack996 0:637e2ec8d164 333 }