programma steve 22-05
Dependencies: HCSR04 mbed-rtos mbed
main.cpp
- Committer:
- Wonderjack996
- Date:
- 2017-05-23
- Revision:
- 0:d8dd32751c38
File content as of revision 0:d8dd32751c38:
/*---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 /*--------------*/ /*---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); 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; /*-----------------------*/ /*---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(D3); while(true){ //int val = in.read(); } } 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 }