File content as of revision 4:de2112ca0735:

#include "Hc05.h"
#include "mbed.h"
#include "math.h"

#define pi 3.14159265
#define longString 128


        Hc05::Hc05(PinName tx, PinName rx) : RawSerial(tx, rx){
            baud(230400);
            mode = vel = ang = 0;
            iniString();
            timer.start();
            x = y = m1 = m2 = m3 = 0;
            tractaString();
            };
            
        Hc05::~Hc05(){
        };
        
        void Hc05::iniString(){
            strLlegit[0] = 'S';
            strLlegit[1] = 't';
            strLlegit[2] = 'r';
            strLlegit[3] = 'i';
            strLlegit[4] = 'n';
            strLlegit[5] = 'g';
            strLlegit[6] = '\0';
            strM1[0] = strM2[0] = strM3[0] = 'V';
            strM1[1] = strM2[1] = strM3[1] = '0';
            strM1[2] = strM2[2] = strM3[2] = '0';
            strM1[3] = strM2[3] = strM3[3] = '0';
            strM1[4] = strM2[4] = strM3[4] = 'F';
            strM1[5] = strM2[5] = strM3[5] = 'M';
            strM1[6] = '1';
            strM2[6] = '2';
            strM3[6] = '3';
            strM1[7] = strM2[7] = strM3[7] = strPos[0] = '\0';
        };
            
        void Hc05::enviaString(char* str){
            int i = 0;
            while((i < longString) && (str[i] != '\0')) {
                putc(str[i]);
                i++;
            }
            putc(13);
        };
        
        
        bool Hc05::llegirString(){
            if(readable()){
                char c = getc();
                int i = 0;
                while((i < longString) && (c != 13)) {
                    if(c!='@'){
                        strLlegit[i] = c;
                        i++;
                    }
                    timer.reset();
                    c = getc();
                }
                strLlegit[i] = '\0';
                return true;
                
            }
            return false;
        };
        
        void Hc05::tractaString(){
            if ((strLlegit[0] == 'a') && (strLlegit[1] == 'v') && (strLlegit[2] == 'a') && (strLlegit[3] == 'n'))
                mode = 1;
            else if ((strLlegit[0] == 'r') && (strLlegit[1] == 'o') && (strLlegit[2] == 't') && (strLlegit[3] == 'a'))
                mode = 2;
            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'))
                mode = 3;
            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'))
                mode = 4;
            else if ((strLlegit[0] == 's') && (strLlegit[1] == 't') && (strLlegit[2] == 'o') && (strLlegit[3] == 'p')){
                mode = 0;
                vel = 0;
                ang = 0;
            }
            else ;
            if ((mode == 1) || (mode==2)){
                char svel[5], sang[5];
                for(int i = 0; i<=4;i++){
                    sang[i] = strLlegit[i+5];
                    svel[i] = strLlegit[i+10];    
                }
                sang[4] = svel[4] = '\0';
                vel = atoi(svel);
                ang = atoi(sang);
            }
            //Calcul dels motors
            //calculaMotors();
            
        };
        
        int Hc05::getMode(){return mode;};
        int Hc05::getVel(){return vel;};
        int Hc05::getAng(){return ang;};
        void Hc05::getStringLlegit(char* str){
            int i = 0;
            while((i < longString) && (strLlegit[i] != '\0')) {
                str[i] = strLlegit[i];
                i++;
            }
            str[i] = '\0';
        };
        
        void Hc05::comprovaConnexio(){
            if(getTimer() > 300)
                mode = -1;
        };
        
        int Hc05::getTimer(){
            return  timer.read_ms();   
        };
        
        //Calculs motors
        void Hc05::calculaMotors(){
            if(mode==1){ //Falta afegir una condició perquè no deixi avançar en determinada direcció si detecta els ultrasons if((mode==1) && ((ultraso1 > 10) && (ang<-45) && (ang>45)) && ((ultraso2 > 10) && (ang<90) && (ang>180)) && ((ultraso3 > 10) && (ang<-180) && (ang>-90))))
                x = cos(ang*pi/180);
                y = sin(ang*pi/180);
                m1 = (-y)*vel;
                m3 = (x*0.8660254038+y*0.5)*vel;
                m2 = (x*(-0.8660254038)+y*0.5)*vel;
            }
            else if ((mode==2) && (ang!=0)){
                x = y = 0;
                if(ang>0)
                    m1 = m2 = m3 = vel;
                else
                    m1 = m2 = m3 = -(vel);
            }
            else {
                m1 = m2 = m3 = x = y = 0;
            }
            //Omple els strings per enviar a l'arduino
            char cDireccio[3];
            if (m1<0)
                cDireccio[0]='B';
            else cDireccio[0]='F';
            if (m2<0)
                cDireccio[1]='B';
            else cDireccio[1]='F';
            if (m3<0)
                cDireccio[2]='B';
            else cDireccio[2]='F';
            
            snprintf(strM1, longString, "V%.3i%cM1", abs(m1), cDireccio[0]);
            snprintf(strM2, longString, "V%.3i%cM2", abs(m2), cDireccio[1]);
            snprintf(strM3, longString, "V%.3i%cM3", abs(m3), cDireccio[2]);
            snprintf(strPos, longString, "X=%g   Y=%g", x, y);
        };
        
        void Hc05::getStringM1(char* str){
            int i = 0;
            while((i < longString) && (strM1[i] != '\0')) {
                str[i] = strM1[i];
                i++;
            }
            str[i] = '\0';
        };
        
        void Hc05::getStringM2(char* str){
            int i = 0;
            while((i < longString) && (strM2[i] != '\0')) {
                str[i] = strM2[i];
                i++;
            }
            str[i] = '\0';
        };
        
        void Hc05::getStringM3(char* str){
            int i = 0;
            while((i < longString) && (strM3[i] != '\0')) {
                str[i] = strM3[i];
                i++;
            }
            str[i] = '\0';
        };
        
        float Hc05::getX(){
            return x;    
        };
        
        float Hc05::getY(){
            return y;    
        };
        
        float Hc05::getM1(){
            return m1;    
        };
        
        float Hc05::getM2(){
            return m2;    
        };
        
        float Hc05::getM3(){
            return m3;    
        };