Fix transformations between robot and wheels frames

Robotito/src/control/control.c

@@ -12,20 +12,21 @@ static void callback_enc_func(int i_callback, int8_t dir, uint32_t counter) {
     motors[i_callback].odom_counter+=dir;
 }
 
+/*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;
 
     sF3dMatrix_t m;
     m.x1 = -sin(phi);
-    m.y1 = cos(phi);
-    m.z1 = ROBOT_RADIUS;
+    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.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.y3 = cos(pi_3 + phi);
+    m.z3 = -ROBOT_RADIUS;
 
 	vector_mul(m, v);
 }
@@ -40,12 +41,12 @@ static void getInverseW(sF3dVector_t* v, float phi){
     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.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;
 
 	vector_mul(m, v);
 
@@ -97,7 +98,7 @@ static void callback_odometry(){
     v->y = tics_motores[1] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
     v->z = tics_motores[2] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
 
-    getInverseW(v, o->phi);
+    getInverseW(v, 0);
 
     o->x        += v->x * OMNI_ODOM_CTRL_TIMER;
     o->x_dot     = v->x;
@@ -211,8 +212,9 @@ esp_err_t omni_drive(float x_dot ,float y_dot ,float w_dot ,float phi){
     v->y = y_dot;
     v->z = w_dot;
     
-    /* Here we transform speed from the robot frame in m/s to motor speeds in count/s */ 
     getW(v, phi);
+    
+    /* Here we transform each motor speed from m/s to motor speeds in count/s */ 
     motors[0].target_v = v->x * M_PER_SEC_TO_COUNTS_PER_SEC;
     motors[1].target_v = v->y * M_PER_SEC_TO_COUNTS_PER_SEC;
     motors[2].target_v = v->z * M_PER_SEC_TO_COUNTS_PER_SEC;