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..805a53d --- /dev/null +++ b/src/chassisMove.cpp @@ -0,0 +1,112 @@ +#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 +} + +void chassisMove::stop() { + leftFrontMotor->stop(0); + rightFrontMotor->stop(0); + leftBackMotor->stop(0); + rightBackMotor->stop(0); +} + //PARTE ANA - leer referencia de velovidad en el IMU + //x1, y1, z1 son aceleromentro + //x2, y2, z2 son giroscopio + +//se llaman los valores dados por el acelerometro de la imu y por las velocidades de los motores. +void chassisMove::LecturaDeDatos(float x1, float y1,float z1 ,float x2,float y2,float z2 ){ + + // Leer los datos del acelerómetro + int16_t ax = IMU_ReadRegister16(0x3B); // Eje X https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf + int16_t ay = IMU_ReadRegister16(0x3D); // Eje Y se encuentra en la pagina 7 del datasheet + int16_t az = IMU_ReadRegister16(0x3F); // Eje Z + + // Leer los datos del giroscopio + int16_t gx = IMU_ReadRegister16(0x43); // Eje X + int16_t gy = IMU_ReadRegister16(0x45); // Eje Y + int16_t gz = IMU_ReadRegister16(0x47); // Eje Z + + float x1g = x1 * (9.81/16384.0); // Conversión dependiendo del acelerometro es el de 16384 y el 9.81 es valor gravedad nos da asi la aceleracion en m/s^2 + float y1g = y1 * (9.81/16384.0); //https://www.diarioelectronicohoy.com/blog/configurar-el-mpu6050 ----se que tal vez no es muy confiable pero es entendible + float z1g = z1 * (9.81/16384.0); + float x2g = x2 * (250.0/32767.0); + float y2g = y2 * (250.0/32767.0); + float z2g = z2 * (250.0/32767.0); // Conversión dependiendo del giroscopio es el de 32767 valores y el 250 grados por segundo es valor gravedad nos da en grados por segundo + + //para imprimir los valores dados por el acelerometro de la imu y por las velocidades de los motores. + printf("Acelerómetro: X=%.2f , Y=%.2f , Z=%.2f \n", x 1g, y1g, z1g); + printf("giroscopio: X=%.2f , Y=%.2f , Z=%.2f \n", x2g, y2g, z2g); + //printf("velocidades llantas: LF=%.2f , RF=%.2f , RB=%.2f, LB=%.2f \n", leftFrontSpeed, rightFrontSpeed, rightBackSpeed, leftBackSpeed); + +}