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?
User | Revision | Line number | New 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 | } |