diff --git a/Robotito/src/control/control.c b/Robotito/src/control/control.c index 5aba34fb5b090c6d0bd4b9f4b9e7e2c57a1d40b0..43bc95735b0a9af95d7e2592b073370d296395ce 100644 --- a/Robotito/src/control/control.c +++ b/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;