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