involucrar imu para actualizacion de la velocidad de las llantas - co…

inc/chassisMove.hpp

@@ -0,0 +1,56 @@
+ /*
+ * 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
+ *
+ */
+
+#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.
+ * 
+ * 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;
+    IntfMotor* rightFrontMotor;
+    IntfMotor* leftBackMotor;
+    IntfMotor* rightBackMotor;
+
+    float maxMotorSpeed_rpm; 
+
+    float normalizeSpeed(float speed);
+    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);
+
+    void stop();
+};
+
+#endif // CHASSIS_MOVE_HPP

src/chassisMove.cpp

@@ -0,0 +1,98 @@
+#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_rpm) return maxMotorSpeed_rpm;
+    if (speed < -maxMotorSpeed_rpm) return -maxMotorSpeed_rpm;
+    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;
+}
+
+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.
+ * 
+ * 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) {
+    // 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)
+
+    // 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 = 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  
+
+    //PARTE ANA - leer referencia de velovidad en el IMU
+    //postitionr1... es la velocidad detectada en el imu para cada uno de los motores. 
+    //planeados1... son la velocidades calculadas en la matriz
+
+    float m1= positionr1-planeado1;
+    float m2= positionr2-planeado2;
+    float m3= positionr3-planeado3;
+    float m4= positionr4-planeado4;
+
+    leftFrontMotor->actuate(wheel_speed[0]+m1);   // Delantera izquierda
+    rightFrontMotor->actuate(wheel_speed[1]+m2);  // Delantera derecha
+    rightBackMotor->actuate(wheel_speed[2]+m3);   // Trasera derecha
+    leftBackMotor->actuate(wheel_speed[3]+m4);    // Trasera izquierda
+
+}
+
+
+void chassisMove::stop() {
+    leftFrontMotor->stop(0);
+    rightFrontMotor->stop(0);
+    leftBackMotor->stop(0);
+    rightBackMotor->stop(0);
+}