Hc05.cpp

00001 #include "Hc05.h"
00002 #include "mbed.h"
00003 #include "math.h"
00004 
00005 #define pi 3.14159265 //Valor de pi
00006 #define longString 128 //Longitud màxima dels strings
00007 #define minUltrasons 200 //Valor mínim dels ultrasons
00008 
00009 
00010         Hc05::Hc05(PinName tx, PinName rx) : RawSerial(tx, rx){
00011             baud(230400);
00012             mode = vel = ang = 0;
00013             iniString();
00014             timer.start();
00015             x = y = m1 = m2 = m3 = 0;
00016             tractaString();
00017             };
00018             
00019         Hc05::~Hc05(){
00020         };
00021         
00022         void Hc05::iniString(){
00023             strLlegit[0] = 'S';
00024             strLlegit[1] = 't';
00025             strLlegit[2] = 'r';
00026             strLlegit[3] = 'i';
00027             strLlegit[4] = 'n';
00028             strLlegit[5] = 'g';
00029             strLlegit[6] = '\0';
00030             strM1[0] = strM2[0] = strM3[0] = 'V';
00031             strM1[1] = strM2[1] = strM3[1] = '0';
00032             strM1[2] = strM2[2] = strM3[2] = '0';
00033             strM1[3] = strM2[3] = strM3[3] = '0';
00034             strM1[4] = strM2[4] = strM3[4] = 'F';
00035             strM1[5] = strM2[5] = strM3[5] = 'M';
00036             strM1[6] = '1';
00037             strM2[6] = '2';
00038             strM3[6] = '3';
00039             strM1[7] = strM2[7] = strM3[7] = strPos[0] = '\0';
00040         };
00041             
00042         void Hc05::enviaString(char* str){
00043             int i = 0;
00044             while((i < longString) && (str[i] != '\0')) {
00045                 putc(str[i]);
00046                 i++;
00047             }
00048             putc(13);
00049         };
00050         
00051         
00052         bool Hc05::llegirString(){
00053             if(readable()){
00054                 char c = getc();
00055                 int i = 0;
00056                 while((i < longString) && (c != 13)) {
00057                     if(c!='@'){
00058                         strLlegit[i] = c;
00059                         i++;
00060                     }
00061                     timer.reset();
00062                     c = getc();
00063                 }
00064                 strLlegit[i] = '\0';
00065                 return true;
00066                 
00067             }
00068             return false;
00069         };
00070         
00071         void Hc05::tractaString(){
00072             if ((strLlegit[0] == 'a') && (strLlegit[1] == 'v') && (strLlegit[2] == 'a') && (strLlegit[3] == 'n'))
00073                 mode = 1;
00074             else if ((strLlegit[0] == 'r') && (strLlegit[1] == 'o') && (strLlegit[2] == 't') && (strLlegit[3] == 'a'))
00075                 mode = 2;
00076             else if ((strLlegit[0] == 'm') && (strLlegit[1] == 'o') && (strLlegit[2] == 'd') && (strLlegit[3] == 'e') && (strLlegit[5] == 'a') && (strLlegit[6] == 'u') && (strLlegit[7] == 't') && (strLlegit[8] == 'o'))
00077                 mode = 3;
00078             else if ((strLlegit[0] == 'm') && (strLlegit[1] == 'o') && (strLlegit[2] == 'd') && (strLlegit[3] == 'e') && (strLlegit[5] == 'm') && (strLlegit[6] == 'a') && (strLlegit[7] == 'n') && (strLlegit[8] == 'u'))
00079                 mode = 4;
00080             else if ((strLlegit[0] == 's') && (strLlegit[1] == 't') && (strLlegit[2] == 'o') && (strLlegit[3] == 'p')){
00081                 mode = 0;
00082                 vel = 0;
00083                 ang = 0;
00084             }
00085             else ;
00086             if ((mode == 1) || (mode==2)){
00087                 char svel[5], sang[5];
00088                 for(int i = 0; i<=4;i++){
00089                     sang[i] = strLlegit[i+5];
00090                     svel[i] = strLlegit[i+10];    
00091                 }
00092                 sang[4] = svel[4] = '\0';
00093                 vel = atoi(svel);
00094                 ang = atoi(sang);
00095             }
00096             //Calcul dels motors
00097             //calculaMotors();
00098             
00099         };
00100         
00101         int Hc05::getMode(){return mode;};
00102         int Hc05::getVel(){return vel;};
00103         int Hc05::getAng(){return ang;};
00104         void Hc05::getStringLlegit(char* str){
00105             int i = 0;
00106             while((i < longString) && (strLlegit[i] != '\0')) {
00107                 str[i] = strLlegit[i];
00108                 i++;
00109             }
00110             str[i] = '\0';
00111         };
00112         
00113         void Hc05::comprovaConnexio(){
00114             if(getTimer() > 300)
00115                 mode = -1;
00116         };
00117         
00118         int Hc05::getTimer(){
00119             return  timer.read_ms();   
00120         };
00121         
00122         //Calculs motors
00123         void Hc05::calculaMotors(float S1, float S2, float S3){
00124             if(mode==1) { // Comprova que no hi hagin obstacles en la direcció en que volem avançar
00125                 if ( (!((ang>=-45) && (ang<=45) && (S1<=minUltrasons))) && (!((ang>=90) && (ang<=180) && (S2<=minUltrasons))) && (!((ang>=-90) && (ang<=-180) && (S3<=minUltrasons))) ) {
00126                     x = cos(ang*pi/180);
00127                     y = sin(ang*pi/180);
00128                     m1 = (-y)*vel;
00129                     m3 = (x*0.8660254038+y*0.5)*vel;
00130                     m2 = (x*(-0.8660254038)+y*0.5)*vel;
00131                 }
00132                 else {m1 = m2 = m3 = x = y = 0;}
00133             }
00134             else if ((mode==2) && (ang!=0)){
00135                 x = y = 0;
00136                 if(ang>0)
00137                     m1 = m2 = m3 = vel;
00138                 else
00139                     m1 = m2 = m3 = -(vel);
00140             }
00141             else {
00142                 m1 = m2 = m3 = x = y = 0;
00143             }
00144             //Omple els strings per enviar a l'arduino
00145             char cDireccio[3];
00146             if (m1<0)
00147                 cDireccio[0]='B';
00148             else cDireccio[0]='F';
00149             if (m2<0)
00150                 cDireccio[1]='B';
00151             else cDireccio[1]='F';
00152             if (m3<0)
00153                 cDireccio[2]='B';
00154             else cDireccio[2]='F';
00155             
00156             snprintf(strM1, longString, "V%.3i%cM1", abs(m1), cDireccio[0]);
00157             snprintf(strM2, longString, "V%.3i%cM2", abs(m2), cDireccio[1]);
00158             snprintf(strM3, longString, "V%.3i%cM3", abs(m3), cDireccio[2]);
00159             snprintf(strPos, longString, "X=%g   Y=%g", x, y);
00160         };
00161         
00162         void Hc05::getStringM1(char* str){
00163             int i = 0;
00164             while((i < longString) && (strM1[i] != '\0')) {
00165                 str[i] = strM1[i];
00166                 i++;
00167             }
00168             str[i] = '\0';
00169         };
00170         
00171         void Hc05::getStringM2(char* str){
00172             int i = 0;
00173             while((i < longString) && (strM2[i] != '\0')) {
00174                 str[i] = strM2[i];
00175                 i++;
00176             }
00177             str[i] = '\0';
00178         };
00179         
00180         void Hc05::getStringM3(char* str){
00181             int i = 0;
00182             while((i < longString) && (strM3[i] != '\0')) {
00183                 str[i] = strM3[i];
00184                 i++;
00185             }
00186             str[i] = '\0';
00187         };
00188         
00189         float Hc05::getX(){
00190             return x;    
00191         };
00192         
00193         float Hc05::getY(){
00194             return y;    
00195         };
00196         
00197         float Hc05::getM1(){
00198             return m1;    
00199         };
00200         
00201         float Hc05::getM2(){
00202             return m2;    
00203         };
00204         
00205         float Hc05::getM3(){
00206             return m3;    
00207         };