programma commando motori
Dependencies: mbed HCSR04 mbed-rtos
main.cpp@2:7df0fc7d8d3c, 2017-05-25 (annotated)
- Committer:
- Wonderjack996
- Date:
- Thu May 25 07:12:44 2017 +0000
- Revision:
- 2:7df0fc7d8d3c
- Parent:
- 1:009a67d6875f
- Child:
- 3:5b91f83642bf
- Child:
- 4:088a0fad279a
programma steve fine giornata 24-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 | 2:7df0fc7d8d3c | 12 | #define DISTANCE_CTRL 10 /*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 | 2:7df0fc7d8d3c | 15 | /*---PIN BLUETHOOT---*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 16 | /*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 | 17 | #define TX PA_11 |
Wonderjack996 | 2:7df0fc7d8d3c | 18 | #define RX PA_12 |
Wonderjack996 | 2:7df0fc7d8d3c | 19 | /*-------------------*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 20 | |
Wonderjack996 | 2:7df0fc7d8d3c | 21 | /*---PIN CONTROLLO MOTORI---*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 22 | #define PWMB PC_7 //D9 |
Wonderjack996 | 2:7df0fc7d8d3c | 23 | #define DIRB D8 //D8 |
Wonderjack996 | 2:7df0fc7d8d3c | 24 | |
Wonderjack996 | 2:7df0fc7d8d3c | 25 | #define PWMA PB_4 //D5 |
Wonderjack996 | 2:7df0fc7d8d3c | 26 | #define DIRA D4 //D4 |
Wonderjack996 | 2:7df0fc7d8d3c | 27 | /*--------------------------*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 28 | |
Wonderjack996 | 2:7df0fc7d8d3c | 29 | /*---PIN ULTRASUONI---*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 30 | #define AVANTI_TRIGGER PA_5 //D13 |
Wonderjack996 | 2:7df0fc7d8d3c | 31 | #define AVANTI_ECHO PA_6 //D12 |
Wonderjack996 | 2:7df0fc7d8d3c | 32 | #define SINISTRA_TRIGGER PA_8 //D7 |
Wonderjack996 | 2:7df0fc7d8d3c | 33 | #define SINISTRA_ECHO PB_10 //D6 |
Wonderjack996 | 2:7df0fc7d8d3c | 34 | #define DESTRA_TRIGGER PA_10 //D2 |
Wonderjack996 | 2:7df0fc7d8d3c | 35 | #define DESTRA_ECHO PB_3 //D3 |
Wonderjack996 | 2:7df0fc7d8d3c | 36 | /*--------------------*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 37 | /*--------------*/ |
Wonderjack996 | 1:009a67d6875f | 38 | |
Wonderjack996 | 2:7df0fc7d8d3c | 39 | /*---VARIABILI---*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 40 | /*---CONTROLLO MOTORI---*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 41 | PwmOut motorBpower(PWMB); //oggetti controllo motori |
Wonderjack996 | 2:7df0fc7d8d3c | 42 | DigitalOut motorBdirection(DIRB); |
Wonderjack996 | 2:7df0fc7d8d3c | 43 | |
Wonderjack996 | 2:7df0fc7d8d3c | 44 | PwmOut motorApower(PWMA); |
Wonderjack996 | 2:7df0fc7d8d3c | 45 | DigitalOut motorAdirection(DIRA); |
Wonderjack996 | 2:7df0fc7d8d3c | 46 | /*---------------------*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 47 | |
Wonderjack996 | 2:7df0fc7d8d3c | 48 | /*---CONTROLLO ULTRASUONI---*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 49 | HCSR04 ultrasuoniAvanti(AVANTI_TRIGGER, AVANTI_ECHO); //istanzio oggetti per utilizzo ultrasuoni |
Wonderjack996 | 2:7df0fc7d8d3c | 50 | HCSR04 ultrasuoniSinistra(SINISTRA_TRIGGER, SINISTRA_ECHO); |
Wonderjack996 | 2:7df0fc7d8d3c | 51 | HCSR04 ultrasuoniDestra(DESTRA_TRIGGER, DESTRA_ECHO); |
Wonderjack996 | 2:7df0fc7d8d3c | 52 | /*--------------------------*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 53 | |
Wonderjack996 | 2:7df0fc7d8d3c | 54 | Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale |
Wonderjack996 | 2:7df0fc7d8d3c | 55 | |
Wonderjack996 | 2:7df0fc7d8d3c | 56 | Serial device(TX, RX); //oggetto per gestione seriale bluethoot |
Wonderjack996 | 2:7df0fc7d8d3c | 57 | |
Wonderjack996 | 2:7df0fc7d8d3c | 58 | bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena |
Wonderjack996 | 2:7df0fc7d8d3c | 59 | |
Wonderjack996 | 2:7df0fc7d8d3c | 60 | bool flagAutomatic; //utilizzo per sapere se utilizzare modalita automatica o meno |
Wonderjack996 | 2:7df0fc7d8d3c | 61 | /*----------------*/ |
Wonderjack996 | 1:009a67d6875f | 62 | |
Wonderjack996 | 2:7df0fc7d8d3c | 63 | /*---PROTOTIPI---*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 64 | void avanti(); |
Wonderjack996 | 2:7df0fc7d8d3c | 65 | void indietro(); |
Wonderjack996 | 2:7df0fc7d8d3c | 66 | void destra(); |
Wonderjack996 | 2:7df0fc7d8d3c | 67 | void sinistra(); |
Wonderjack996 | 2:7df0fc7d8d3c | 68 | void fermo(); |
Wonderjack996 | 2:7df0fc7d8d3c | 69 | bool checkFermo(); |
Wonderjack996 | 2:7df0fc7d8d3c | 70 | void automaticProgram(); |
Wonderjack996 | 2:7df0fc7d8d3c | 71 | void check_sirena_thread(void const *args); |
Wonderjack996 | 2:7df0fc7d8d3c | 72 | void check_stop_thread(void const *args); |
Wonderjack996 | 2:7df0fc7d8d3c | 73 | /*---------------*/ |
Wonderjack996 | 0:637e2ec8d164 | 74 | |
Wonderjack996 | 0:637e2ec8d164 | 75 | int main() { |
Wonderjack996 | 0:637e2ec8d164 | 76 | |
Wonderjack996 | 1:009a67d6875f | 77 | unsigned char rx; |
Wonderjack996 | 1:009a67d6875f | 78 | |
Wonderjack996 | 2:7df0fc7d8d3c | 79 | motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare ) |
Wonderjack996 | 2:7df0fc7d8d3c | 80 | motorApower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare ) |
Wonderjack996 | 0:637e2ec8d164 | 81 | |
Wonderjack996 | 1:009a67d6875f | 82 | while(true){ |
Wonderjack996 | 1:009a67d6875f | 83 | if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo |
Wonderjack996 | 1:009a67d6875f | 84 | rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot |
Wonderjack996 | 2:7df0fc7d8d3c | 85 | //pc.printf("recived: %c \r\n",rx); //stampa a video |
Wonderjack996 | 1:009a67d6875f | 86 | switch(rx){ //in base alla lettera che riceve sa cosa deve fare |
Wonderjack996 | 1:009a67d6875f | 87 | case 'a': |
Wonderjack996 | 1:009a67d6875f | 88 | avanti(); |
Wonderjack996 | 1:009a67d6875f | 89 | break; |
Wonderjack996 | 1:009a67d6875f | 90 | case 'b': |
Wonderjack996 | 1:009a67d6875f | 91 | indietro(); |
Wonderjack996 | 1:009a67d6875f | 92 | break; |
Wonderjack996 | 1:009a67d6875f | 93 | case 'c': |
Wonderjack996 | 1:009a67d6875f | 94 | destra(); |
Wonderjack996 | 1:009a67d6875f | 95 | break; |
Wonderjack996 | 1:009a67d6875f | 96 | case 'd': |
Wonderjack996 | 1:009a67d6875f | 97 | sinistra(); |
Wonderjack996 | 1:009a67d6875f | 98 | break; |
Wonderjack996 | 1:009a67d6875f | 99 | case 'e': |
Wonderjack996 | 1:009a67d6875f | 100 | fermo(); |
Wonderjack996 | 1:009a67d6875f | 101 | break; |
Wonderjack996 | 2:7df0fc7d8d3c | 102 | case 'f': |
Wonderjack996 | 2:7df0fc7d8d3c | 103 | flagAutomatic = true; |
Wonderjack996 | 2:7df0fc7d8d3c | 104 | automaticProgram(); //funzione pilota automatico |
Wonderjack996 | 2:7df0fc7d8d3c | 105 | break; |
Wonderjack996 | 1:009a67d6875f | 106 | } |
Wonderjack996 | 1:009a67d6875f | 107 | } |
Wonderjack996 | 1:009a67d6875f | 108 | } |
Wonderjack996 | 0:637e2ec8d164 | 109 | |
Wonderjack996 | 0:637e2ec8d164 | 110 | } |
Wonderjack996 | 0:637e2ec8d164 | 111 | |
Wonderjack996 | 2:7df0fc7d8d3c | 112 | void automaticProgram() |
Wonderjack996 | 2:7df0fc7d8d3c | 113 | { |
Wonderjack996 | 2:7df0fc7d8d3c | 114 | int avantiDis, distance; //la variabile direction contiene 0 se il robot deve andare a destra, 1 se deve andare a sinistra |
Wonderjack996 | 2:7df0fc7d8d3c | 115 | |
Wonderjack996 | 2:7df0fc7d8d3c | 116 | int distanceDx, distanceSx; |
Wonderjack996 | 2:7df0fc7d8d3c | 117 | |
Wonderjack996 | 2:7df0fc7d8d3c | 118 | Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena |
Wonderjack996 | 2:7df0fc7d8d3c | 119 | Thread ctrlStopThread(check_stop_thread); // start Thread controllo sirena |
Wonderjack996 | 2:7df0fc7d8d3c | 120 | |
Wonderjack996 | 2:7df0fc7d8d3c | 121 | fermo(); |
Wonderjack996 | 2:7df0fc7d8d3c | 122 | |
Wonderjack996 | 2:7df0fc7d8d3c | 123 | while(flagAutomatic){ //flag modificabile dal thread che aspetta il segnale di stop |
Wonderjack996 | 2:7df0fc7d8d3c | 124 | while(flagSirena && flagAutomatic){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza |
Wonderjack996 | 2:7df0fc7d8d3c | 125 | ultrasuoniAvanti.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 126 | avantiDis = ultrasuoniAvanti.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 127 | //pc.printf("avanti: %d \r\n",avantiDis); |
Wonderjack996 | 2:7df0fc7d8d3c | 128 | 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 | 129 | avanti(); |
Wonderjack996 | 2:7df0fc7d8d3c | 130 | ultrasuoniAvanti.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 131 | avantiDis = ultrasuoniAvanti.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 132 | //pc.printf("avanti: %d \r\n",avantiDis); |
Wonderjack996 | 2:7df0fc7d8d3c | 133 | wait_ms(10); |
Wonderjack996 | 2:7df0fc7d8d3c | 134 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 135 | |
Wonderjack996 | 2:7df0fc7d8d3c | 136 | fermo(); |
Wonderjack996 | 2:7df0fc7d8d3c | 137 | |
Wonderjack996 | 2:7df0fc7d8d3c | 138 | if( checkFermo() && flagSirena && flagAutomatic ){ |
Wonderjack996 | 2:7df0fc7d8d3c | 139 | /*funzione utilizzata per leggere un valore attendibile ( nel range dell ultrasuono ). se dopo 100 tentativi |
Wonderjack996 | 2:7df0fc7d8d3c | 140 | il valore non e' ancora attendibile allora esco dal controllo in modo da non rimanere bloccato*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 141 | 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 | 142 | ultrasuoniDestra.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 143 | distanceDx = ultrasuoniDestra.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 144 | //wait_ms(10); |
Wonderjack996 | 2:7df0fc7d8d3c | 145 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 146 | |
Wonderjack996 | 2:7df0fc7d8d3c | 147 | 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 | 148 | ultrasuoniSinistra.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 149 | distanceSx = ultrasuoniSinistra.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 150 | //wait_ms(10); |
Wonderjack996 | 2:7df0fc7d8d3c | 151 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 152 | |
Wonderjack996 | 2:7df0fc7d8d3c | 153 | /*controllo se il valori valori di uscita dalle iterazioni sono attendibili ( nel range ) |
Wonderjack996 | 2:7df0fc7d8d3c | 154 | se non soddisfano i criteri allora li setto al valore MAX in modo che i seguenti controlli non li prendano |
Wonderjack996 | 2:7df0fc7d8d3c | 155 | in considerazione*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 156 | if( distanceSx < 2 || distanceSx > 400 ) |
Wonderjack996 | 2:7df0fc7d8d3c | 157 | distanceSx = MAX; |
Wonderjack996 | 2:7df0fc7d8d3c | 158 | |
Wonderjack996 | 2:7df0fc7d8d3c | 159 | if( distanceDx < 2 || distanceDx > 400 ) |
Wonderjack996 | 2:7df0fc7d8d3c | 160 | distanceDx = MAX; |
Wonderjack996 | 2:7df0fc7d8d3c | 161 | |
Wonderjack996 | 2:7df0fc7d8d3c | 162 | pc.printf("distanceSx: %d distanceDx: %d\r\n",distanceSx, distanceDx); |
Wonderjack996 | 2:7df0fc7d8d3c | 163 | |
Wonderjack996 | 2:7df0fc7d8d3c | 164 | //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 | 165 | if( distanceSx > distanceDx ){ |
Wonderjack996 | 2:7df0fc7d8d3c | 166 | sinistra(); |
Wonderjack996 | 2:7df0fc7d8d3c | 167 | wait_ms(1000); |
Wonderjack996 | 2:7df0fc7d8d3c | 168 | |
Wonderjack996 | 2:7df0fc7d8d3c | 169 | ultrasuoniDestra.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 170 | distance = ultrasuoniDestra.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 171 | //pc.printf("sinistra: %d \r\n",sinistraDis); |
Wonderjack996 | 2:7df0fc7d8d3c | 172 | 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 | 173 | ultrasuoniDestra.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 174 | distance = ultrasuoniDestra.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 175 | //pc.printf("distance: %d \r\n",distance); |
Wonderjack996 | 2:7df0fc7d8d3c | 176 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 177 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 178 | else{ |
Wonderjack996 | 2:7df0fc7d8d3c | 179 | destra(); |
Wonderjack996 | 2:7df0fc7d8d3c | 180 | wait_ms(1000); |
Wonderjack996 | 2:7df0fc7d8d3c | 181 | |
Wonderjack996 | 2:7df0fc7d8d3c | 182 | ultrasuoniSinistra.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 183 | distance = ultrasuoniSinistra.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 184 | //pc.printf("sinistra: %d \r\n",sinistraDis); |
Wonderjack996 | 2:7df0fc7d8d3c | 185 | 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 | 186 | ultrasuoniSinistra.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 187 | distance = ultrasuoniSinistra.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 188 | //pc.printf("distance: %d \r\n",distance); |
Wonderjack996 | 2:7df0fc7d8d3c | 189 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 190 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 191 | |
Wonderjack996 | 2:7df0fc7d8d3c | 192 | wait_ms(200); |
Wonderjack996 | 2:7df0fc7d8d3c | 193 | fermo(); |
Wonderjack996 | 2:7df0fc7d8d3c | 194 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 195 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 196 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 197 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 198 | |
Wonderjack996 | 2:7df0fc7d8d3c | 199 | void check_stop_thread(void const *args) |
Wonderjack996 | 2:7df0fc7d8d3c | 200 | { |
Wonderjack996 | 2:7df0fc7d8d3c | 201 | /*thread che controlla se l'utente via dispositivi ha chiesto lo stop del proamma automatico*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 202 | unsigned char rx; |
Wonderjack996 | 2:7df0fc7d8d3c | 203 | while(true) { |
Wonderjack996 | 2:7df0fc7d8d3c | 204 | if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo |
Wonderjack996 | 2:7df0fc7d8d3c | 205 | rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot |
Wonderjack996 | 2:7df0fc7d8d3c | 206 | if( rx == 'g' ) |
Wonderjack996 | 2:7df0fc7d8d3c | 207 | flagAutomatic = false; |
Wonderjack996 | 2:7df0fc7d8d3c | 208 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 209 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 210 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 211 | |
Wonderjack996 | 2:7df0fc7d8d3c | 212 | void check_sirena_thread(void const *args) |
Wonderjack996 | 2:7df0fc7d8d3c | 213 | { |
Wonderjack996 | 2:7df0fc7d8d3c | 214 | /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento |
Wonderjack996 | 2:7df0fc7d8d3c | 215 | con un flag al main che si adatta alla situazione*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 216 | DigitalIn in(CTRL_SIRENA); |
Wonderjack996 | 2:7df0fc7d8d3c | 217 | while(true){ |
Wonderjack996 | 2:7df0fc7d8d3c | 218 | int val = in.read(); |
Wonderjack996 | 2:7df0fc7d8d3c | 219 | if( val == 1 ) |
Wonderjack996 | 2:7df0fc7d8d3c | 220 | flagSirena = false; |
Wonderjack996 | 2:7df0fc7d8d3c | 221 | else |
Wonderjack996 | 2:7df0fc7d8d3c | 222 | flagSirena = true; |
Wonderjack996 | 2:7df0fc7d8d3c | 223 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 224 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 225 | |
Wonderjack996 | 2:7df0fc7d8d3c | 226 | bool checkFermo() |
Wonderjack996 | 2:7df0fc7d8d3c | 227 | { |
Wonderjack996 | 2:7df0fc7d8d3c | 228 | /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili |
Wonderjack996 | 2:7df0fc7d8d3c | 229 | La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/ |
Wonderjack996 | 2:7df0fc7d8d3c | 230 | bool ret = true; |
Wonderjack996 | 2:7df0fc7d8d3c | 231 | int avantiDis, ctrl = 50; |
Wonderjack996 | 2:7df0fc7d8d3c | 232 | for(int i = 0; i < ctrl; i++){ |
Wonderjack996 | 2:7df0fc7d8d3c | 233 | ultrasuoniAvanti.start(); |
Wonderjack996 | 2:7df0fc7d8d3c | 234 | avantiDis = ultrasuoniAvanti.get_dist_cm(); |
Wonderjack996 | 2:7df0fc7d8d3c | 235 | if( avantiDis >= DISTANCE_CTRL ){ |
Wonderjack996 | 2:7df0fc7d8d3c | 236 | ret = false; |
Wonderjack996 | 2:7df0fc7d8d3c | 237 | break; |
Wonderjack996 | 2:7df0fc7d8d3c | 238 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 239 | wait_ms(10); |
Wonderjack996 | 2:7df0fc7d8d3c | 240 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 241 | return ret; |
Wonderjack996 | 2:7df0fc7d8d3c | 242 | } |
Wonderjack996 | 2:7df0fc7d8d3c | 243 | |
Wonderjack996 | 0:637e2ec8d164 | 244 | void avanti() |
Wonderjack996 | 0:637e2ec8d164 | 245 | { |
Wonderjack996 | 0:637e2ec8d164 | 246 | motorBdirection = 1; |
Wonderjack996 | 0:637e2ec8d164 | 247 | motorAdirection = 0; |
Wonderjack996 | 0:637e2ec8d164 | 248 | motorBpower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:637e2ec8d164 | 249 | motorApower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:637e2ec8d164 | 250 | } |
Wonderjack996 | 0:637e2ec8d164 | 251 | |
Wonderjack996 | 0:637e2ec8d164 | 252 | void indietro() |
Wonderjack996 | 0:637e2ec8d164 | 253 | { |
Wonderjack996 | 0:637e2ec8d164 | 254 | motorBdirection = 0; |
Wonderjack996 | 0:637e2ec8d164 | 255 | motorAdirection = 1; |
Wonderjack996 | 0:637e2ec8d164 | 256 | motorBpower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:637e2ec8d164 | 257 | motorApower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:637e2ec8d164 | 258 | } |
Wonderjack996 | 0:637e2ec8d164 | 259 | |
Wonderjack996 | 0:637e2ec8d164 | 260 | void destra() |
Wonderjack996 | 0:637e2ec8d164 | 261 | { |
Wonderjack996 | 0:637e2ec8d164 | 262 | motorBdirection = 0; |
Wonderjack996 | 0:637e2ec8d164 | 263 | motorAdirection = 0; |
Wonderjack996 | 0:637e2ec8d164 | 264 | motorBpower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:637e2ec8d164 | 265 | motorApower.pulsewidth(0.007); // 70% |
Wonderjack996 | 0:637e2ec8d164 | 266 | } |
Wonderjack996 | 0:637e2ec8d164 | 267 | |
Wonderjack996 | 0:637e2ec8d164 | 268 | void sinistra() |
Wonderjack996 | 0:637e2ec8d164 | 269 | { |
Wonderjack996 | 0:637e2ec8d164 | 270 | motorBdirection = 1; |
Wonderjack996 | 0:637e2ec8d164 | 271 | motorAdirection = 1; |
Wonderjack996 | 0:637e2ec8d164 | 272 | motorBpower.pulsewidth(0.007); // 100% |
Wonderjack996 | 0:637e2ec8d164 | 273 | motorApower.pulsewidth(0.01); // 100% |
Wonderjack996 | 0:637e2ec8d164 | 274 | } |
Wonderjack996 | 0:637e2ec8d164 | 275 | |
Wonderjack996 | 0:637e2ec8d164 | 276 | void fermo() |
Wonderjack996 | 0:637e2ec8d164 | 277 | { |
Wonderjack996 | 0:637e2ec8d164 | 278 | motorBpower.pulsewidth(0); // fermo |
Wonderjack996 | 0:637e2ec8d164 | 279 | motorApower.pulsewidth(0); // fermo |
Wonderjack996 | 0:637e2ec8d164 | 280 | } |