From fb29873038ef4b9c07108be045ac9b188bbfa19e Mon Sep 17 00:00:00 2001 From: sofiaariasv2002 Date: Tue, 7 Jan 2025 20:33:47 -0600 Subject: [PATCH 1/7] involucrar imu para actualizacion de la velocidad de las llantas - codigo de ana --- inc/chassisMove.hpp | 56 ++++++++++++++++++++++++++ src/chassisMove.cpp | 98 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100644 inc/chassisMove.hpp create mode 100644 src/chassisMove.cpp 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); +} From 1c752a666e8035a28486e0324a5eec69e4c30b2d Mon Sep 17 00:00:00 2001 From: Ana Date: Thu, 9 Jan 2025 22:44:45 -0600 Subject: [PATCH 2/7] primer intento ana --- src/chassisMove.cpp | 33 ++++++++++++++++----------------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp index e1e4cde..9b604ac 100644 --- a/src/chassisMove.cpp +++ b/src/chassisMove.cpp @@ -71,28 +71,27 @@ void chassisMove::joystickToMotors(float x1, float y1, float x2, float y2) { 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 - + 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 + +//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 leftFrontSpeed, float rightFrontSpeed, float rightBackSpeed, float leftBackSpeed){ + + float x1g = x1 / 16384.0; // Conversión a G (para ±2g) esto solo los valores del acelerometro(tengo duda si se tiene que convertir) + float y1g = y1 / 16384.0; + float z1g = z1 / 16384.0; + + //para imprimir los valores dados por el acelerometro de la imu y por las velocidades de los motores. + printf("Acelerómetro: X=%.2f g, Y=%.2f g, Z=%.2f g\n", x1g, y1g, z1g); + printf("velocidades llantas: LF=%.2f , RF=%.2f , RB=%.2f, LB=%.2f \n", leftFrontSpeed, rightFrontSpeed, rightBackSpeed, leftBackSpeed); + +} From 75b6e8f6e1baf39c5848743bc09e6ebd8093f1fa Mon Sep 17 00:00:00 2001 From: Ana Date: Sat, 11 Jan 2025 19:26:11 -0600 Subject: [PATCH 3/7] 2ndo cambio ana, uso de la imu acelerometro y giroscopio --- src/chassisMove.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp index 9b604ac..60f5457 100644 --- a/src/chassisMove.cpp +++ b/src/chassisMove.cpp @@ -82,16 +82,21 @@ void chassisMove::stop() { } //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 leftFrontSpeed, float rightFrontSpeed, float rightBackSpeed, float leftBackSpeed){ - float x1g = x1 / 16384.0; // Conversión a G (para ±2g) esto solo los valores del acelerometro(tengo duda si se tiene que convertir) - float y1g = y1 / 16384.0; - float z1g = z1 / 16384.0; + 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 g, Y=%.2f g, Z=%.2f g\n", x1g, y1g, z1g); + printf("Acelerómetro: X=%.2f , Y=%.2f , Z=%.2f \n", x1g, 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); } From 6c0d21a88b458b172025512368df41eef924854c Mon Sep 17 00:00:00 2001 From: Ana Date: Sat, 15 Feb 2025 13:50:13 -0600 Subject: [PATCH 4/7] correccion de x2,y2 y z2 ana (no los habia agregado) --- src/chassisMove.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp index 60f5457..42a235f 100644 --- a/src/chassisMove.cpp +++ b/src/chassisMove.cpp @@ -85,17 +85,17 @@ void chassisMove::stop() { //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 leftFrontSpeed, float rightFrontSpeed, float rightBackSpeed, float leftBackSpeed){ +void chassisMove::LecturaDeDatos(float x1, float y1,float z1 ,float x2,float y2,float z2, float leftFrontSpeed, float rightFrontSpeed, float rightBackSpeed, float leftBackSpeed){ - 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 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 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", x1g, y1g, z1g); + 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); From b741c57adad31f1dac0829f89aff6470fc86ada8 Mon Sep 17 00:00:00 2001 From: Ana Date: Sat, 15 Feb 2025 15:31:25 -0600 Subject: [PATCH 5/7] modificaciones sin velocidad individual de las llantas Ana --- src/chassisMove.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp index 42a235f..88d2114 100644 --- a/src/chassisMove.cpp +++ b/src/chassisMove.cpp @@ -85,7 +85,7 @@ void chassisMove::stop() { //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, float leftFrontSpeed, float rightFrontSpeed, float rightBackSpeed, float leftBackSpeed){ +void chassisMove::LecturaDeDatos(float x1, float y1,float z1 ,float x2,float y2,float z2 ){ 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 @@ -97,6 +97,6 @@ void chassisMove::LecturaDeDatos(float x1, float y1,float z1 ,float x2,float y2, //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); + //printf("velocidades llantas: LF=%.2f , RF=%.2f , RB=%.2f, LB=%.2f \n", leftFrontSpeed, rightFrontSpeed, rightBackSpeed, leftBackSpeed); } From 12eafd142d1859dd970e416f4d1b28768cea91cf Mon Sep 17 00:00:00 2001 From: Ana Date: Sat, 15 Feb 2025 16:32:25 -0600 Subject: [PATCH 6/7] checar registros imu para spi ANA --- src/chassisMove.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp index 88d2114..1f8262f 100644 --- a/src/chassisMove.cpp +++ b/src/chassisMove.cpp @@ -86,6 +86,16 @@ void chassisMove::stop() { //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 + int16_t ay = IMU_ReadRegister16(0x3D); // Eje Y + 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 From 7be6ac5b06444c0d286d9d96e61f7af6b1e641f5 Mon Sep 17 00:00:00 2001 From: Ana Date: Sat, 22 Feb 2025 11:06:26 -0600 Subject: [PATCH 7/7] imu registros ya con la info --- src/chassisMove.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/chassisMove.cpp b/src/chassisMove.cpp index 1f8262f..805a53d 100644 --- a/src/chassisMove.cpp +++ b/src/chassisMove.cpp @@ -86,10 +86,10 @@ void chassisMove::stop() { //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 - int16_t ay = IMU_ReadRegister16(0x3D); // Eje Y + 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