From 90942edfd5694ac82015a9ed486ba8ef2b659680 Mon Sep 17 00:00:00 2001
From: RicardoEA <ricardo.ercoli@fing.edu.uy>
Date: Sun, 23 Jul 2023 17:28:03 -0300
Subject: [PATCH] Update transformation matrices to match with refs
---
Robotito/src/control/control.c | 38 +++++++++++++++++-----------------
1 file changed, 19 insertions(+), 19 deletions(-)
diff --git a/Robotito/src/control/control.c b/Robotito/src/control/control.c
index 43bc957..71e1ddd 100644
--- a/Robotito/src/control/control.c
+++ b/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);
--
GitLab