diff --git a/inc/chassisMove.hpp b/inc/chassisMove.hpp new file mode 100644 index 0000000..e7706da --- /dev/null +++ b/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 +#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 diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp new file mode 100644 index 0000000..e1e4cde --- /dev/null +++ b/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); +}