main.cpp

00001 /*---LIBRERIE---*/
00002 #include "mbed.h"
00003 #include "rtos.h"
00004 #include "hcsr04.h"
00005 /*--------------*/
00006 
00007 /*---COSTANTI---*/
00008 #define PWMB PC_7  //D9
00009 #define DIRB D8    //D8
00010 
00011 #define PWMA PB_4  //D5
00012 #define DIRA D4    //D4
00013 
00014 #define avantiTrigger PA_5  //D13
00015 #define avantiEcho PA_6     //D12
00016 #define sinistraTrigger PA_8  //D7
00017 #define sinistraEcho PB_10    //D6
00018 /*--------------*/
00019 
00020 /*---VARIABILI GLOBALI---*/
00021 PwmOut motorApower(PWMA); //istanzio oggetto per controllo motore A ( vedi scheda per capire quale sia )
00022 DigitalOut motorAdirection(DIRA);
00023 
00024 PwmOut motorBpower(PWMB); //istanzio oggetto per controllo motore B ( vedi scheda per capire quale sia )
00025 DigitalOut motorBdirection(DIRB);
00026 
00027 HCSR04 ultrasuoniAvanti(avantiTrigger, avantiEcho); //istanzio oggetti per controllo ultrasuoni
00028 HCSR04 ultrasuoniSinistra(sinistraTrigger, sinistraEcho); 
00029 
00030 Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale
00031 
00032 int distanceCtrl = 10; /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
00033                          per capire quando la macchina si deve fermare*/
00034 /*-----------------------*/
00035 
00036 
00037 /*---PROTOTIPI DELLE FUNZIONI---*/
00038 void avanti();
00039 void indietro();
00040 void destra();
00041 void sinistra();
00042 void fermo();
00043 bool checkFermo();
00044 /*-------------------------------*/
00045 
00046 int main() {
00047     
00048     int avantiDis, sinistraDis;
00049     
00050     motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
00051     motorApower.period_ms(10);
00052     
00053     fermo();
00054     
00055     while(true){
00056         ultrasuoniAvanti.start();
00057         avantiDis = ultrasuoniAvanti.get_dist_cm();
00058         pc.printf("avanti: %d \r\n",avantiDis);
00059         while( avantiDis >= distanceCtrl ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere
00060             avanti();
00061             ultrasuoniAvanti.start();
00062             avantiDis = ultrasuoniAvanti.get_dist_cm();
00063             pc.printf("avanti: %d \r\n",avantiDis);
00064             wait_ms(10);
00065         }
00066         
00067         fermo();
00068         
00069         if( checkFermo() ){
00070             destra();
00071             wait_ms(200);
00072             
00073             ultrasuoniSinistra.start();
00074             sinistraDis = ultrasuoniSinistra.get_dist_cm();
00075             pc.printf("sinistra: %d \r\n",sinistraDis);
00076             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
00077                 ultrasuoniSinistra.start();
00078                 sinistraDis = ultrasuoniSinistra.get_dist_cm();
00079                 pc.printf("sinistra: %d \r\n",sinistraDis);
00080             }
00081             wait_ms(200);
00082             
00083             fermo();
00084         }
00085     }
00086     
00087 }
00088 
00089 bool checkFermo()
00090 {
00091     /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili
00092     La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/
00093     bool ret = true;
00094     int avantiDis, ctrl = 50;
00095     for(int i = 0; i < ctrl; i++){
00096         ultrasuoniAvanti.start();
00097         avantiDis = ultrasuoniAvanti.get_dist_cm();
00098         if( avantiDis >= distanceCtrl ){
00099             ret = false;
00100             break;
00101         }
00102         wait_ms(10);
00103     }
00104     return ret;
00105 }
00106 
00107 void avanti()
00108 {
00109     motorBdirection = 1; 
00110     motorAdirection = 0;
00111     motorBpower.pulsewidth(0.01); // 100%
00112     motorApower.pulsewidth(0.01); // 100%
00113 }
00114 
00115 void indietro()
00116 {
00117     motorBdirection = 0; 
00118     motorAdirection = 1;
00119     motorBpower.pulsewidth(0.01); // 100%
00120     motorApower.pulsewidth(0.01); // 100%
00121 }
00122 
00123 void destra()
00124 {
00125     motorBdirection = 0; 
00126     motorAdirection = 0;
00127     motorBpower.pulsewidth(0.01); // 100%
00128     motorApower.pulsewidth(0.007); // 70%
00129 }
00130 
00131 void sinistra()
00132 {
00133     motorBdirection = 1; 
00134     motorAdirection = 1;
00135     motorBpower.pulsewidth(0.007); // 100%
00136     motorApower.pulsewidth(0.01); // 100%
00137 }
00138 
00139 void fermo()
00140 {
00141     motorBpower.pulsewidth(0); // fermo
00142     motorApower.pulsewidth(0); // fermo
00143 }