From a1657c1b0b3596037b10427948fc386425475bb5 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Fri, 3 Jan 2025 14:39:03 -0600
Subject: [PATCH 01/13] add hpp to chassis movement and start left and rigth
 method

---
 inc/chassisMove.cpp | 34 ++++++++++++++++++++++++++++++++++
 inc/chassisMove.hpp | 41 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 75 insertions(+)
 create mode 100644 inc/chassisMove.cpp
 create mode 100644 inc/chassisMove.hpp

diff --git a/inc/chassisMove.cpp b/inc/chassisMove.cpp
new file mode 100644
index 0000000..14c0b50
--- /dev/null
+++ b/inc/chassisMove.cpp
@@ -0,0 +1,34 @@
+#include "chassisMove.hpp"
+
+chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
+                    IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, int16_t speed = 100);
+
+void chassisMove::moveFront() {
+
+}
+
+
+void chassisMove::moveBack() {
+
+}
+
+void chassisMove::moveRight() {
+    leftFrontMotor->actuate(speed);   
+    rightFrontMotor->actuate(-speed); 
+    leftBackMotor->actuate(-speed);
+    rightBackMotor->actuate(speed);
+}
+
+void chassisMove::moveLeft() {
+    leftMotor->actuate(-speed);  
+    rightMotor->actuate(speed); 
+    leftBackMotor->actuate(speed); 
+    rightBackMotor->actuate(-speed);
+}
+
+void chassisMove::stop() {
+    leftMotor->stop(0); 
+    rightMotor->stop(0); 
+    leftBackMotor->stop(0); 
+    rightBackMotor->stop(0); 
+}
diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
new file mode 100644
index 0000000..9177de1
--- /dev/null
+++ b/inc/chassisMove.hpp
@@ -0,0 +1,41 @@
+/*
+ * ControllerCAN.hpp
+ *
+ *  Created on: April 12, 2024
+ *      Author: @JorgePerC
+ * 
+ * For mor information about how to use this driver: 
+ * um2217-description-of-stm32h7-hal-and-lowlayer-drivers-stmicroelectronics-1
+ */
+
+#ifndef chassisMove_HPP
+#define chassisMove_HPP
+
+// ===== Includes =====
+#include "IntfMotor.hpp" 
+#include "stm32f4xx_hal.h"
+
+
+class chassisMove {
+    private:
+        IntfMotor* leftFrontMotor;   
+        IntfMotor* rightFrontMotor;  
+        IntfMotor* leftBackMotor;   
+        IntfMotor* rightBackMotor;  
+
+        int16_t speed;  
+
+    public:
+        chassisMove();
+        chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
+                    IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, int16_t speed = 100);
+
+        void moveFront();
+        void moveBack();
+        void moveRight();
+        void moveLeft();
+
+        void stop();
+};
+
+#endif /*chassisMove_HPP*/
\ No newline at end of file

From e10bb2815f07e91189a2a30754f4a2b129afe373 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Fri, 3 Jan 2025 14:46:11 -0600
Subject: [PATCH 02/13] move cpp to src

---
 {inc => src}/chassisMove.cpp | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename {inc => src}/chassisMove.cpp (100%)

diff --git a/inc/chassisMove.cpp b/src/chassisMove.cpp
similarity index 100%
rename from inc/chassisMove.cpp
rename to src/chassisMove.cpp

From 7ca38f7c1b652b42046748b07ccbd41dd9650b9e Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Mon, 6 Jan 2025 14:42:57 -0600
Subject: [PATCH 03/13] hacer el movimiento en un metodo, tomar en cuenta el
 control remoto

---
 inc/chassisMove.hpp | 55 +++++++++++++++++++++------------------------
 src/chassisMove.cpp | 51 +++++++++++++++++++++++++++--------------
 2 files changed, 59 insertions(+), 47 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 9177de1..2f2f290 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -1,41 +1,36 @@
-/*
- * ControllerCAN.hpp
- *
- *  Created on: April 12, 2024
- *      Author: @JorgePerC
- * 
- * For mor information about how to use this driver: 
- * um2217-description-of-stm32h7-hal-and-lowlayer-drivers-stmicroelectronics-1
- */
+#ifndef CHASSIS_MOVE_HPP
+#define CHASSIS_MOVE_HPP
 
-#ifndef chassisMove_HPP
-#define chassisMove_HPP
+#include "IntfMotor.hpp"
 
-// ===== Includes =====
-#include "IntfMotor.hpp" 
-#include "stm32f4xx_hal.h"
+#define MOTOR_SPEED_TO_CHASSIS_SPEED_WZ
+#define MOTOR_DISTANCE_TO_CENTER
 
 
 class chassisMove {
-    private:
-        IntfMotor* leftFrontMotor;   
-        IntfMotor* rightFrontMotor;  
-        IntfMotor* leftBackMotor;   
-        IntfMotor* rightBackMotor;  
+private:
+    IntfMotor* leftFrontMotor;
+    IntfMotor* rightFrontMotor;
+    IntfMotor* leftBackMotor;
+    IntfMotor* rightBackMotor;
 
-        int16_t speed;  
+    float maxMotorSpeed; 
 
-    public:
-        chassisMove();
-        chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
-                    IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, int16_t speed = 100);
+    float normalizeSpeed(float speed);
 
-        void moveFront();
-        void moveBack();
-        void moveRight();
-        void moveLeft();
+public:
+    // Constructor
+    chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
+                IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, float maxMotorSpeed = 100.0f);
 
-        void stop();
+    // Traducir joystick a movimiento de motores
+    void joystickToMotors(float x, float y, float w);
+
+    // Método para actualizar las velocidades basándose en el joystick
+    void update();
+
+    //Metodo para detener motores 
+    void stop();
 };
 
-#endif /*chassisMove_HPP*/
\ No newline at end of file
+#endif /* CHASSIS_MOVE_HPP */
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 14c0b50..d9b7c7f 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -1,29 +1,46 @@
 #include "chassisMove.hpp"
 
+// Constructor
 chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
-                    IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, int16_t speed = 100);
-
-void chassisMove::moveFront() {
-
+                         IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, float maxMotorSpeed)
+    : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
+      leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor), maxMotorSpeed(maxMotorSpeed) {}
+
+// Normalizar la velocidad para que esté dentro del rango permitido
+float chassisMove::normalizeSpeed(float speed) {
+    if (speed > maxMotorSpeed) return maxMotorSpeed;
+    if (speed < -maxMotorSpeed) return -maxMotorSpeed;
+    return speed;
 }
 
-
-void chassisMove::moveBack() {
-
+// Traducir joystick a velocidades de las ruedas
+void chassisMove::joystickToMotors(float x, float y, float w) {
+    float wheel_speed[4];
+
+    // Calcular velocidades de las ruedas mecanum
+    wheel_speed[0] = -x - y + w; // Delantera izquierda
+    wheel_speed[1] = x - y + w;  // Delantera derecha
+    wheel_speed[2] = x + y + w;  // Trasera derecha
+    wheel_speed[3] = -x + y + w; // Trasera izquierda
+
+    // Normalizar y actuar las velocidades en los motores
+    leftFrontMotor->actuate(normalizeSpeed(wheel_speed[0]));
+    rightFrontMotor->actuate(normalizeSpeed(wheel_speed[1]));
+    rightBackMotor->actuate(normalizeSpeed(wheel_speed[2]));
+    leftBackMotor->actuate(normalizeSpeed(wheel_speed[3]));
 }
 
-void chassisMove::moveRight() {
-    leftFrontMotor->actuate(speed);   
-    rightFrontMotor->actuate(-speed); 
-    leftBackMotor->actuate(-speed);
-    rightBackMotor->actuate(speed);
+void getJoystickRotation(){
+    w = (-wheel_speed[0] - wheel_speed[1] - wheel_speed[2] - wheel_speed[3]) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;
 }
 
-void chassisMove::moveLeft() {
-    leftMotor->actuate(-speed);  
-    rightMotor->actuate(speed); 
-    leftBackMotor->actuate(speed); 
-    rightBackMotor->actuate(-speed);
+// Actualizar el movimiento basado en joystick
+void chassisMove::update() {
+    float x = getJoystickX();
+    float y = getJoystickY();
+    float w = getJoystickRotation();
+
+    joystickToMotors(x, y, w);
 }
 
 void chassisMove::stop() {

From 3536f0d130e0f742d0ff295ee679b458a2f89dcb Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Mon, 6 Jan 2025 20:06:46 -0600
Subject: [PATCH 04/13] Velocidad del chassis  con vectores

---
 inc/chassisMove.hpp | 19 +++++--------
 src/chassisMove.cpp | 67 ++++++++++++++++++++-------------------------
 2 files changed, 37 insertions(+), 49 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 2f2f290..04e94e0 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -1,11 +1,11 @@
 #ifndef CHASSIS_MOVE_HPP
 #define CHASSIS_MOVE_HPP
 
-#include "IntfMotor.hpp"
-
-#define MOTOR_SPEED_TO_CHASSIS_SPEED_WZ
-#define MOTOR_DISTANCE_TO_CENTER
+#include <Eigen/Dense> 
+#include "IntfMotor.hpp" 
 
+#define CHASSIS_RADIUS 0.3f  // Radio del chasis (distancia del centro a una rueda) en metros
+#define MAX_MOTOR_SPEED 1.0f // Velocidad máxima del motor
 
 class chassisMove {
 private:
@@ -19,18 +19,13 @@ class chassisMove {
     float normalizeSpeed(float speed);
 
 public:
-    // Constructor
     chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
-                IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, float maxMotorSpeed = 100.0f);
+                IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
+                float maxMotorSpeed = MAX_MOTOR_SPEED);
 
-    // Traducir joystick a movimiento de motores
     void joystickToMotors(float x, float y, float w);
 
-    // Método para actualizar las velocidades basándose en el joystick
-    void update();
-
-    //Metodo para detener motores 
     void stop();
 };
 
-#endif /* CHASSIS_MOVE_HPP */
+#endif // CHASSIS_MOVE_HPP
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index d9b7c7f..94bfa70 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -1,51 +1,44 @@
 #include "chassisMove.hpp"
 
-// Constructor
-chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
-                         IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, float maxMotorSpeed)
-    : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
-      leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor), maxMotorSpeed(maxMotorSpeed) {}
-
-// Normalizar la velocidad para que esté dentro del rango permitido
 float chassisMove::normalizeSpeed(float speed) {
     if (speed > maxMotorSpeed) return maxMotorSpeed;
     if (speed < -maxMotorSpeed) return -maxMotorSpeed;
     return speed;
 }
 
-// Traducir joystick a velocidades de las ruedas
-void chassisMove::joystickToMotors(float x, float y, float w) {
-    float wheel_speed[4];
-
-    // Calcular velocidades de las ruedas mecanum
-    wheel_speed[0] = -x - y + w; // Delantera izquierda
-    wheel_speed[1] = x - y + w;  // Delantera derecha
-    wheel_speed[2] = x + y + w;  // Trasera derecha
-    wheel_speed[3] = -x + y + w; // Trasera izquierda
-
-    // Normalizar y actuar las velocidades en los motores
-    leftFrontMotor->actuate(normalizeSpeed(wheel_speed[0]));
-    rightFrontMotor->actuate(normalizeSpeed(wheel_speed[1]));
-    rightBackMotor->actuate(normalizeSpeed(wheel_speed[2]));
-    leftBackMotor->actuate(normalizeSpeed(wheel_speed[3]));
-}
-
-void getJoystickRotation(){
-    w = (-wheel_speed[0] - wheel_speed[1] - wheel_speed[2] - wheel_speed[3]) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;
-}
+chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
+                         IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
+                         float maxMotorSpeed)
+    : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
+      leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor),
+      maxMotorSpeed(maxMotorSpeed) {}
 
-// Actualizar el movimiento basado en joystick
-void chassisMove::update() {
-    float x = getJoystickX();
-    float y = getJoystickY();
-    float w = getJoystickRotation();
 
-    joystickToMotors(x, y, w);
+void chassisMove::joystickToMotors(float x, float y, float w) {
+    //v=M*u
+    // u
+    Eigen::Vector3f joystick_input(x, y, w);
+
+    // M
+    Eigen::MatrixXf control_matrix(4, 3);
+    control_matrix << -1, -1,  CHASSIS_RADIUS,  // Delantera izquierda
+                       1, -1,  CHASSIS_RADIUS,  // Delantera derecha
+                       1,  1,  CHASSIS_RADIUS,  // Trasera derecha
+                      -1,  1,  CHASSIS_RADIUS;  // Trasera izquierda
+
+    // v
+    Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
+    wheel_speed = wheel_speed.unaryExpr([this](float speed) { return normalizeSpeed(speed); });
+
+    leftFrontMotor->actuate(wheel_speed[0]);
+    rightFrontMotor->actuate(wheel_speed[1]);
+    rightBackMotor->actuate(wheel_speed[2]);
+    leftBackMotor->actuate(wheel_speed[3]);
 }
 
 void chassisMove::stop() {
-    leftMotor->stop(0); 
-    rightMotor->stop(0); 
-    leftBackMotor->stop(0); 
-    rightBackMotor->stop(0); 
+    leftFrontMotor->stop(0);
+    rightFrontMotor->stop(0);
+    leftBackMotor->stop(0);
+    rightBackMotor->stop(0);
 }

From 7c9b265cf9e4bfe71694c4da52ec8343cdf8313c Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Tue, 7 Jan 2025 15:55:42 -0600
Subject: [PATCH 05/13] =?UTF-8?q?Obtener=20torci=C3=B3n=20por=20medio=20de?=
 =?UTF-8?q?=20xy=20del=20joy2=20y=20documentar?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 inc/chassisMove.hpp | 27 +++++++++++++++++++++--
 src/chassisMove.cpp | 54 +++++++++++++++++++++++++++++++++++++--------
 2 files changed, 70 insertions(+), 11 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 04e94e0..e1452ad 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -1,3 +1,16 @@
+ /*
+ * chassisMove.hpp
+ *
+ *  Created on: Jan 02, 2025
+ *      Author: @sofiaariasv2002
+ *              @AnaValeria
+ * 
+ * For mor information:
+ * https://learning.oreilly.com/library/view/wheeled-mobile-robotics/9780128042380/B9780128042045000020_1.xhtml#s0070
+ * https://www.robomaster.com/en-US/products/components/general/M3508
+ *
+ */
+
 #ifndef CHASSIS_MOVE_HPP
 #define CHASSIS_MOVE_HPP
 
@@ -6,7 +19,16 @@
 
 #define CHASSIS_RADIUS 0.3f  // Radio del chasis (distancia del centro a una rueda) en metros
 #define MAX_MOTOR_SPEED 1.0f // Velocidad máxima del motor
-
+#define K_TWIST 1.0f         // Sensibilidad para torsión del chasis
+#define PI 3.14159265358979323846
+
+/**
+ * @brief Clase para controlar el movimiento de un chasis mecanum utilizando joysticks.
+ * 
+ * Esta clase permite controlar el movimiento de un robot con chasis mecanum. La clase proporciona 
+ * métodos para convertir las entradas de joystick en velocidades de motor, además de normalizar 
+ * las velocidades y calcular la torsión en base a las entradas de control.
+ */
 class chassisMove {
 private:
     IntfMotor* leftFrontMotor;
@@ -17,13 +39,14 @@ class chassisMove {
     float maxMotorSpeed; 
 
     float normalizeSpeed(float speed);
+    float normalizeW(float w);
 
 public:
     chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
                 IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
                 float maxMotorSpeed = MAX_MOTOR_SPEED);
 
-    void joystickToMotors(float x, float y, float w);
+    void joystickToMotors(float x1, float y1, float x2, float y2, float theta_robot);
 
     void stop();
 };
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 94bfa70..1ba8adf 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -1,11 +1,32 @@
 #include "chassisMove.hpp"
 
+/**
+ * @brief Normaliza la velocidad del motor para asegurarse de que no exceda la velocidad máxima.
+ * 
+ * @param speed Velocidad a normalizar.
+ * @return La velocidad normalizada.
+*/
 float chassisMove::normalizeSpeed(float speed) {
     if (speed > maxMotorSpeed) return maxMotorSpeed;
     if (speed < -maxMotorSpeed) return -maxMotorSpeed;
     return speed;
 }
 
+/**
+ * @brief Normaliza la velocidad angular (torsión) en función de los valores de entrada del joystick.
+ * 
+ * @param theta_joy Ángulo del joystick que indica la dirección de la torsión.
+ * @param theta_robot Ángulo actual del robot.
+ * @return La velocidad angular normalizada en radianes por segundo.
+ */
+float chassisMove::normalizeW(float theta_joy_rads, float theta_robot_rads) {
+    float angle_error = theta_joy_rads - theta_robot_rads;
+    if (angle_error > M_PI) angle_error -= 2 * PI;
+    if (angle_error < -M_PI) angle_error += 2 * PI;
+    float w_rs = K_TWIST * angle_error;
+    return w_rs;
+}
+
 chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
                          IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
                          float maxMotorSpeed)
@@ -13,12 +34,27 @@ chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
       leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor),
       maxMotorSpeed(maxMotorSpeed) {}
 
+/**
+ * @brief Convierte las entradas de los joysticks en velocidades de los motores.
+ * 
+ * Este método toma las entradas de los dos joysticks (para movimiento y torsión) y las convierte en 
+ * velocidades para cada rueda del chasis mecanum.
+ * 
+ * @param x1 Entrada del joystick 1 (eje X para desplazamiento en el plano horizontal).
+ * @param y1 Entrada del joystick 1 (eje Y para desplazamiento en el plano vertical).
+ * @param x2 Entrada del joystick 2 (eje X para control de torsión).
+ * @param y2 Entrada del joystick 2 (eje Y para control de torsión).
+ * @param theta_robot Ángulo de orientación del robot (en radianes).
+ * @param theta_joy Ángulo de orientación del joystick2 (en radianes).
+ */
+void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2, float theta_robot_rads) {
+    // Cálculo del ángulo deseado 
+    float theta_joy_rads = atan2(y2, x2);
+    // Cálculo de la torsión (velocidad angular)
+    float w_rs = normalizeW(theta_joy_rads, theta_robot_rads)
 
-void chassisMove::joystickToMotors(float x, float y, float w) {
-    //v=M*u
     // u
-    Eigen::Vector3f joystick_input(x, y, w);
-
+    Eigen::Vector3f joystick_input(x1, y1, w);
     // M
     Eigen::MatrixXf control_matrix(4, 3);
     control_matrix << -1, -1,  CHASSIS_RADIUS,  // Delantera izquierda
@@ -26,14 +62,14 @@ void chassisMove::joystickToMotors(float x, float y, float w) {
                        1,  1,  CHASSIS_RADIUS,  // Trasera derecha
                       -1,  1,  CHASSIS_RADIUS;  // Trasera izquierda
 
-    // v
+    //v=M*u
     Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
     wheel_speed = wheel_speed.unaryExpr([this](float speed) { return normalizeSpeed(speed); });
 
-    leftFrontMotor->actuate(wheel_speed[0]);
-    rightFrontMotor->actuate(wheel_speed[1]);
-    rightBackMotor->actuate(wheel_speed[2]);
-    leftBackMotor->actuate(wheel_speed[3]);
+    leftFrontMotor->actuate(wheel_speed[0]);   // Delantera izquierda
+    rightFrontMotor->actuate(wheel_speed[1]);  // Delantera derecha
+    rightBackMotor->actuate(wheel_speed[2]);   // Trasera derecha
+    leftBackMotor->actuate(wheel_speed[3]);    // Trasera izquierda
 }
 
 void chassisMove::stop() {

From c16fcee357f39aec48500f9a6d5259ab9a473a78 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Tue, 7 Jan 2025 16:09:29 -0600
Subject: [PATCH 06/13] agregar canController y magnitudes

---
 inc/chassisMove.hpp | 7 ++++---
 src/chassisMove.cpp | 6 +++---
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index e1452ad..864c55a 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -16,9 +16,10 @@
 
 #include <Eigen/Dense> 
 #include "IntfMotor.hpp" 
+#include "ControllerCAN.hpp"
 
 #define CHASSIS_RADIUS 0.3f  // Radio del chasis (distancia del centro a una rueda) en metros
-#define MAX_MOTOR_SPEED 1.0f // Velocidad máxima del motor
+#define MAX_MOTOR_SPEED 465.0f // Velocidad máxima del motor rpm
 #define K_TWIST 1.0f         // Sensibilidad para torsión del chasis
 #define PI 3.14159265358979323846
 
@@ -36,7 +37,7 @@ class chassisMove {
     IntfMotor* leftBackMotor;
     IntfMotor* rightBackMotor;
 
-    float maxMotorSpeed; 
+    float maxMotorSpeed_rpm; 
 
     float normalizeSpeed(float speed);
     float normalizeW(float w);
@@ -44,7 +45,7 @@ class chassisMove {
 public:
     chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
                 IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
-                float maxMotorSpeed = MAX_MOTOR_SPEED);
+                float maxMotorSpeed_rpm = MAX_MOTOR_SPEED);
 
     void joystickToMotors(float x1, float y1, float x2, float y2, float theta_robot);
 
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 1ba8adf..28a17d7 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -7,8 +7,8 @@
  * @return La velocidad normalizada.
 */
 float chassisMove::normalizeSpeed(float speed) {
-    if (speed > maxMotorSpeed) return maxMotorSpeed;
-    if (speed < -maxMotorSpeed) return -maxMotorSpeed;
+    if (speed > maxMotorSpeed_rpm) return maxMotorSpeed_rpm;
+    if (speed < -maxMotorSpeed_rpm) return -maxMotorSpeed_rpm;
     return speed;
 }
 
@@ -32,7 +32,7 @@ chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
                          float maxMotorSpeed)
     : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
       leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor),
-      maxMotorSpeed(maxMotorSpeed) {}
+      maxMotorSpeed(maxMotorSpeed_rpm) {}
 
 /**
  * @brief Convierte las entradas de los joysticks en velocidades de los motores.

From 54cc82dbd1d578106c5b0dfbdaed69d41ae9ebcd Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Tue, 7 Jan 2025 17:16:21 -0600
Subject: [PATCH 07/13] theta robot a variable global

---
 inc/chassisMove.hpp | 11 ++++++-----
 src/chassisMove.cpp |  8 +++++---
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 864c55a..e7706da 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -11,18 +11,19 @@
  *
  */
 
-#ifndef CHASSIS_MOVE_HPP
-#define CHASSIS_MOVE_HPP
-
 #include <Eigen/Dense> 
 #include "IntfMotor.hpp" 
 #include "ControllerCAN.hpp"
 
+#ifndef CHASSIS_MOVE_HPP
+#define CHASSIS_MOVE_HPP
 #define CHASSIS_RADIUS 0.3f  // Radio del chasis (distancia del centro a una rueda) en metros
 #define MAX_MOTOR_SPEED 465.0f // Velocidad máxima del motor rpm
 #define K_TWIST 1.0f         // Sensibilidad para torsión del chasis
 #define PI 3.14159265358979323846
 
+float theta_robot_rads; //angulo actual del robot
+
 /**
  * @brief Clase para controlar el movimiento de un chasis mecanum utilizando joysticks.
  * 
@@ -40,14 +41,14 @@ class chassisMove {
     float maxMotorSpeed_rpm; 
 
     float normalizeSpeed(float speed);
-    float normalizeW(float w);
+    float normalizeW(float theta_joy_rads);
 
 public:
     chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
                 IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
                 float maxMotorSpeed_rpm = MAX_MOTOR_SPEED);
 
-    void joystickToMotors(float x1, float y1, float x2, float y2, float theta_robot);
+    void joystickToMotors(float x1, float y1, float x2, float y2);
 
     void stop();
 };
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 28a17d7..2328500 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -19,11 +19,13 @@ float chassisMove::normalizeSpeed(float speed) {
  * @param theta_robot Ángulo actual del robot.
  * @return La velocidad angular normalizada en radianes por segundo.
  */
-float chassisMove::normalizeW(float theta_joy_rads, float theta_robot_rads) {
+float chassisMove::normalizeW(float theta_joy_rads) {
     float angle_error = theta_joy_rads - theta_robot_rads;
     if (angle_error > M_PI) angle_error -= 2 * PI;
     if (angle_error < -M_PI) angle_error += 2 * PI;
     float w_rs = K_TWIST * angle_error;
+    //TODO: si se actualiza?
+    //theta_robot_rads=w_rs; 
     return w_rs;
 }
 
@@ -47,11 +49,11 @@ chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
  * @param theta_robot Ángulo de orientación del robot (en radianes).
  * @param theta_joy Ángulo de orientación del joystick2 (en radianes).
  */
-void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2, float theta_robot_rads) {
+void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     // Cálculo del ángulo deseado 
     float theta_joy_rads = atan2(y2, x2);
     // Cálculo de la torsión (velocidad angular)
-    float w_rs = normalizeW(theta_joy_rads, theta_robot_rads)
+    float w_rs = normalizeW(theta_joy_rads)
 
     // u
     Eigen::Vector3f joystick_input(x1, y1, w);

From d9bd197c6589bdda5fa1b8cb2bf038b9722ab022 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Thu, 9 Jan 2025 15:58:31 -0600
Subject: [PATCH 08/13] agregar queue para leer datos de los motores y ajustar
 velocidad

---
 inc/chassisMove.hpp |  4 +---
 src/chassisMove.cpp | 44 +++++++++++++++++++++-----------------------
 2 files changed, 22 insertions(+), 26 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index e7706da..2961bc5 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -22,8 +22,6 @@
 #define K_TWIST 1.0f         // Sensibilidad para torsión del chasis
 #define PI 3.14159265358979323846
 
-float theta_robot_rads; //angulo actual del robot
-
 /**
  * @brief Clase para controlar el movimiento de un chasis mecanum utilizando joysticks.
  * 
@@ -37,11 +35,11 @@ class chassisMove {
     IntfMotor* rightFrontMotor;
     IntfMotor* leftBackMotor;
     IntfMotor* rightBackMotor;
+    float currentMotorSpeeds[4];
 
     float maxMotorSpeed_rpm; 
 
     float normalizeSpeed(float speed);
-    float normalizeW(float theta_joy_rads);
 
 public:
     chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 2328500..ba701cd 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -12,21 +12,16 @@ float chassisMove::normalizeSpeed(float speed) {
     return speed;
 }
 
-/**
- * @brief Normaliza la velocidad angular (torsión) en función de los valores de entrada del joystick.
- * 
- * @param theta_joy Ángulo del joystick que indica la dirección de la torsión.
- * @param theta_robot Ángulo actual del robot.
- * @return La velocidad angular normalizada en radianes por segundo.
- */
-float chassisMove::normalizeW(float theta_joy_rads) {
-    float angle_error = theta_joy_rads - theta_robot_rads;
-    if (angle_error > M_PI) angle_error -= 2 * PI;
-    if (angle_error < -M_PI) angle_error += 2 * PI;
-    float w_rs = K_TWIST * angle_error;
-    //TODO: si se actualiza?
-    //theta_robot_rads=w_rs; 
-    return w_rs;
+// Función para recibir las velocidades de los motores
+void queueReceive() {
+    float actualMotorSpeeds[4];
+    if (xQueueReceive(motorSpeedQueue, &actualMotorSpeeds, portMAX_DELAY) == pdPASS) {
+        for (int i = 0; i < 4; i++) {
+            currentMotorSpeeds[i] = actualMotorSpeeds[i]; // Almacena las velocidades actuales
+        }
+    } else {
+        printf("Error.\n");
+    }
 }
 
 chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
@@ -46,14 +41,11 @@ chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
  * @param y1 Entrada del joystick 1 (eje Y para desplazamiento en el plano vertical).
  * @param x2 Entrada del joystick 2 (eje X para control de torsión).
  * @param y2 Entrada del joystick 2 (eje Y para control de torsión).
- * @param theta_robot Ángulo de orientación del robot (en radianes).
  * @param theta_joy Ángulo de orientación del joystick2 (en radianes).
  */
 void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     // Cálculo del ángulo deseado 
-    float theta_joy_rads = atan2(y2, x2);
-    // Cálculo de la torsión (velocidad angular)
-    float w_rs = normalizeW(theta_joy_rads)
+    float w=atan2(y2, x2);
 
     // u
     Eigen::Vector3f joystick_input(x1, y1, w);
@@ -68,10 +60,16 @@ void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
     wheel_speed = wheel_speed.unaryExpr([this](float speed) { return normalizeSpeed(speed); });
 
-    leftFrontMotor->actuate(wheel_speed[0]);   // Delantera izquierda
-    rightFrontMotor->actuate(wheel_speed[1]);  // Delantera derecha
-    rightBackMotor->actuate(wheel_speed[2]);   // Trasera derecha
-    leftBackMotor->actuate(wheel_speed[3]);    // Trasera izquierda
+        // Restar las velocidades actuales (obtenidas de la cola)
+    Eigen::VectorXf adjusted_speed(4);
+    for (int i = 0; i < 4; i++) {
+        adjusted_speed[i] = currentMotorSpeeds[i]-wheel_speed[i] ;
+    }
+
+    leftFrontMotor->actuate(adjusted_speed[0]);   // Delantera izquierda
+    rightFrontMotor->actuate(adjusted_speed[1]);  // Delantera derecha
+    rightBackMotor->actuate(adjusted_speed[2]);   // Trasera derecha
+    leftBackMotor->actuate(adjusted_speed[3]);    // Trasera izquierda
 }
 
 void chassisMove::stop() {

From 9a17c49e5a67d5e039e246e9cac8a1f8e65d17f4 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Fri, 10 Jan 2025 13:22:52 -0600
Subject: [PATCH 09/13] queueSend y queueReceive de los motores can

---
 inc/chassisMove.hpp | 12 ++++++++--
 src/chassisMove.cpp | 55 +++++++++++++++++++++++++++++----------------
 2 files changed, 46 insertions(+), 21 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 2961bc5..0a7589b 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -35,10 +35,18 @@ class chassisMove {
     IntfMotor* rightFrontMotor;
     IntfMotor* leftBackMotor;
     IntfMotor* rightBackMotor;
-    float currentMotorSpeeds[4];
-
     float maxMotorSpeed_rpm; 
 
+    BaseType_t xQueueSend(
+        QueueHandle_t wheelSpeedQueue,
+        const void * adjusted_speed,
+        TickType_t xTicksToWait
+    );
+    // Cola de 10 elementos de tamaño float[4] para send
+    wheelSpeedQueue = xQueueCreate(10, sizeof(float[4]));
+    void queueSend();
+    Eigen::Vector4f queueReceive();
+
     float normalizeSpeed(float speed);
 
 public:
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index ba701cd..6ebace3 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -6,31 +6,49 @@
  * @param speed Velocidad a normalizar.
  * @return La velocidad normalizada.
 */
+
+chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
+                         IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
+                         float maxMotorSpeed)
+    : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
+      leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor),
+      maxMotorSpeed(maxMotorSpeed_rpm) {}
+
+
 float chassisMove::normalizeSpeed(float speed) {
     if (speed > maxMotorSpeed_rpm) return maxMotorSpeed_rpm;
     if (speed < -maxMotorSpeed_rpm) return -maxMotorSpeed_rpm;
     return speed;
 }
 
-// Función para recibir las velocidades de los motores
-void queueReceive() {
-    float actualMotorSpeeds[4];
+// Función para mandar las velocidades claculadas de los motores desde queue de CAN
+//TODO: poner tiempo limitado de espera tanto a send como receive
+void queueSend() {
+    float adjusted_speed[4];
+    // cambiar de vector eigen a array
+    for (int i = 0; i < 4; i++) {
+        adjusted_speed[i] = currentMotorSpeeds[i];  
+    }
+    if (xQueueSend(wheelSpeedQueue, (void *)adjusted_speed, portMAX_DELAY) != pdPASS) {
+        printf("Error: No se pudo enviar los datos a la cola.\n");
+    } else {
+        printf("Datos enviados a la cola.\n");
+    }
+}
+
+// Función para recibir las velocidades de los motores desde queue de CAN
+Eigen::VectorXf chassisMove::queueReceive() {
+    float actualMotorSpeeds[4]={0};
+    Eigen::VectorXf current_speeds
     if (xQueueReceive(motorSpeedQueue, &actualMotorSpeeds, portMAX_DELAY) == pdPASS) {
-        for (int i = 0; i < 4; i++) {
-            currentMotorSpeeds[i] = actualMotorSpeeds[i]; // Almacena las velocidades actuales
-        }
+        current_speeds = Eigen::Map<Eigen::VectorXf>(actualMotorSpeeds, 4);
     } else {
         printf("Error.\n");
+        return current_speeds.setZero();
     }
+    return current_speeds;
 }
 
-chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
-                         IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
-                         float maxMotorSpeed)
-    : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
-      leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor),
-      maxMotorSpeed(maxMotorSpeed_rpm) {}
-
 /**
  * @brief Convierte las entradas de los joysticks en velocidades de los motores.
  * 
@@ -41,7 +59,7 @@ chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
  * @param y1 Entrada del joystick 1 (eje Y para desplazamiento en el plano vertical).
  * @param x2 Entrada del joystick 2 (eje X para control de torsión).
  * @param y2 Entrada del joystick 2 (eje Y para control de torsión).
- * @param theta_joy Ángulo de orientación del joystick2 (en radianes).
+ * @param w Ángulo de orientación del joystick2 (en radianes).
  */
 void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     // Cálculo del ángulo deseado 
@@ -60,11 +78,10 @@ void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
     wheel_speed = wheel_speed.unaryExpr([this](float speed) { return normalizeSpeed(speed); });
 
-        // Restar las velocidades actuales (obtenidas de la cola)
-    Eigen::VectorXf adjusted_speed(4);
-    for (int i = 0; i < 4; i++) {
-        adjusted_speed[i] = currentMotorSpeeds[i]-wheel_speed[i] ;
-    }
+    Eigen::VectorXf currentMotorSpeeds=queueReceive();
+
+    //obtener la diferencia de velocidades
+    Eigen::VectorXf adjusted_speed = wheel_speed - currentMotorSpeeds;
 
     leftFrontMotor->actuate(adjusted_speed[0]);   // Delantera izquierda
     rightFrontMotor->actuate(adjusted_speed[1]);  // Delantera derecha

From cd1e54c9f47ad4a63fd6363e097cbad1d11f4d29 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Tue, 14 Jan 2025 00:04:33 -0600
Subject: [PATCH 10/13] Struct TDB vectores

---
 inc/chassisMove.hpp |  42 ++++++++++++------
 src/chassisMove.cpp | 102 +++++++++++++++++++++++++++-----------------
 2 files changed, 92 insertions(+), 52 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 0a7589b..7e30582 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -3,7 +3,7 @@
  *
  *  Created on: Jan 02, 2025
  *      Author: @sofiaariasv2002
- *              @AnaValeria
+ *              @anaValeria098       
  * 
  * For mor information:
  * https://learning.oreilly.com/library/view/wheeled-mobile-robotics/9780128042380/B9780128042045000020_1.xhtml#s0070
@@ -11,23 +11,33 @@
  *
  */
 
+
+#ifndef CHASSIS_MOVE_HPP
+#define CHASSIS_MOVE_HPP
+
 #include <Eigen/Dense> 
 #include "IntfMotor.hpp" 
 #include "ControllerCAN.hpp"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
 
-#ifndef CHASSIS_MOVE_HPP
-#define CHASSIS_MOVE_HPP
 #define CHASSIS_RADIUS 0.3f  // Radio del chasis (distancia del centro a una rueda) en metros
 #define MAX_MOTOR_SPEED 465.0f // Velocidad máxima del motor rpm
-#define K_TWIST 1.0f         // Sensibilidad para torsión del chasis
 #define PI 3.14159265358979323846
 
+/**
+ * @brief Estructura Vector de velocidades de los motores.
+ */
+struct TDB {
+    Eigen::Vector4i motorSpeeds; 
+};
+
 /**
  * @brief Clase para controlar el movimiento de un chasis mecanum utilizando joysticks.
  * 
  * Esta clase permite controlar el movimiento de un robot con chasis mecanum. La clase proporciona 
  * métodos para convertir las entradas de joystick en velocidades de motor, además de normalizar 
- * las velocidades y calcular la torsión en base a las entradas de control.
+ * las velocidades y calcular la torsión en base a las entradas de control mediante colas de FreeRtos
  */
 class chassisMove {
 private:
@@ -37,18 +47,22 @@ class chassisMove {
     IntfMotor* rightBackMotor;
     float maxMotorSpeed_rpm; 
 
-    BaseType_t xQueueSend(
-        QueueHandle_t wheelSpeedQueue,
-        const void * adjusted_speed,
-        TickType_t xTicksToWait
-    );
-    // Cola de 10 elementos de tamaño float[4] para send
-    wheelSpeedQueue = xQueueCreate(10, sizeof(float[4]));
-    void queueSend();
-    Eigen::Vector4f queueReceive();
+    QueueHandle_t sendQueue; 
+    QueueHandle_t receiveQueue;
 
     float normalizeSpeed(float speed);
 
+    BaseType_t xQueueSend(
+        QueueHandle_t queue, 
+        const TDB* data, 
+        TickType_t xTicksToWait);
+        
+    BaseType_t xQueueReceive(
+        QueueHandle_t queue, 
+        TDB* data, 
+        TickType_t xTicksToWait);
+
+
 public:
     chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
                 IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 6ebace3..ac7e7ca 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -1,59 +1,79 @@
 #include "chassisMove.hpp"
 
 /**
- * @brief Normaliza la velocidad del motor para asegurarse de que no exceda la velocidad máxima.
+ * @brief Constructor de la clase chassisMove.
+ * 
+ * Inicializa los punteros de motor, las velocidades máximas, y crea las colas de envío y recepción
+ * 
+ * cada instancia de la clase necesita tener su propia cola privada. 
+ * Esto asegura que cada objeto de tipo chassisMove maneje su propia cola,
+ * sin interferir con otras instancias.
  * 
- * @param speed Velocidad a normalizar.
- * @return La velocidad normalizada.
 */
-
 chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
                          IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
-                         float maxMotorSpeed)
+                         float maxMotorSpeed_rpm)
     : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
       leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor),
-      maxMotorSpeed(maxMotorSpeed_rpm) {}
-
+      maxMotorSpeed_rpm(maxMotorSpeed_rpm) {
+    sendQueue = xQueueCreate(10, sizeof(TDB));
+    receiveQueue = xQueueCreate(10, sizeof(TDB));
+    if (sendQueue == NULL || receiveQueue == NULL) {
+        printf("Error: No se pudieron crear las colas.\n");
+    }
+}
 
+/**
+ * @brief Normaliza la velocidad del motor.
+ * 
+ * Si la velocidad excede los límites, se ajusta al máximo permitido.
+ * 
+ * @param speed Velocidad del motor a normalizar.
+ * @return Velocidad normalizada.
+ */
 float chassisMove::normalizeSpeed(float speed) {
     if (speed > maxMotorSpeed_rpm) return maxMotorSpeed_rpm;
     if (speed < -maxMotorSpeed_rpm) return -maxMotorSpeed_rpm;
     return speed;
 }
 
-// Función para mandar las velocidades claculadas de los motores desde queue de CAN
-//TODO: poner tiempo limitado de espera tanto a send como receive
-void queueSend() {
-    float adjusted_speed[4];
-    // cambiar de vector eigen a array
-    for (int i = 0; i < 4; i++) {
-        adjusted_speed[i] = currentMotorSpeeds[i];  
-    }
-    if (xQueueSend(wheelSpeedQueue, (void *)adjusted_speed, portMAX_DELAY) != pdPASS) {
+/**
+ * @brief Envía datos de velocidad de motores a la cola especificada.
+ * 
+ * @param queue Cola donde se enviarán los datos.
+ * @param data Estructura TDB con las velocidades de los motores.
+ * @param xTicksToWait Tiempo de espera máximo en ticks.
+ * @return `pdPASS` si el envío fue exitoso, `pdFAIL` en caso contrario.
+ */
+BaseType_t chassisMove::xQueueSend(QueueHandle_t queue, const TDB* data, TickType_t xTicksToWait) {
+    if (xQueueSend(queue, (void*)data, xTicksToWait) != pdPASS) {
         printf("Error: No se pudo enviar los datos a la cola.\n");
-    } else {
-        printf("Datos enviados a la cola.\n");
+        return pdFAIL;
     }
+    return pdPASS;
 }
 
-// Función para recibir las velocidades de los motores desde queue de CAN
-Eigen::VectorXf chassisMove::queueReceive() {
-    float actualMotorSpeeds[4]={0};
-    Eigen::VectorXf current_speeds
-    if (xQueueReceive(motorSpeedQueue, &actualMotorSpeeds, portMAX_DELAY) == pdPASS) {
-        current_speeds = Eigen::Map<Eigen::VectorXf>(actualMotorSpeeds, 4);
-    } else {
-        printf("Error.\n");
-        return current_speeds.setZero();
+/**
+ * @brief Recibe datos de velocidad de motores desde la cola especificada.
+ * 
+ * @param queue Cola desde donde se recibirán los datos.
+ * @param data Estructura TDB donde se almacenarán las velocidades recibidas.
+ * @param xTicksToWait Tiempo de espera máximo en ticks.
+ * @return `pdPASS` si la recepción fue exitosa, `pdFAIL` en caso contrario.
+ */
+BaseType_t chassisMove::xQueueReceive(QueueHandle_t queue, TDB* data, TickType_t xTicksToWait) {
+    if (xQueueReceive(queue, (void*)data, xTicksToWait) != pdPASS) {
+        printf("Error: No se pudo recibir los datos de la cola.\n");
+        return pdFAIL;
     }
-    return current_speeds;
+    return pdPASS;
 }
 
 /**
  * @brief Convierte las entradas de los joysticks en velocidades de los motores.
  * 
- * Este método toma las entradas de los dos joysticks (para movimiento y torsión) y las convierte en 
- * velocidades para cada rueda del chasis mecanum.
+ * Calcula las velocidades para cada rueda en función de las entradas de joystick, 
+ * las normaliza y las envía a la cola de envío.
  * 
  * @param x1 Entrada del joystick 1 (eje X para desplazamiento en el plano horizontal).
  * @param y1 Entrada del joystick 1 (eje Y para desplazamiento en el plano vertical).
@@ -64,7 +84,6 @@ Eigen::VectorXf chassisMove::queueReceive() {
 void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     // Cálculo del ángulo deseado 
     float w=atan2(y2, x2);
-
     // u
     Eigen::Vector3f joystick_input(x1, y1, w);
     // M
@@ -78,15 +97,22 @@ void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
     wheel_speed = wheel_speed.unaryExpr([this](float speed) { return normalizeSpeed(speed); });
 
-    Eigen::VectorXf currentMotorSpeeds=queueReceive();
+    Eigen::Vector4f currentMotorSpeeds;
+    if (xQueueReceive(receiveQueue, &currentMotorSpeeds, pdMS_TO_TICKS(100)) != pdPASS) {
+        printf("Error: No se pudieron recibir las velocidades actuales.\n");
+        return;
+    }
 
-    //obtener la diferencia de velocidades
-    Eigen::VectorXf adjusted_speed = wheel_speed - currentMotorSpeeds;
+    Eigen::Vector4f error_speed = wheel_speed - currentMotorSpeeds;
+    leftFrontMotor->actuate(error_speed[0]);   // Delantera izquierda
+    rightFrontMotor->actuate(error_speed[1]);  // Delantera derecha
+    rightBackMotor->actuate(error_speed[2]);   // Trasera derecha
+    leftBackMotor->actuate(error_speed[3]);    // Trasera izquierda
+
+    if (xQueueSend(sendQueue, &error_speed, pdMS_TO_TICKS(100)) != pdPASS) {
+        printf("Error: No se pudo enviar el error de velocidades.\n");
+    }
 
-    leftFrontMotor->actuate(adjusted_speed[0]);   // Delantera izquierda
-    rightFrontMotor->actuate(adjusted_speed[1]);  // Delantera derecha
-    rightBackMotor->actuate(adjusted_speed[2]);   // Trasera derecha
-    leftBackMotor->actuate(adjusted_speed[3]);    // Trasera izquierda
 }
 
 void chassisMove::stop() {

From 656bd83ce4be485ae5020488d0b37cdea23ebca8 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Wed, 15 Jan 2025 20:52:04 -0600
Subject: [PATCH 11/13] modificar estructura de mortores a motor1...

---
 inc/chassisMove.hpp |  9 ++++++---
 src/chassisMove.cpp | 27 ++++++++++++++++-----------
 2 files changed, 22 insertions(+), 14 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 7e30582..076a19c 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -29,7 +29,10 @@
  * @brief Estructura Vector de velocidades de los motores.
  */
 struct TDB {
-    Eigen::Vector4i motorSpeeds; 
+    int motor1; // Velocidad motor delantero izquierdo
+    int motor2; // Velocidad motor delantero derecho
+    int motor3; // Velocidad motor trasero derecho
+    int motor4; // Velocidad motor trasero izquierdo
 };
 
 /**
@@ -47,8 +50,8 @@ class chassisMove {
     IntfMotor* rightBackMotor;
     float maxMotorSpeed_rpm; 
 
-    QueueHandle_t sendQueue; 
-    QueueHandle_t receiveQueue;
+    QueueHandle_t sendQueueCAN; 
+    QueueHandle_t receiveQueueCAN;
 
     float normalizeSpeed(float speed);
 
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index ac7e7ca..6d55acc 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -47,7 +47,7 @@ float chassisMove::normalizeSpeed(float speed) {
  */
 BaseType_t chassisMove::xQueueSend(QueueHandle_t queue, const TDB* data, TickType_t xTicksToWait) {
     if (xQueueSend(queue, (void*)data, xTicksToWait) != pdPASS) {
-        printf("Error: No se pudo enviar los datos a la cola.\n");
+        printf("Error: No se pudo enviar los datos a la cola de CAN.\n");
         return pdFAIL;
     }
     return pdPASS;
@@ -63,7 +63,7 @@ BaseType_t chassisMove::xQueueSend(QueueHandle_t queue, const TDB* data, TickTyp
  */
 BaseType_t chassisMove::xQueueReceive(QueueHandle_t queue, TDB* data, TickType_t xTicksToWait) {
     if (xQueueReceive(queue, (void*)data, xTicksToWait) != pdPASS) {
-        printf("Error: No se pudo recibir los datos de la cola.\n");
+        printf("Error: No se pudo recibir los datos de la cola de CAN.\n");
         return pdFAIL;
     }
     return pdPASS;
@@ -97,24 +97,29 @@ void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
     Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
     wheel_speed = wheel_speed.unaryExpr([this](float speed) { return normalizeSpeed(speed); });
 
-    Eigen::Vector4f currentMotorSpeeds;
-    if (xQueueReceive(receiveQueue, &currentMotorSpeeds, pdMS_TO_TICKS(100)) != pdPASS) {
-        printf("Error: No se pudieron recibir las velocidades actuales.\n");
-        return;
-    }
+    TDB currentSpeeds;
+    if (xQueueReceive(receiveQueue, &currentSpeeds, portMAX_DELAY) == pdPASS) {
+        Eigen::VectorXf currentMotorSpeeds(4);
+        currentMotorSpeeds << currentSpeeds.motor1, 
+                              currentSpeeds.motor2, 
+                              currentSpeeds.motor3, 
+                              currentSpeeds.motor4;
 
-    Eigen::Vector4f error_speed = wheel_speed - currentMotorSpeeds;
+    Eigen::Vector4f error_speed = wheel_speed.cast<float>() - currentMotorSpeeds;
     leftFrontMotor->actuate(error_speed[0]);   // Delantera izquierda
     rightFrontMotor->actuate(error_speed[1]);  // Delantera derecha
     rightBackMotor->actuate(error_speed[2]);   // Trasera derecha
     leftBackMotor->actuate(error_speed[3]);    // Trasera izquierda
 
-    if (xQueueSend(sendQueue, &error_speed, pdMS_TO_TICKS(100)) != pdPASS) {
-        printf("Error: No se pudo enviar el error de velocidades.\n");
+        TDB error_TDB = {error_speed[0], error_speed[1], error_speed[2], error_speed[3]};
+        xQueueSend(sendQueue, &error_TDB, portMAX_DELAY);
+    } else {
+        printf("Error: No se pudieron obtener las velocidades actuales desde la cola de CAN.\n");
     }
-
 }
 
+
+
 void chassisMove::stop() {
     leftFrontMotor->stop(0);
     rightFrontMotor->stop(0);

From 54202b29089071619f3c26d8a81cbf1a208f3665 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Wed, 22 Jan 2025 18:12:00 -0600
Subject: [PATCH 12/13] normalizar las velocidades si una exece el rango

---
 inc/chassisMove.hpp |  2 +-
 src/chassisMove.cpp | 32 ++++++++++++++++++++++++--------
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 076a19c..20df000 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -53,7 +53,7 @@ class chassisMove {
     QueueHandle_t sendQueueCAN; 
     QueueHandle_t receiveQueueCAN;
 
-    float normalizeSpeed(float speed);
+    float normalizeSpeed(TDB currentSpeeds);
 
     BaseType_t xQueueSend(
         QueueHandle_t queue, 
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 6d55acc..3cbbadd 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -24,19 +24,35 @@ chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
 }
 
 /**
- * @brief Normaliza la velocidad del motor.
+ * @brief Normaliza las velocidades de los motores para que ninguna exceda el rango permitido.
  * 
- * Si la velocidad excede los límites, se ajusta al máximo permitido.
+ * Si alguna velocidad supera el rango (-maxMotorSpeed_rpm a maxMotorSpeed_rpm), 
+ * se escalan todas proporcionalmente manteniendo sus signos originales.
  * 
- * @param speed Velocidad del motor a normalizar.
- * @return Velocidad normalizada.
+ * @param currentSpeeds Estructura TDB que contiene las velocidades de los motores.
+ * @return Una estructura TDB con las velocidades normalizadas.
  */
-float chassisMove::normalizeSpeed(float speed) {
-    if (speed > maxMotorSpeed_rpm) return maxMotorSpeed_rpm;
-    if (speed < -maxMotorSpeed_rpm) return -maxMotorSpeed_rpm;
-    return speed;
+TDB chassisMove::normalizeSpeed(TDB currentSpeeds) {
+    float maxSpeed = std::max({
+        std::abs(currentSpeeds.motor1),
+        std::abs(currentSpeeds.motor2),
+        std::abs(currentSpeeds.motor3),
+        std::abs(currentSpeeds.motor4)
+    });
+
+    if (maxSpeed > maxMotorSpeed_rpm) {
+        float scale = maxMotorSpeed_rpm / maxSpeed; 
+
+        currentSpeeds.motor1 *= scale;
+        currentSpeeds.motor2 *= scale;
+        currentSpeeds.motor3 *= scale;
+        currentSpeeds.motor4 *= scale;
+    }
+    
+    return currentSpeeds;
 }
 
+
 /**
  * @brief Envía datos de velocidad de motores a la cola especificada.
  * 

From e5c6f4f0229ff38f9f1750ea2fe4db5358a7a2f3 Mon Sep 17 00:00:00 2001
From: sofiaariasv2002 <sofiaariasv2002@gmail.com>
Date: Wed, 22 Jan 2025 19:54:58 -0600
Subject: [PATCH 13/13] normalize con eigen

---
 inc/chassisMove.hpp |  2 +-
 src/chassisMove.cpp | 31 +++++++++++++++----------------
 2 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp
index 20df000..10cd080 100644
--- a/inc/chassisMove.hpp
+++ b/inc/chassisMove.hpp
@@ -53,7 +53,7 @@ class chassisMove {
     QueueHandle_t sendQueueCAN; 
     QueueHandle_t receiveQueueCAN;
 
-    float normalizeSpeed(TDB currentSpeeds);
+    Eigen normalizeSpeed(const Eigen::VectorXf& wheel_speed);
 
     BaseType_t xQueueSend(
         QueueHandle_t queue, 
diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp
index 3cbbadd..55bf667 100644
--- a/src/chassisMove.cpp
+++ b/src/chassisMove.cpp
@@ -32,27 +32,26 @@ chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
  * @param currentSpeeds Estructura TDB que contiene las velocidades de los motores.
  * @return Una estructura TDB con las velocidades normalizadas.
  */
-TDB chassisMove::normalizeSpeed(TDB currentSpeeds) {
-    float maxSpeed = std::max({
-        std::abs(currentSpeeds.motor1),
-        std::abs(currentSpeeds.motor2),
-        std::abs(currentSpeeds.motor3),
-        std::abs(currentSpeeds.motor4)
-    });
-
+/**
+ * @brief Normaliza el vector de velocidades de las ruedas para que ninguna supere el rango permitido.
+ * 
+ * Si alguna velocidad en el vector excede el rango (-maxMotorSpeed_rpm a maxMotorSpeed_rpm), 
+ * todo el vector se escala proporcionalmente manteniendo los signos.
+ * 
+ * @param wheel_speed Vector de velocidades de los motores (Eigen::VectorXf).
+ * @return Un vector Eigen::VectorXf con las velocidades normalizadas.
+ */
+Eigen::VectorXf chassisMove::normalizeSpeed(const Eigen::VectorXf& wheel_speed) {
+    float maxSpeed = wheel_speed.cwiseAbs().maxCoeff();
     if (maxSpeed > maxMotorSpeed_rpm) {
         float scale = maxMotorSpeed_rpm / maxSpeed; 
-
-        currentSpeeds.motor1 *= scale;
-        currentSpeeds.motor2 *= scale;
-        currentSpeeds.motor3 *= scale;
-        currentSpeeds.motor4 *= scale;
+        return wheel_speed * scale; // Escala todo el vector manteniendo los signos
     }
-    
-    return currentSpeeds;
+    return wheel_speed;
 }
 
 
+
 /**
  * @brief Envía datos de velocidad de motores a la cola especificada.
  * 
@@ -111,7 +110,7 @@ void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) {
 
     //v=M*u
     Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
-    wheel_speed = wheel_speed.unaryExpr([this](float speed) { return normalizeSpeed(speed); });
+    wheel_speed = normalizeSpeed(wheel_speed);
 
     TDB currentSpeeds;
     if (xQueueReceive(receiveQueue, &currentSpeeds, portMAX_DELAY) == pdPASS) {