Diff: Hc05.cpp

Revision:

1:cd73654d13ed

Parent:

0:acddf8d6d0cb

Child:

2:298c9cc5af48

--- a/Hc05.cpp	Fri Dec 18 17:13:25 2015 +0000
+++ b/Hc05.cpp	Mon Dec 21 17:21:34 2015 +0000
@@ -1,12 +1,15 @@
 #include "Hc05.h"
 #include "mbed.h"
+#include "math.h"
 
+#define pi 3.14159265
 
         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;
             };
             
         Hc05::~Hc05(){
@@ -96,3 +99,31 @@
             return  timer.read_ms();   
         };
         
+        //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;
+        };
+        
+        float getX(){
+            return x;    
+        };
+        
+        float getY(){
+            return y;    
+        };
+        
+        float getM1(){
+            return m1;    
+        };
+        
+        float getM2(){
+            return m2;    
+        };
+        
+        float getM3(){
+            return m3;    
+        };