Skip to content
Snippets Groups Projects
Commit 90942edf authored by Ricardo Andres Ercoli Auersperg's avatar Ricardo Andres Ercoli Auersperg
Browse files

Update transformation matrices to match with refs

parent 6aefad58
No related branches found
No related tags found
No related merge requests found
...@@ -15,38 +15,38 @@ static void callback_enc_func(int i_callback, int8_t dir, uint32_t counter) { ...@@ -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 */ /*This function transforms speed from the robot frame to each wheel frame */
static void getW(sF3dVector_t* v, float phi){ static void getW(sF3dVector_t* v, float phi){
float pi_3 = PI/3; float dosPi_3 = 2*PI/3;
sF3dMatrix_t m; sF3dMatrix_t m;
m.x1 = -sin(phi); m.x1 = -sin(phi);
m.y1 = -cos(phi); m.y1 = cos(phi);
m.z1 = -ROBOT_RADIUS; m.z1 = ROBOT_RADIUS;
m.x2 = -sin(pi_3 - phi); m.x2 = -sin(phi + dosPi_3);
m.y2 = cos(pi_3 - phi); m.y2 = cos(phi + dosPi_3);
m.z2 = -ROBOT_RADIUS; m.z2 = ROBOT_RADIUS;
m.x3 = sin(pi_3 + phi); m.x3 = -sin(phi - dosPi_3);
m.y3 = cos(pi_3 + phi); m.y3 = cos(phi - dosPi_3);
m.z3 = -ROBOT_RADIUS; m.z3 = ROBOT_RADIUS;
vector_mul(m, v); vector_mul(m, v);
} }
static void getInverseW(sF3dVector_t* v, float phi){ 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 dostercios = 2.0/3;
float robot_r_3 = 1.0/(ROBOT_RADIUS*3); float robot_r_3 = 1.0/(ROBOT_RADIUS*3);
sF3dMatrix_t m; sF3dMatrix_t m;
m.x1 = -sin(phi)*dostercios; m.x1 = -sin(phi)*dostercios;
m.y1 = -cos(pi_6 + phi)*dostercios; m.y1 = -sin(phi + dosPi_3)*dostercios;
m.z1 = cos(phi - pi_6)*dostercios; m.z1 = -sin(phi - dosPi_3)*dostercios;
m.x2 = -cos(phi)*dostercios; m.x2 = cos(phi)*dostercios;
m.y2 = sin(pi_6 + phi)*dostercios; m.y2 = cos(phi + dosPi_3)*dostercios;
m.z2 = -sin(phi - pi_6)*dostercios; m.z2 = cos(phi + dosPi_3)*dostercios;
m.x3 = -robot_r_3; m.x3 = robot_r_3;
m.y3 = -robot_r_3; m.y3 = robot_r_3;
m.z3 = -robot_r_3; m.z3 = robot_r_3;
vector_mul(m, v); vector_mul(m, v);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment