programma steve 15-05-2017
Dependencies: HCSR04 mbed-rtos mbed
main.cpp
- Committer:
- Wonderjack996
- Date:
- 2017-05-22
- Revision:
- 0:cd38bc5c5a72
File content as of revision 0:cd38bc5c5a72:
/*---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*/ /*-----------------------*/ /*---PROTOTIPI DELLE FUNZIONI---*/ void avanti(); void indietro(); void destra(); void sinistra(); void fermo(); bool checkFermo(); /*-------------------------------*/ int main() { int avantiDis, sinistraDis; motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare ) motorApower.period_ms(10); fermo(); while(true){ ultrasuoniAvanti.start(); avantiDis = ultrasuoniAvanti.get_dist_cm(); pc.printf("avanti: %d \r\n",avantiDis); while( avantiDis >= distanceCtrl ){ //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(200); ultrasuoniSinistra.start(); sinistraDis = ultrasuoniSinistra.get_dist_cm(); pc.printf("sinistra: %d \r\n",sinistraDis); while(sinistraDis >= distanceCtrl){ //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(); } } } 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 }