main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "Hc05.h"
00004 #include "RawSerialPc.h"
00005 
00006 //JOSEP
00007 RawSerialPc pc(PC_10, PC_11);    //Port serie TX->PC_10 RX->PC_11
00008 Hc05 bt(D8, D2);        //Crea l'objecte bt, connectat als pins D8 (TX) i D2 (RX) (és la connexió sèrie BT)
00009 
00010 //EVA
00011     int maximumRange = 4000; // Rang màxim
00012     int minimumRange = 30; // Rang mínim
00013 
00014     float duration; //Variable per calcular l'amplada en us
00015     float distance; //Variable per calcular la distància en mm després de la conversió
00016     
00017     float S1,S2,S3;
00018     
00019     char F[100];     //Taula per poder passar els enters a strings
00020 
00021 float SensorUltrasons(int n){
00022 
00023     if (n==1){
00024         DigitalOut trigPin1(D3); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant)
00025         Timer timer1;            // Creem un comptador per saber el temps que té de resposta
00026         
00027         trigPin1 = 0;
00028         wait_ms(2);
00029 
00030         trigPin1 = 1;    //Envia un pols de 10ms pel pin “Trigger”
00031         wait_ms(10);
00032         trigPin1 = 0;
00033  
00034         DigitalIn echoPin1(D3); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida)
00035  
00036         while (!echoPin1);   //Espera a la pujada
00037         timer1.start();
00038 
00039         while (echoPin1);    //Espera a la baixada
00040         duration = timer1.read_us();
00041     }
00042     
00043     if (n==2){
00044         DigitalOut trigPin2(D5); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant)
00045         Timer timer2;            // Creem un comptador per saber el temps que té de resposta
00046         
00047         trigPin2 = 0;
00048         wait_ms(2);
00049 
00050         trigPin2 = 1;    //Envia un pols de 10ms pel pin “Trigger”
00051         wait_ms(10);
00052         trigPin2 = 0;
00053  
00054         DigitalIn echoPin2(D5); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida)
00055  
00056         while (!echoPin2);   //Espera a la pujada
00057         timer2.start();
00058 
00059         while (echoPin2);    //Espera a la baixada
00060         duration = timer2.read_us();
00061     
00062     }
00063     
00064     if (n==3){
00065         DigitalOut trigPin3(D6); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant)
00066         Timer timer3;            // Creem un comptador per saber el temps que té de resposta
00067         
00068         trigPin3 = 0;
00069         wait_ms(2);
00070 
00071         trigPin3 = 1;    //Envia un pols de 10ms pel pin “Trigger”
00072         wait_ms(10);
00073         trigPin3 = 0;
00074  
00075         DigitalIn echoPin3(D6); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida)
00076  
00077         while (!echoPin3);   //Espera a la pujada
00078         timer3.start();
00079 
00080         while (echoPin3);    //Espera a la baixada
00081         duration = timer3.read_us();
00082     }
00083     
00084     // Calcular la distancia (en mm)
00085     float x = (duration/5.6);     
00086     return x;
00087     }
00088 
00089 
00090 void ModeAutomatic(){
00091         
00092     float a=3999;
00093     float b=3999;
00094     float c=3999;
00095     
00096     pc.enviaString("V050FM2");
00097     pc.enviaString("V050FM3");
00098     //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 0º
00099     while ((a>100)&&(bt.getMode()==3)){
00100     
00101         a=SensorUltrasons(1); //Crida la funció amb valor n=1, per tant sensor 1
00102         
00103         //pc.enviaString(p);
00104     
00105         if (a >= maximumRange || a <= minimumRange){
00106         // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error.
00107         pc.enviaString("Sensor 1 fora de Rang \n");
00108         }
00109         else {
00110         // Enviar la distancia si està dins el rang de 0 a 4m
00111         snprintf(F, 100, "Sensor 1: Distancia: %f mm\n",a);
00112         pc.enviaString(F);
00113         }  
00114     }
00115     
00116     pc.enviaString("V000FM2");
00117     pc.enviaString("V000FM3");
00118  
00119     pc.enviaString("V050FM1");
00120     pc.enviaString("V050FM3");
00121     //MARCEL: PARA MOTORS
00122     //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 120º
00123 
00124     while ((b>100)&&(bt.getMode()==3)){
00125         b=SensorUltrasons(2); //Crida la funció amb valor n=2, per tant sensor 2
00126     
00127         if (b >= maximumRange || b <= minimumRange){
00128         // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error.
00129         pc.enviaString("Sensor 2 fora de Rang \n");
00130         }
00131         else {
00132         // Enviar la distancia si està dins el rang de 0 a 4m
00133         snprintf(F, 100, "Sensor 2: Distancia: %f mm\n",b);
00134         pc.enviaString(F);
00135         }
00136     }
00137     
00138     pc.enviaString("V000FM1");
00139     pc.enviaString("V000FM3");
00140     
00141     pc.enviaString("V050FM1");
00142     pc.enviaString("V050FM2");
00143     //MARCEL: PARA MOTORS
00144     //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 240º
00145 
00146     while ((c>100)&&(bt.getMode()==3)){
00147         c=SensorUltrasons(3); //Crida la funció amb valor n=3, per tant sensor 3
00148     
00149         if (c >= maximumRange || c <= minimumRange){
00150         // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error.
00151         pc.enviaString("Sensor 3 fora de Rang \n");
00152         }
00153         else {
00154         // Enviar la distancia si està dins el rang de 0 a 4m
00155         snprintf(F, 100, "Sensor 3: Distancia: %f mm\n",c);
00156         pc.enviaString(F);
00157         }
00158     }
00159     
00160     pc.enviaString("V000FM1");
00161     pc.enviaString("V000FM2");
00162     //MARCEL: PARA MOTORS    
00163     
00164 }
00165     
00166 
00167 
00168 
00169 
00170 //Aquest serà el meu fil, que va llegint cada 300ms del HC-05 
00171 void llegir_thread(void const *args) {
00172     while (true) {
00173         //Intenta llegir un string, i si el llegeix, el tracta
00174         if(bt.llegirString())
00175             bt.tractaString();      
00176         //Comprova la connexió (mira si ha rebut alguna cosa)
00177         bt.comprovaConnexio();
00178         //Temps del Thread que està esperant
00179         Thread::wait(100);
00180         
00181     }
00182 }
00183 
00184 
00185 //Aquest serà el fil de l'Eva, que va llegint els sensors d'ultrasons 
00186 void llegir_ultrathread(void const *args) {
00187     while (true) {
00188         S1=SensorUltrasons(1);
00189         S2=SensorUltrasons(2);
00190         S3=SensorUltrasons(3);
00191         
00192         //Temps del Thread que està esperant
00193         Thread::wait(100);
00194         
00195     }
00196 }
00197 
00198 //Aquest serà el programa principal
00199 int main() {
00200     //osThreadSetPriority(osThreadGetId(),osPriorityRealtime);
00201     Thread thread(llegir_thread);       //Inicia el fil llegir_thread
00202     thread.set_priority(osPriorityLow);
00203     
00204     //Thread thread2(llegir_ultrathread); //Inicia el fil dels ultrasons
00205     //char m[128];                        //Per visualitzar el mode i el timer, no es necessari
00206     while (true) {
00207         //pc.enviaString(bt.strLlegit);   //Per veure l'ultim string llegit, no es necessari
00208         bt.calculaMotors(S1, S2, S3); //Calcula com s'han de moure els motors
00209         //snprintf(m, 128, "Mode: %d | timer: %dms", bt.getMode(), bt.getTimer()); //Per visualitzar el mode i el timer, no es necessari
00210         //pc.enviaString(m);              //Per visualitzar el mode i el timer, no es necessari
00211         //pc.enviaString(bt.strPos);      //Per visualitzar la posicio on ha d'anar el robot, no es necessari
00212         if((bt.getMode() == 0) || (bt.getMode() == 1) || (bt.getMode() == 2)) {
00213             pc.enviaString(bt.strM1);       //String motor1 que s'envia a l'arduino d'en Marcel
00214             pc.enviaString(bt.strM2);       //String motor2 que s'envia a l'arduino d'en Marcel
00215             pc.enviaString(bt.strM3);       //String motor3 que s'envia a l'arduino d'en Marcel
00216         }
00217         else if (bt.getMode() == 3) {
00218             ModeAutomatic();
00219         }
00220         else if (bt.getMode() == 4) {
00221             ;//Mode mando (programa Marc)
00222         }
00223         else {
00224             pc.enviaString("V000FM1");       //String motor1 que s'envia a l'arduino d'en Marcel
00225             pc.enviaString("V000FM2");       //String motor2 que s'envia a l'arduino d'en Marcel
00226             pc.enviaString("V000FM3");       //String motor3 que s'envia a l'arduino d'en Marcel
00227         }
00228         Thread::wait(500);              //Temps del Thread que està esperant
00229     }
00230 }