#include < Wire.h >
#include < PID_v1.h >
#include < Adafruit_Sensor.h >
#include < Adafruit_MPU6050.h >
#include < Adafruit_MS_PWMServoDriver.h >
// Definición de pines
const int PIN_ULTRASONIC_TRIGGER = 2;
const int PIN_ULTRASONIC_ECHO = 3;
// Variables para el PID de altura
double setPointAltura = 50.0; // Altura deseada en centímetros
double inputAltura, outputAltura;
// Parámetros PID de altura: Ajusta estos valores según sea necesario
double KpAltura = 2.0, KiAltura = 0.0, KdAltura = 0.0;
PID pidAltura(& inputAltura, & outputAltura, & setPointAltura, KpAltura, KiAltura, KdAltura, DIRECT);
// Variables para el PID de estabilización angular
double setPointAngular = 0.0; // Ángulo deseado en grados
double inputAngular, outputAngular;