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 bool flagSirena = true;
00036 /*-----------------------*/
00037 
00038 
00039 /*---PROTOTIPI DELLE FUNZIONI---*/
00040 void avanti();
00041 void indietro();
00042 void destra();
00043 void sinistra();
00044 void fermo();
00045 bool checkFermo();
00046 void check_sirena_thread(void const *args);
00047 /*-------------------------------*/
00048 
00049 int main() {
00050     
00051     int avantiDis, sinistraDis;
00052     
00053     Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
00054     
00055     motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
00056     motorApower.period_ms(10);
00057     
00058     fermo();
00059     
00060     while(true){ //utilizzo per mandare in loop la funzione principale ( main tipo )
00061         while(flagSirena){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza
00062             ultrasuoniAvanti.start();
00063             avantiDis = ultrasuoniAvanti.get_dist_cm();
00064             //pc.printf("avanti: %d \r\n",avantiDis);
00065             while( avantiDis >= distanceCtrl && flagSirena ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere
00066                 avanti();
00067                 ultrasuoniAvanti.start();
00068                 avantiDis = ultrasuoniAvanti.get_dist_cm();
00069                 //pc.printf("avanti: %d \r\n",avantiDis);
00070                 wait_ms(10);
00071             }
00072             
00073             fermo();
00074             
00075             if( checkFermo() ){
00076                 destra();
00077                 wait_ms(1000);
00078                 
00079                 ultrasuoniSinistra.start();
00080                 sinistraDis = ultrasuoniSinistra.get_dist_cm();
00081                 //pc.printf("sinistra: %d \r\n",sinistraDis);
00082                 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
00083                     ultrasuoniSinistra.start();
00084                     sinistraDis = ultrasuoniSinistra.get_dist_cm();
00085                     //pc.printf("sinistra: %d \r\n",sinistraDis);
00086                 }
00087                 wait_ms(200);
00088                 
00089                 fermo();
00090             }
00091         }
00092     }
00093     
00094 }
00095 
00096 void check_sirena_thread(void const *args) {
00097     /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento 
00098     con un flag al main che si adatta alla situazione*/
00099     //DigitalIn in(D3);
00100     while(true){
00101         //int val = in.read();
00102     } 
00103 }
00104 
00105 bool checkFermo()
00106 {
00107     /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili
00108     La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/
00109     bool ret = true;
00110     int avantiDis, ctrl = 50;
00111     for(int i = 0; i < ctrl; i++){
00112         ultrasuoniAvanti.start();
00113         avantiDis = ultrasuoniAvanti.get_dist_cm();
00114         if( avantiDis >= distanceCtrl ){
00115             ret = false;
00116             break;
00117         }
00118         wait_ms(10);
00119     }
00120     return ret;
00121 }
00122 
00123 void avanti()
00124 {
00125     motorBdirection = 1; 
00126     motorAdirection = 0;
00127     motorBpower.pulsewidth(0.01); // 100%
00128     motorApower.pulsewidth(0.01); // 100%
00129 }
00130 
00131 void indietro()
00132 {
00133     motorBdirection = 0; 
00134     motorAdirection = 1;
00135     motorBpower.pulsewidth(0.01); // 100%
00136     motorApower.pulsewidth(0.01); // 100%
00137 }
00138 
00139 void destra()
00140 {
00141     motorBdirection = 0; 
00142     motorAdirection = 0;
00143     motorBpower.pulsewidth(0.01); // 100%
00144     motorApower.pulsewidth(0.007); // 70%
00145 }
00146 
00147 void sinistra()
00148 {
00149     motorBdirection = 1; 
00150     motorAdirection = 1;
00151     motorBpower.pulsewidth(0.007); // 100%
00152     motorApower.pulsewidth(0.01); // 100%
00153 }
00154 
00155 void fermo()
00156 {
00157     motorBpower.pulsewidth(0); // fermo
00158     motorApower.pulsewidth(0); // fermo
00159 }