#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;
// Parámetros PID de estabilización angular: Ajusta estos valores según sea necesario
double KpAngular = 2.0, KiAngular = 0.0, KdAngular = 0.0;
PID pidAngular(& inputAngular, & outputAngular, & setPointAngular, KpAngular, KiAngular, KdAngular, DIRECT);
// Configuración del sensor MPU6050
Adafruit_MPU6050 mpu;