Buenos días.
Como proyecto me propuse hacer un pequeño gimbal con un MPU6050 como sensor y dos servomotores SG90 como actuadores. He encontrado mucha documentación sobre como obtener datos de este sensor y creo tener listo todo el código, además he creado una función para el control PID de modo que este me genere la posición que debo enviar a cada servomotor.
El problema es que su comportamiento no es el adecuado, tanto sin como con el PID, genera muchas oscilaciones y se estabiliza en un angulo que no es 0 (respecto el suelo). He intentado realizar el ajuste de las ganancias del control mediante el metodo de ziegler-nichols pero no logro obtener la oscilación requerida en su primera condición, con lo cual ahora mismo mis parámetros son más bien "a ojo".
Adjunto el video de su comportamiento y el código utilizado.
#include "I2Cdev.h" // Librerias I2C para controlar el mpu6050
#include "Wire.h"
#include "MPU6050.h" // la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h
#include <Servo.h> //Libreria Servomotor
MPU6050 sensor; //Definicion del sensor
Servo servo_x; //Definicion de los servomotores
Servo servo_y;
// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;
long tiempo_prev;
float dt;
double ang_x, ang_y;
float ang_x_prev, ang_y_prev;
////////Constantes del controlador////////
double kp = 450, ki = 10, kd =6;
////////Variables calculo PID////////
double Setpoint = 0;
unsigned long currentTime, previousTime;
double elapsedTime;
double error_x, lastError_x, cumError_x, rateError_x;
double error_y, lastError_y, cumError_y, rateError_y;
void setup() {
Serial.begin(57600); //Iniciando puerto serial
Wire.begin(); //Iniciando I2C
servo_x.attach(9);
servo_x.write(90);
servo_y.attach(8);
servo_y.write(90);
delay(1000);
sensor.initialize(); //Iniciando el sensor
////////Ajuste de los OFFSETS////////
// sensor.setXGyroOffset(147);
// sensor.setYGyroOffset(0);
// sensor.setZGyroOffset(15);
// sensor.setXAccelOffset(-2751);
// sensor.setYAccelOffset(-921);
// sensor.setZAccelOffset(922);
sensor.setXGyroOffset(164);
sensor.setYGyroOffset(4);
sensor.setZGyroOffset(18);
sensor.setXAccelOffset(-2844);
sensor.setYAccelOffset(-1035);
sensor.setZAccelOffset(4683);
Serial.println("ang_y, error");
while (sensor.testConnection() != true) {
Serial.println("Error al iniciar el sensor");
}
Serial.println("Sensor iniciado correctamente");
delay(100);
}
void loop() {
////////Lectura de las aceleraciones y velocidades angulares////////
sensor.getAcceleration(&ax, &ay, &az);
sensor.getRotation(&gx, &gy, &gz);
dt = (millis() - tiempo_prev) / 1000.0;
tiempo_prev = millis();
////////Calcular los ángulos con acelerómetro////////
float accel_ang_x = atan(ay / sqrt(pow(ax, 2) + pow(az, 2))) * (180.0 / 3.14);
float accel_ang_y = atan(-ax / sqrt(pow(ay, 2) + pow(az, 2))) * (180.0 / 3.14);
////////Filtro de Kalman////////
//Calcular ángulo de rotación con giroscopio y filtro complemento
ang_x = 0.98 * (ang_x_prev + (gx / 131) * dt) + 0.02 * accel_ang_x;
ang_y = 0.98 * (ang_y_prev + (gy / 131) * dt) + 0.02 * accel_ang_y;
ang_x_prev = ang_x;
ang_y_prev = ang_y;
Serial.print(ang_y);
Serial.print(" ");
calcularPID(ang_x, ang_y); //Pasamos los ángulos a la función con los cuales calcularemos el error por referencia
////////Mostrar los valores por serial////////
// Serial.print(ang_x);
//Serial.print(" ");
// Serial.println(ang_y);
////////Movimiento de los servomotores////////
servo_x.write(90 + ang_x);
servo_y.write(90 + ang_y);
//////////////////////////////////////////
//delay(10);
}
double calcularPID(double in_x, double in_y) {
////////Calcular los tiempos indispensables////////
currentTime = millis(); // Tiempo actual
elapsedTime = (double)(currentTime - previousTime); // Tiempo transcurrido
////////Calcular el error entre referencia y medición////////
error_x = Setpoint - in_x;
error_y = Setpoint - in_y;
// error_x = in_x;
// error_y = in_y;
////////Calcular la integral del error////////
cumError_x += error_x * elapsedTime;
rateError_x = (error_x - lastError_x) / elapsedTime;
cumError_y += error_y * elapsedTime;
rateError_y = (error_y - lastError_y) / elapsedTime;
////////Calcular la salida del PID////////
double output_x = kp * error_x + ki * cumError_x + kd * rateError_x;
double output_y = kp * error_y + ki * cumError_y + kd * rateError_y;
////////Almacenar el error anterior////////
lastError_x = error_x;
lastError_y = error_y;
////////Almacenar el tiempo anterior////////
previousTime = currentTime;
////////Cambiar la variable////////
in_x = output_x;
in_y = error_y;
Serial.println(in_y);
}
Gracias de antemano.
