Update transformation matrices to match with refs

Robotito/src/control/control.c

@@ -15,38 +15,38 @@ static void callback_enc_func(int i_callback, int8_t dir, uint32_t counter) {
 /*This function transforms speed from the robot frame to each wheel frame */
 static void getW(sF3dVector_t* v, float phi){
 
-    float pi_3 = PI/3;
+    float dosPi_3 = 2*PI/3;
 
     sF3dMatrix_t m;
     m.x1 = -sin(phi);
-    m.y1 = -cos(phi);
-    m.z1 = -ROBOT_RADIUS;
-	m.x2 = -sin(pi_3 - phi);
-    m.y2 = cos(pi_3 - phi);
-    m.z2 = -ROBOT_RADIUS;
-	m.x3 = sin(pi_3 + phi);
-    m.y3 = cos(pi_3 + phi);
-    m.z3 = -ROBOT_RADIUS;
+    m.y1 = cos(phi);
+    m.z1 = ROBOT_RADIUS;
+	m.x2 = -sin(phi + dosPi_3);
+    m.y2 = cos(phi + dosPi_3);
+    m.z2 = ROBOT_RADIUS;
+	m.x3 = -sin(phi - dosPi_3);
+    m.y3 = cos(phi - dosPi_3);
+    m.z3 = ROBOT_RADIUS;
 
 	vector_mul(m, v);
 }
 
 static void getInverseW(sF3dVector_t* v, float phi){
 
-    float pi_6 = PI/6;
+    float dosPi_3 = 2*PI/3;
     float dostercios = 2.0/3;
     float robot_r_3 = 1.0/(ROBOT_RADIUS*3);
 
 	sF3dMatrix_t m;
-    m.x1 =  -sin(phi)*dostercios;
-    m.y1 = -cos(pi_6 + phi)*dostercios;
-    m.z1 = cos(phi - pi_6)*dostercios;
-    m.x2 = -cos(phi)*dostercios;
-    m.y2 = sin(pi_6 + phi)*dostercios;
-    m.z2 = -sin(phi -  pi_6)*dostercios;
-    m.x3 = -robot_r_3;
-    m.y3 = -robot_r_3;
-    m.z3 = -robot_r_3;
+    m.x1 = -sin(phi)*dostercios;
+    m.y1 = -sin(phi + dosPi_3)*dostercios;
+    m.z1 = -sin(phi - dosPi_3)*dostercios;
+    m.x2 = cos(phi)*dostercios;
+    m.y2 = cos(phi + dosPi_3)*dostercios;
+    m.z2 = cos(phi + dosPi_3)*dostercios;
+    m.x3 = robot_r_3;
+    m.y3 = robot_r_3;
+    m.z3 = robot_r_3;
 
 	vector_mul(m, v);