programma steve fine giornata 23-05
Dependencies: HCSR04 mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:2fa228a32a83
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed May 24 08:04:12 2017 +0000 @@ -0,0 +1,169 @@ +/*---LIBRERIE---*/ +#include "mbed.h" +#include "rtos.h" +#include "hcsr04.h" +/*--------------*/ + +/*---COSTANTI---*/ +#define PWMB PC_7 //D9 +#define DIRB D8 //D8 + +#define PWMA PB_4 //D5 +#define DIRA D4 //D4 + +#define avantiTrigger PA_5 //D13 +#define avantiEcho PA_6 //D12 +#define sinistraTrigger PA_8 //D7 +#define sinistraEcho PB_10 //D6 +#define destraTrigger PA_10 //D2 +#define destraEcho PB_3 //D3 + +#define CTRL_SIRENA D11 //D11 +/*--------------*/ + +/*---VARIABILI GLOBALI---*/ +PwmOut motorApower(PWMA); //istanzio oggetto per controllo motore A ( vedi scheda per capire quale sia ) +DigitalOut motorAdirection(DIRA); + +PwmOut motorBpower(PWMB); //istanzio oggetto per controllo motore B ( vedi scheda per capire quale sia ) +DigitalOut motorBdirection(DIRB); + +HCSR04 ultrasuoniAvanti(avantiTrigger, avantiEcho); //istanzio oggetti per controllo ultrasuoni +HCSR04 ultrasuoniSinistra(sinistraTrigger, sinistraEcho); +HCSR04 ultrasuoniDestra(destraTrigger, destraEcho); + + +Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale + +int distanceCtrl = 10; /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore + per capire quando la macchina si deve fermare*/ + +bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena +/*-----------------------*/ + + +/*---PROTOTIPI DELLE FUNZIONI---*/ +void avanti(); +void indietro(); +void destra(); +void sinistra(); +void fermo(); +bool checkFermo(); +void check_sirena_thread(void const *args); +/*-------------------------------*/ + +int main() { + + int avantiDis, sinistraDis; + + Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena + + motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare ) + motorApower.period_ms(10); + + fermo(); + + while(true){ //utilizzo per mandare in loop la funzione principale ( main tipo ) + while(flagSirena){ //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 >= distanceCtrl && flagSirena ){ //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() ){ + destra(); + wait_ms(1000); + + ultrasuoniSinistra.start(); + sinistraDis = ultrasuoniSinistra.get_dist_cm(); + //pc.printf("sinistra: %d \r\n",sinistraDis); + while(sinistraDis >= distanceCtrl && flagSirena ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana + ultrasuoniSinistra.start(); + sinistraDis = ultrasuoniSinistra.get_dist_cm(); + //pc.printf("sinistra: %d \r\n",sinistraDis); + } + wait_ms(200); + + fermo(); + } + } + } + +} + +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 >= distanceCtrl ){ + ret = false; + break; + } + wait_ms(10); + } + return ret; +} + +void avanti() +{ + motorBdirection = 1; + motorAdirection = 0; + motorBpower.pulsewidth(0.01); // 100% + motorApower.pulsewidth(0.01); // 100% +} + +void indietro() +{ + motorBdirection = 0; + motorAdirection = 1; + motorBpower.pulsewidth(0.01); // 100% + motorApower.pulsewidth(0.01); // 100% +} + +void destra() +{ + motorBdirection = 0; + motorAdirection = 0; + motorBpower.pulsewidth(0.01); // 100% + motorApower.pulsewidth(0.007); // 70% +} + +void sinistra() +{ + motorBdirection = 1; + motorAdirection = 1; + motorBpower.pulsewidth(0.007); // 100% + motorApower.pulsewidth(0.01); // 100% +} + +void fermo() +{ + motorBpower.pulsewidth(0); // fermo + motorApower.pulsewidth(0); // fermo +}