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 #define destraTrigger PA_10  //D2
00019 #define destraEcho PB_3 //D3
00020 
00021 #define CTRL_SIRENA D11  //D11
00022 /*--------------*/
00023 
00024 /*---VARIABILI GLOBALI---*/
00025 PwmOut motorApower(PWMA); //istanzio oggetto per controllo motore A ( vedi scheda per capire quale sia )
00026 DigitalOut motorAdirection(DIRA);
00027 
00028 PwmOut motorBpower(PWMB); //istanzio oggetto per controllo motore B ( vedi scheda per capire quale sia )
00029 DigitalOut motorBdirection(DIRB);
00030 
00031 HCSR04 ultrasuoniAvanti(avantiTrigger, avantiEcho); //istanzio oggetti per controllo ultrasuoni
00032 HCSR04 ultrasuoniSinistra(sinistraTrigger, sinistraEcho); 
00033 HCSR04 ultrasuoniDestra(destraTrigger, destraEcho); 
00034 
00035 
00036 Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale
00037 
00038 int distanceCtrl = 10; /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
00039                          per capire quando la macchina si deve fermare*/
00040                          
00041 bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena
00042 /*-----------------------*/
00043 
00044 
00045 /*---PROTOTIPI DELLE FUNZIONI---*/
00046 void avanti();
00047 void indietro();
00048 void destra();
00049 void sinistra();
00050 void fermo();
00051 bool checkFermo();
00052 void check_sirena_thread(void const *args);
00053 /*-------------------------------*/
00054 
00055 int main() {
00056     
00057     int avantiDis, sinistraDis;
00058     
00059     Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
00060     
00061     motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
00062     motorApower.period_ms(10);
00063     
00064     fermo();
00065     
00066     while(true){ //utilizzo per mandare in loop la funzione principale ( main tipo )
00067         while(flagSirena){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza
00068             ultrasuoniAvanti.start();
00069             avantiDis = ultrasuoniAvanti.get_dist_cm();
00070             //pc.printf("avanti: %d \r\n",avantiDis);
00071             while( avantiDis >= distanceCtrl && flagSirena ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere
00072                 avanti();
00073                 ultrasuoniAvanti.start();
00074                 avantiDis = ultrasuoniAvanti.get_dist_cm();
00075                 //pc.printf("avanti: %d \r\n",avantiDis);
00076                 wait_ms(10);
00077             }
00078             
00079             fermo();
00080             
00081             if( checkFermo() ){
00082                 destra();
00083                 wait_ms(1000);
00084                 
00085                 ultrasuoniSinistra.start();
00086                 sinistraDis = ultrasuoniSinistra.get_dist_cm();
00087                 //pc.printf("sinistra: %d \r\n",sinistraDis);
00088                 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
00089                     ultrasuoniSinistra.start();
00090                     sinistraDis = ultrasuoniSinistra.get_dist_cm();
00091                     //pc.printf("sinistra: %d \r\n",sinistraDis);
00092                 }
00093                 wait_ms(200);
00094                 
00095                 fermo();
00096             }
00097         }
00098     }
00099     
00100 }
00101 
00102 void check_sirena_thread(void const *args) {
00103     /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento 
00104     con un flag al main che si adatta alla situazione*/
00105     DigitalIn in(CTRL_SIRENA);
00106     while(true){
00107         int val = in.read();
00108         if( val == 1 )
00109             flagSirena = false;
00110         else
00111             flagSirena = true;
00112     } 
00113 }
00114 
00115 bool checkFermo()
00116 {
00117     /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili
00118     La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/
00119     bool ret = true;
00120     int avantiDis, ctrl = 50;
00121     for(int i = 0; i < ctrl; i++){
00122         ultrasuoniAvanti.start();
00123         avantiDis = ultrasuoniAvanti.get_dist_cm();
00124         if( avantiDis >= distanceCtrl ){
00125             ret = false;
00126             break;
00127         }
00128         wait_ms(10);
00129     }
00130     return ret;
00131 }
00132 
00133 void avanti()
00134 {
00135     motorBdirection = 1; 
00136     motorAdirection = 0;
00137     motorBpower.pulsewidth(0.01); // 100%
00138     motorApower.pulsewidth(0.01); // 100%
00139 }
00140 
00141 void indietro()
00142 {
00143     motorBdirection = 0; 
00144     motorAdirection = 1;
00145     motorBpower.pulsewidth(0.01); // 100%
00146     motorApower.pulsewidth(0.01); // 100%
00147 }
00148 
00149 void destra()
00150 {
00151     motorBdirection = 0; 
00152     motorAdirection = 0;
00153     motorBpower.pulsewidth(0.01); // 100%
00154     motorApower.pulsewidth(0.007); // 70%
00155 }
00156 
00157 void sinistra()
00158 {
00159     motorBdirection = 1; 
00160     motorAdirection = 1;
00161     motorBpower.pulsewidth(0.007); // 100%
00162     motorApower.pulsewidth(0.01); // 100%
00163 }
00164 
00165 void fermo()
00166 {
00167     motorBpower.pulsewidth(0); // fermo
00168     motorApower.pulsewidth(0); // fermo
00169 }