programma commando motori
Dependencies: mbed HCSR04 mbed-rtos
Diff: main.cpp
- Revision:
- 2:7df0fc7d8d3c
- Parent:
- 1:009a67d6875f
- Child:
- 3:5b91f83642bf
- Child:
- 4:088a0fad279a
--- a/main.cpp Tue May 23 10:46:00 2017 +0000 +++ b/main.cpp Thu May 25 07:12:44 2017 +0000 @@ -1,42 +1,88 @@ -#include "mbed.h" - -/*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 )*/ -#define TX PA_11 -#define RX PA_12 +/*---LIBRERIE---*/ + #include "mbed.h" + #include "rtos.h" + #include "hcsr04.h" +/*--------------*/ -#define PWMB PC_7 //D9 -#define DIRB D8 //D8 +/*---COSTANTI---*/ + #define MAX 999999 //valore intero massimo utilizzabile -#define PWMA PB_4 //D5 -#define DIRA D4 //D4 + #define CTRL_SIRENA D11 //D11 - pin aggancio sensore sonoro + + #define DISTANCE_CTRL 10 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore + per capire quando la macchina si deve fermare*/ -PwmOut motorBpower(PWMB); -DigitalOut motorBdirection(DIRB); - -PwmOut motorApower(PWMA); -DigitalOut motorAdirection(DIRA); - -Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale + /*---PIN BLUETHOOT---*/ + /*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 )*/ + #define TX PA_11 + #define RX PA_12 + /*-------------------*/ + + /*---PIN CONTROLLO MOTORI---*/ + #define PWMB PC_7 //D9 + #define DIRB D8 //D8 + + #define PWMA PB_4 //D5 + #define DIRA D4 //D4 + /*--------------------------*/ + + /*---PIN ULTRASUONI---*/ + #define AVANTI_TRIGGER PA_5 //D13 + #define AVANTI_ECHO PA_6 //D12 + #define SINISTRA_TRIGGER PA_8 //D7 + #define SINISTRA_ECHO PB_10 //D6 + #define DESTRA_TRIGGER PA_10 //D2 + #define DESTRA_ECHO PB_3 //D3 + /*--------------------*/ +/*--------------*/ -Serial device(TX, RX); //oggetto per gestione seriale bluethoot +/*---VARIABILI---*/ + /*---CONTROLLO MOTORI---*/ + PwmOut motorBpower(PWMB); //oggetti controllo motori + DigitalOut motorBdirection(DIRB); + + PwmOut motorApower(PWMA); + DigitalOut motorAdirection(DIRA); + /*---------------------*/ + + /*---CONTROLLO ULTRASUONI---*/ + HCSR04 ultrasuoniAvanti(AVANTI_TRIGGER, AVANTI_ECHO); //istanzio oggetti per utilizzo ultrasuoni + HCSR04 ultrasuoniSinistra(SINISTRA_TRIGGER, SINISTRA_ECHO); + HCSR04 ultrasuoniDestra(DESTRA_TRIGGER, DESTRA_ECHO); + /*--------------------------*/ + + Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale + + Serial device(TX, RX); //oggetto per gestione seriale bluethoot + + bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena + + bool flagAutomatic; //utilizzo per sapere se utilizzare modalita automatica o meno +/*----------------*/ -void avanti(); -void indietro(); -void destra(); -void sinistra(); -void fermo(); +/*---PROTOTIPI---*/ + void avanti(); + void indietro(); + void destra(); + void sinistra(); + void fermo(); + bool checkFermo(); + void automaticProgram(); + void check_sirena_thread(void const *args); + void check_stop_thread(void const *args); +/*---------------*/ int main() { unsigned char rx; - motorBpower.period_ms(10); - motorApower.period_ms(10); + motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare ) + motorApower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare ) while(true){ if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot - pc.printf("recived: %c \r\n",rx); //stampa a video + //pc.printf("recived: %c \r\n",rx); //stampa a video switch(rx){ //in base alla lettera che riceve sa cosa deve fare case 'a': avanti(); @@ -53,12 +99,148 @@ case 'e': fermo(); break; + case 'f': + flagAutomatic = true; + automaticProgram(); //funzione pilota automatico + break; } } } } +void automaticProgram() +{ + int avantiDis, distance; //la variabile direction contiene 0 se il robot deve andare a destra, 1 se deve andare a sinistra + + int distanceDx, distanceSx; + + Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena + Thread ctrlStopThread(check_stop_thread); // start Thread controllo sirena + + fermo(); + + while(flagAutomatic){ //flag modificabile dal thread che aspetta il segnale di stop + while(flagSirena && flagAutomatic){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza + ultrasuoniAvanti.start(); + avantiDis = ultrasuoniAvanti.get_dist_cm(); + //pc.printf("avanti: %d \r\n",avantiDis); + while( avantiDis >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere + avanti(); + ultrasuoniAvanti.start(); + avantiDis = ultrasuoniAvanti.get_dist_cm(); + //pc.printf("avanti: %d \r\n",avantiDis); + wait_ms(10); + } + + fermo(); + + if( checkFermo() && flagSirena && flagAutomatic ){ + /*funzione utilizzata per leggere un valore attendibile ( nel range dell ultrasuono ). se dopo 100 tentativi + il valore non e' ancora attendibile allora esco dal controllo in modo da non rimanere bloccato*/ + 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 ) + ultrasuoniDestra.start(); + distanceDx = ultrasuoniDestra.get_dist_cm(); + //wait_ms(10); + } + + 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 ) + ultrasuoniSinistra.start(); + distanceSx = ultrasuoniSinistra.get_dist_cm(); + //wait_ms(10); + } + + /*controllo se il valori valori di uscita dalle iterazioni sono attendibili ( nel range ) + se non soddisfano i criteri allora li setto al valore MAX in modo che i seguenti controlli non li prendano + in considerazione*/ + if( distanceSx < 2 || distanceSx > 400 ) + distanceSx = MAX; + + if( distanceDx < 2 || distanceDx > 400 ) + distanceDx = MAX; + + pc.printf("distanceSx: %d distanceDx: %d\r\n",distanceSx, distanceDx); + + //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 + if( distanceSx > distanceDx ){ + sinistra(); + wait_ms(1000); + + ultrasuoniDestra.start(); + distance = ultrasuoniDestra.get_dist_cm(); + //pc.printf("sinistra: %d \r\n",sinistraDis); + 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 + ultrasuoniDestra.start(); + distance = ultrasuoniDestra.get_dist_cm(); + //pc.printf("distance: %d \r\n",distance); + } + } + else{ + destra(); + wait_ms(1000); + + ultrasuoniSinistra.start(); + distance = ultrasuoniSinistra.get_dist_cm(); + //pc.printf("sinistra: %d \r\n",sinistraDis); + 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 + ultrasuoniSinistra.start(); + distance = ultrasuoniSinistra.get_dist_cm(); + //pc.printf("distance: %d \r\n",distance); + } + } + + wait_ms(200); + fermo(); + } + } + } +} + +void check_stop_thread(void const *args) +{ + /*thread che controlla se l'utente via dispositivi ha chiesto lo stop del proamma automatico*/ + unsigned char rx; + while(true) { + if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo + rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot + if( rx == 'g' ) + flagAutomatic = false; + } + } +} + +void check_sirena_thread(void const *args) +{ + /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento + con un flag al main che si adatta alla situazione*/ + DigitalIn in(CTRL_SIRENA); + while(true){ + int val = in.read(); + if( val == 1 ) + flagSirena = false; + else + flagSirena = true; + } +} + +bool checkFermo() +{ + /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili + La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/ + bool ret = true; + int avantiDis, ctrl = 50; + for(int i = 0; i < ctrl; i++){ + ultrasuoniAvanti.start(); + avantiDis = ultrasuoniAvanti.get_dist_cm(); + if( avantiDis >= DISTANCE_CTRL ){ + ret = false; + break; + } + wait_ms(10); + } + return ret; +} + void avanti() { motorBdirection = 1;