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?
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 | 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 | } |