Diff: Hc05.cpp

Revision:

2:298c9cc5af48

Parent:

1:cd73654d13ed

Child:

3:c594d33c7523

--- a/Hc05.cpp	Mon Dec 21 17:21:34 2015 +0000
+++ b/Hc05.cpp	Mon Dec 21 19:04:55 2015 +0000
@@ -3,13 +3,16 @@
 #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.0;
+            x = y = m1 = m2 = m3 = 0;
+            tractaString();
             };
             
         Hc05::~Hc05(){
@@ -23,6 +26,16 @@
             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){
@@ -76,6 +89,22 @@
             }
             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;
+            calculaMotors();
+            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);
         };
         
         int Hc05::getMode(){return mode;};
@@ -101,29 +130,68 @@
         
         //Calculs motors
         void Hc05::calculaMotors(){
-            x = cos(ang*pi/180);
-            y = sin(ang*pi/180);
-            m1 = -y;
-            m3 = x*0.8660254038+y*0.5;
-            m2 = x*(-0.8660254038)+y*0.5;
+            if(mode==1){
+                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;
+            }
         };
         
-        float getX(){
+        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 getY(){
+        float Hc05::getY(){
             return y;    
         };
         
-        float getM1(){
+        float Hc05::getM1(){
             return m1;    
         };
         
-        float getM2(){
+        float Hc05::getM2(){
             return m2;    
         };
         
-        float getM3(){
+        float Hc05::getM3(){
             return m3;    
         };