Left rigth

inc/chassisMove.hpp

@@ -0,0 +1,79 @@
+ /*
+ * chassisMove.hpp
+ *
+ *  Created on: Jan 02, 2025
+ *      Author: @sofiaariasv2002
+ *              @anaValeria098       
+ * 
+ * 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
+
+#include <Eigen/Dense> 
+#include "IntfMotor.hpp" 
+#include "ControllerCAN.hpp"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+#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 PI 3.14159265358979323846
+
+/**
+ * @brief Estructura Vector de velocidades de los motores.
+ */
+struct TDB {
+    int motor1; // Velocidad motor delantero izquierdo
+    int motor2; // Velocidad motor delantero derecho
+    int motor3; // Velocidad motor trasero derecho
+    int motor4; // Velocidad motor trasero izquierdo
+};
+
+/**
+ * @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 mediante colas de FreeRtos
+ */
+class chassisMove {
+private:
+    IntfMotor* leftFrontMotor;
+    IntfMotor* rightFrontMotor;
+    IntfMotor* leftBackMotor;
+    IntfMotor* rightBackMotor;
+    float maxMotorSpeed_rpm; 
+
+    QueueHandle_t sendQueueCAN; 
+    QueueHandle_t receiveQueueCAN;
+
+    Eigen normalizeSpeed(const Eigen::VectorXf& wheel_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, 
+                float maxMotorSpeed_rpm = MAX_MOTOR_SPEED);
+
+    void joystickToMotors(float x1, float y1, float x2, float y2);
+
+    void stop();
+};
+
+#endif // CHASSIS_MOVE_HPP

src/chassisMove.cpp

@@ -0,0 +1,143 @@
+#include "chassisMove.hpp"
+
+/**
+ * @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.
+ * 
+*/
+chassisMove::chassisMove(IntfMotor* leftFrontMotor, IntfMotor* rightFrontMotor,
+                         IntfMotor* leftBackMotor, IntfMotor* rightBackMotor, 
+                         float maxMotorSpeed_rpm)
+    : leftFrontMotor(leftFrontMotor), rightFrontMotor(rightFrontMotor),
+      leftBackMotor(leftBackMotor), rightBackMotor(rightBackMotor),
+      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 las velocidades de los motores para que ninguna exceda el rango permitido.
+ * 
+ * Si alguna velocidad supera el rango (-maxMotorSpeed_rpm a maxMotorSpeed_rpm), 
+ * se escalan todas proporcionalmente manteniendo sus signos originales.
+ * 
+ * @param currentSpeeds Estructura TDB que contiene las velocidades de los motores.
+ * @return Una estructura TDB con las velocidades normalizadas.
+ */
+/**
+ * @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; 
+        return wheel_speed * scale; // Escala todo el vector manteniendo los signos
+    }
+    return wheel_speed;
+}
+
+
+
+/**
+ * @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 de CAN.\n");
+        return pdFAIL;
+    }
+    return pdPASS;
+}
+
+/**
+ * @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 de CAN.\n");
+        return pdFAIL;
+    }
+    return pdPASS;
+}
+
+/**
+ * @brief Convierte las entradas de los joysticks en velocidades de los motores.
+ * 
+ * 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).
+ * @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 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 
+    float w=atan2(y2, x2);
+    // u
+    Eigen::Vector3f joystick_input(x1, y1, 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=M*u
+    Eigen::VectorXf wheel_speed = control_matrix * joystick_input;
+    wheel_speed = normalizeSpeed(wheel_speed);
+
+    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.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
+
+        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);
+    leftBackMotor->stop(0);
+    rightBackMotor->stop(0);
+}