Día 0: Bases.
- intentar replicar algún proyecto existente.
- Avanzar rápido y documentar todo rápido tambien.
- Fotos y vídeo más que texto.
Primera idea: que la base sea una tarjeta PVC: Una idea tonta que limita el tamaño del desarrollo innecesariamente.
La idea de pegar cosas a la tarjeta PVC no es muy buena, porque de hecho la goma se despegaba.
Día 1
Idea dos: Copia y pega:
https://github.com/keyestudio/KS0193-Self-balancing-Car-Kit/blob/master/KS0193.md:
Ir soldando todos los componentes.
Esto puede ser otra idea: https://wiki.keyestudio.com/Ks0193_keyestudio_Self-balancing_Car construir el hardware, e ir haciendo pequeños proyectos y librerías para ir probando las funciones, y documentando.
Noche 2
Comparar el hardware de KSO193 y ver qué tengo que se asemeje:
La tarjeta principal es un clon de Arduino uno. La tarjeta segundaria tiene conexiones para el módulo Xbee y drivers para los motores:
- 6612FNG chip driver
- MPU-6050 unidad inercial
Cuáles son mis componentes?
- Tengo motores amarillos,
- Xiao ESP32-S3 (240 MHz, cámara, micrófono, microSD, 2 gpio adicionales en la expansión, 2.4GHz Wi-Fi y BLE, bluetooth mesh, 8 MB RAM y Flash, pines touch)
- Xiao nrf52840 sense (64 MHz, BLE, NFC, 6 DOF IMU, micrófono, rgb)
- Como el NRF52840 es el que tiene IMU LSM6DS3TR, ese es el que voy a usar.
- Tengo unos L293DD en sop20.
Creo que esto es todo lo que necesito… El motor sí necesito comprar uno. este es el link del motor y tiene unos links abajo de lecturas interesantes que no voy a leer nunca XD
Día 3
Compré headers, y drivers de corriente (los primeros que vi no dan la corriente de los motores con encoders). Tuve pesadillas donde soñé que los servos y los motores de paso no necesitan realimentar, y parece que sí es así (son los de paso sí, con los servos no estoy seguro). Empecé a soldar y me he tardado.
Día 4.
Desistí de soldar porque la punta del soldador es muy gruesa. Compré un soldador en aliexpress, pero empecé a montar lo mínimo en un protoboard mínimo. La intención es hacer una base para el carrito, y poner el circuito ahí. Comencé con movimientos básicos del motor. y empezando a leer del giroscopio:
Despues de un ratoo peleando con el IMU…
Proximo paso? PID!
Noche 5:Filtros de Kalman.
Filtros de kalman
Encontré este vídeo que me gustó la explicación
Hice este prompt en ChatGPT: Estoy haciendo un carro de dos ruedas que se balancea. Estoy midiendo mi aceleración y cantidad de giro con una IMU LSM6DS3. ¿Porqué me proponen usar un filtro kalman para encontrar el ángulo respecto a la vertical, y el giro en el eje X?
ChatGPT: El filtro de Kalman es una herramienta muy útil para fusionar datos de múltiples fuentes, como en tu caso, donde estás midiendo la aceleración y la cantidad de giro con una IMU (unidad de medición inercial). Aquí hay algunas razones por las que se propone el uso de un filtro de Kalman para estimar el ángulo respecto a la vertical (inclinación) y el giro en el eje X:
- Precisión mejorada: Los datos brutos de una IMU pueden ser ruidosos y estar sujetos a errores, especialmente debido a factores como la vibración y la deriva. Un filtro de Kalman puede ayudar a filtrar este ruido y proporcionar una estimación más precisa y suavizada del ángulo y la velocidad angular.
- Fusión de datos: La IMU proporciona mediciones de aceleración lineal y velocidad angular en diferentes ejes. Utilizando un filtro de Kalman, puedes fusionar estas mediciones para obtener una estimación más completa y confiable del estado del sistema, que en tu caso sería el ángulo de inclinación y la velocidad angular en el eje X.
- Compensación de errores: Los sensores inerciales como la IMU pueden verse afectados por errores sistemáticos y de sesgo a lo largo del tiempo. El filtro de Kalman puede ayudar a compensar estos errores y proporcionar estimaciones más estables y consistentes a lo largo del tiempo.
- Adaptabilidad: El filtro de Kalman es un método adaptable que puede ajustarse dinámicamente según la confiabilidad relativa de las mediciones de la IMU en diferentes condiciones. Esto significa que puede adaptarse a cambios en la dinámica del sistema, como aceleraciones repentinas o movimientos bruscos del vehículo.
En resumen, el uso de un filtro de Kalman te permitirá obtener estimaciones más precisas y confiables del ángulo de inclinación y la velocidad angular de tu carro de dos ruedas, al fusionar de manera óptima las mediciones de la IMU y compensar los errores inherentes a los sensores inerciales.
Recordar que en este link hay un código propuesto. Que me tocó adaptar un poco a mi micro, y a mi acelerómetro.
#include <variant.h> //modifiqué este archivo para incluir STBY en el pin P0.19
// ver C:\Users\TU_USER_ACA_!!!\AppData\Local\Arduino15\packages\Seeeduino\hardware\nrf52\1.1.8\variants\Seeed_XIAO_nRF52840_Sense
//////////////////// MOTORES ////////////////////
#include <SparkFun_TB6612.h>
#define PWMA 0
#define AIN2 1
#define AIN1 2
#define STBY 33 //D33 está mapeado a P0.19 que no está conectado a nada. Lo hice porque la librería pide un STBY, y no se me ocurrió modificar la librería XD
#define BIN1 4
#define BIN2 5
#define PWMB 6
const int offsetA = 1;
const int offsetB = 1;
Motor motor0 = Motor(AIN1, AIN2, PWMA, offsetA, STBY);
Motor motor1 = Motor(BIN1, BIN2, PWMB, offsetB, STBY);
////////////////// FIN MOTORES //////////////////
// For the XIAO BLE and XIAO BLE Sense boards, pins_arduino.h
// defines LEDR, LEDG, and LEDB
#include <pins_arduino.h>
#define LED_BLUE_PIN LED_BLUE
#define LED_RED_PIN LED_RED /* Same as LED_BUILTIN */
//////////////////// ACELEROMETROS ////////////////////
//Create a instance of class LSM6DS3
#include "LSM6DS3.h"
LSM6DS3 myIMU(I2C_MODE, 0x6A); //I2C device address 0x6A
//////////////////// FIN ACELEROMETROS ////////////////////
///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001; //Covariance of gyroscope noise
float Q_gyro = 0.003; //Covariance of gyroscope drift noise
float R_angle = 0.5; //Covariance of accelerometer
char C_0 = 1;
float dt = 0.005; //The value of dt is the filter sampling time.
float K1 = 0.05; // a function containing the Kalman gain is used to calculate the deviation of the optimal estimate.
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias; //gyroscope drift
float accelz = 0;
float angle;
float angle_speed;
float angleY_one;
float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////
void setup()
{
Serial.begin(115200);
// IO GENERALES //
pinMode(LED_RED, OUTPUT);
pinMode(LED_BLUE, OUTPUT);
pinMode(LED_GREEN, OUTPUT);
// FIN IO GENERALES //
// ACELEROMETROS //
while (!Serial);
//Call .begin() to configure the IMUs
if (myIMU.begin() != 0) {
Serial.println("Device error");
} else {
Serial.println("Device OK!");
}
myIMU.settings.accelRange = 4;//Max G force readable. Can be: 2, 4, 8, 16
myIMU.settings.accelBandWidth = 50; //Hz. Can be: 50, 100, 200, 400;
myIMU.settings.accelSampleRate = 208;//Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
// FIN ACELEROMETROS //
Serial.flush(); // Waits for the transmission of outgoing serial data to complete.
}
// PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID //
///////////////////////angle parameters//////////////////////////////
float Angle;
float angle_X; //calculate the inclined angle variable of X-axis by accelerometer
float angle_Y; //calculate the inclined angle variable of Y-axis by accelerometer
float angle0 = 1; //Actual measured angle (ideally 0 degrees)
float Gyro_x,Gyro_y,Gyro_z; //Angular angular velocity for gyroscope calculation
///////////////////////angle parameters//////////////////////////////
int16_t xa, ya, za;
int16_t xg, yg, zg;
//////////////////////PID parameters///////////////////////////////
double kp = 24, ki = 0.3, kd = 0.32; //angle loop parameters
double kp_speed = 3.6, ki_speed = 0.080, kd_speed = 0; // speed loop parameters
double setp0 = 0; //angle balance point
int PD_pwm; //angle output
float pwm1=0,pwm2=0;
void DSzhongduan()
{
xa = myIMU.readRawAccelX();
ya = myIMU.readRawAccelY();
za = myIMU.readRawAccelZ();
xg = myIMU.readRawGyroX();
yg = myIMU.readRawGyroY();
zg = myIMU.readRawGyroZ();
angle_calculate(xa, ya, za, xg, yg, zg, dt, Q_angle, Q_gyro, R_angle, C_0, K1); //get angle and Kalman_Filter
PD(); // angle loop of PD control
anglePWM();
}
//////////////////angle PD////////////////////
void PD()
{
PD_pwm = kp * (angle + angle0) + kd * angle_speed; //PD angle loop control
}
void anglePWM()
{
pwm2=-PD_pwm; //The final value assigned to the motor PWM
pwm1=-PD_pwm;
if(pwm1>255) //limit PWM value not greater than 255
{
pwm1=255;
}
if(pwm1<-255)
{
pwm1=-255;
}
if(pwm2>255)
{
pwm2=255;
}
if(pwm2<-255)
{
pwm2=-255;
}
if(angle>80 || angle<-80) //When the self-balancing trolley’s tilt angle is greater than 45 degrees, the motor will stop.
{
pwm1=pwm2=0;
}
motor0.drive(pwm2);
motor1.drive(pwm1);
}
// VARIABLES GLOBALES DEL LOOP.
int incremento = 0;
int velocidad = 0;
void loop()
{
DSzhongduan();
delay(10);
}
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
Angle = -atan2(ay , az) * (180/ PI); //Radial rotation angle calculation formula; negative sign is direction processing
Gyro_x = -gx / 131; //The X-axis angular velocity calculated by the gyroscope; the negative sign is the direction processing
Kalman_Filter(Angle, Gyro_x); // Kalman Filter
//Rotation Angle Z axis parameter
Gyro_z = -gz / 131; //Z-axis angular velocity
//accelz = az / 16.4;
float angleAx = -atan2(ax, az) * (180 / PI); //Calculate the angle with the x-axis
Gyro_y = -gy / 131.00; //Y-axis angular velocity
Yiorderfilter(angleAx, Gyro_y); //first-order filter
}
////////////////////////////////////////////////////////////////
/////////////////////first-order Filter/////////////////
void Yiorderfilter(float angle_m, float gyro_m)
{
angleY_one = K1 * angle_m + (1 - K1) * (angleY_one + gyro_m * dt);
}
///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
angle += (gyro_m - q_bias) * dt; //Prior estimate
angle_err = angle_m - angle;
Pdot[0] = Q_angle - P[0][1] - P[1][0]; //Differential of azimuth error covariance
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt; //The integral of the covariance differential of the prior estimate error
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
//Intermediate variable of matrix multiplication
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
//Denominator
E = R_angle + C_0 * PCt_0;
//Gain value
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0; //Intermediate variable of matrix multiplication
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0; //Posterior estimation error covariance
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
q_bias += K_1 * angle_err; //Posterior estimation
angle_speed = gyro_m - q_bias; //The differential value of the output value; work out the optimal angular velocity
angle += K_0 * angle_err; ////Posterior estimation; work out the optimal angle
}Luego de todo esto, imprimir algunas cosas con la impresora 3D y jugar un poco con las constantes PID, llegué a esto:
Por ahora voy a considerar cerrado este tema XD aunque todavía necesito implementar la parte de control en el micro, y en el celular. En particular el micro NRF52840 sense es útil porque incluye la conectividad bluetooth también y no es necesario un periférico adicional.
Actualizaciones
Acá hay unas imágenes de las bases que usé, y un poco de la lógica de porqué las usé así:
Mi intención es que fuese algo muy simple, y sujetar todas las piezas con alguna cinta. Quise que fuera algo simple porque en el proyecto anterior modelé esto:
Y para modelar/3D-imprimir esto me tomó bastante tiempo y bastantes iteraciones. Y sentí que mi tiempo iba a estar mejor invertido si simplemente modelaba una base muy sencilla y me dedicaba a resolver otros problemas más interesantes como la comunicación, o la lógica de control.
Espero que este post le ahorre la locura a alguien.
Anibalismo 