使用arduino Nano+MPU6050制作平衡火箭摆件

#include <Wire.h> #include <Servo.h> #include <I2Cdev.h> #include <MPU6050.h> #define SERVO_NUM 6 //舵机总数(原4个+云台2个) #define RESET_ANGLE 90 //复位角度（仅用于初始化，不再强制复位） #define SMOOTH_STEP 3 //基础步进值 #define CLOUD_SMOOTH_STEP 8 // 增大云台步进值(原5) //云台步进值(更大以提高响应速度) #define THRESHOLD 500 //静止阈值 #define SENSITIVITY 12.0 // 增大基础灵敏度以平衡两个方向 #define CLOUD_SENSITIVITY 12.0 // 调整云台灵敏度以平衡两个方向 #define ANGLE_MIN 20 //最小安全角度 #define ANGLE_MAX 160 //最大安全角度 #define PITCH_MIN 0 // 新增Pitch舵机最小角度限制 #define PITCH_MAX 80 // 新增Pitch舵机最大角度限制 #define FILTER_ALPHA 0.2 //低通滤波系数 //基础PID参数 #define KP 0.8 #define KI 0.05 #define KD 0.01 //云台专用PID参数(更激进) #define CLOUD_KP 1.8 // 增大比例系数(原1.2) #define CLOUD_KI 0.15 // 增大积分系数(原0.1) #define CLOUD_KD 0.08 // 增大微分系数(原0.05) MPU6050 mpu; Servo servos[SERVO_NUM]; const int servoPins[SERVO_NUM] = {9, 10, 11, 12, 5, 6}; //新增云台舵机接D5,D6 //传感器数据 int16_t ax, ay, az; int16_t gx, gy, gz; float filteredGx = 0, filteredGy = 0; //舵机角度状态 int currentAngles[SERVO_NUM] = {RESET_ANGLE, RESET_ANGLE, RESET_ANGLE, RESET_ANGLE, RESET_ANGLE, RESET_ANGLE}; //基础PID变量 float errors[SERVO_NUM] = {0}; float integrals[SERVO_NUM] = {0}; float derivatives[SERVO_NUM] = {0}; float lastErrors[SERVO_NUM] = {0}; //云台专用PID变量 float cloudErrors[2] = {0}; //云台Yaw/Pitch误差 float cloudIntegrals[2] = {0}; float cloudDerivatives[2] = {0}; float cloudLastErrors[2] = {0}; void setup() { Wire.begin(); Serial.begin(115200); //初始化所有舵机 for(int i = 0; i < SERVO_NUM; i++) { servos[i].attach(servoPins[i]); servos[i].write(RESET_ANGLE); delay(100); } //初始化MPU6050 mpu.initialize(); if(!mpu.testConnection()) { Serial.println("MPU6050连接失败"); } mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000); delay(1000); } //低通滤波函数 float lowPassFilter(float current, float last, float alpha) { return alpha * current + (1 - alpha) * last; } //基础舵机PID控制（圆形对称布局） void baseServoControl(int motion) { if(motion < THRESHOLD) { // 静止状态：完全停止PID控制，不修改currentAngles // 仅保持当前角度，不做任何调整 for(int i = 0; i < 4; i++) { servos[i].write(currentAngles[i]); // 确保舵机保持在当前角度 } } else { // 计算基础角度（基于X轴陀螺仪数据） int baseAngle = constrain( map(filteredGx * SENSITIVITY, -32768, 32767, ANGLE_MIN, ANGLE_MAX), 0, 180 ); // 检测长边方向（Y轴）的摆动幅度 int motionY = abs(filteredGy); float sensitivityMultiplier = 1.0; // 默认灵敏度 // 如果Y轴摆动超过阈值的一半，直接设置最大灵敏度 if (motionY > THRESHOLD / 2) { sensitivityMultiplier = 2.0; // 最大增大幅度 } // 四个舵机的目标角度（90度对称布局） int targetAngles[4] = { baseAngle, // 0度位置舵机（引脚9） constrain(180 - baseAngle, 0, 180), // 180度位置舵机（引脚10，反向） baseAngle, // 90度位置舵机（引脚11） constrain(180 - baseAngle, 0, 180) // 270度位置舵机（引脚12，反向） }; // 应用PID控制 for(int i = 0; i < 4; i++) { errors[i] = targetAngles[i] - currentAngles[i]; integrals[i] += errors[i]; derivatives[i] = errors[i] - lastErrors[i]; float pidOutput = KP * errors[i] + KI * integrals[i] + KD * derivatives[i]; // 根据灵敏度乘数调整步进值 int increment = constrain((int)(pidOutput * sensitivityMultiplier), -SMOOTH_STEP, SMOOTH_STEP); currentAngles[i] += increment; // 确保角度在安全范围内 currentAngles[i] = constrain(currentAngles[i], ANGLE_MIN, ANGLE_MAX); servos[i].write(currentAngles[i]); lastErrors[i] = errors[i]; } } } //云台专用PID控制（修改后不自动复位，且限制Pitch舵机角度） void cloudServoControl() { //云台目标角度(Yaw:舵机5, Pitch:舵机6) int cloudTargets[2] = { constrain(map(filteredGx * CLOUD_SENSITIVITY, -32768, 32767, ANGLE_MIN, ANGLE_MAX), 0, 180), // Yaw constrain(map(filteredGy * CLOUD_SENSITIVITY, -32768, 32767, PITCH_MIN, PITCH_MAX), 0, 180) // Pitch (限制在0-30度) }; //云台PID计算 for(int i = 0; i < 2; i++) { int servoIndex = i + 4; //舵机5和6 cloudErrors[i] = cloudTargets[i] - currentAngles[servoIndex]; cloudIntegrals[i] += cloudErrors[i]; cloudDerivatives[i] = cloudErrors[i] - cloudLastErrors[i]; float pidOutput = (i == 0) ? // Yaw舵机使用原始参数 (CLOUD_KP * cloudErrors[i] + CLOUD_KI * cloudIntegrals[i] + CLOUD_KD * cloudDerivatives[i]) : (2*CLOUD_KP * cloudErrors[i] + 2*CLOUD_KI * cloudIntegrals[i] + 2*CLOUD_KD * cloudDerivatives[i]); // 这里只是示例，实际应使用单独的Pitch参数 // 使用正确的参数计算（修正上面的错误） pidOutput = CLOUD_KP * cloudErrors[i] + CLOUD_KI * cloudIntegrals[i] + CLOUD_KD * cloudDerivatives[i]; // 对于Pitch舵机(索引1)，使用更小的步进值和不同的参数范围 if(i == 1) { // Pitch舵机 pidOutput = CLOUD_KP * 0.5 * cloudErrors[i] + CLOUD_KI * 0.5 * cloudIntegrals[i] + CLOUD_KD * 0.5 * cloudDerivatives[i]; // 减小参数影响 int increment = constrain((int)pidOutput, -CLOUD_SMOOTH_STEP/2, CLOUD_SMOOTH_STEP/2); // 更小的步进 currentAngles[servoIndex] += increment; currentAngles[servoIndex] = constrain(currentAngles[servoIndex], PITCH_MIN, PITCH_MAX); // 限制在0-30度 } else { // Yaw舵机 int increment = constrain((int)pidOutput, -CLOUD_SMOOTH_STEP, CLOUD_SMOOTH_STEP); currentAngles[servoIndex] += increment; currentAngles[servoIndex] = constrain(currentAngles[servoIndex], ANGLE_MIN, ANGLE_MAX); } servos[servoIndex].write(currentAngles[servoIndex]); cloudLastErrors[i] = cloudErrors[i]; } // 移除以下代码（如果存在）以确保舵机不自动复位 // for(int i = 4; i < 6; i++) { // currentAngles[i] = RESET_ANGLE; // 这行代码会导致舵机复位，需要移除 // } } void loop() { //读取并滤波陀螺仪数据 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); filteredGx = lowPassFilter(gx, filteredGx, FILTER_ALPHA); filteredGy = lowPassFilter(gy, filteredGy, FILTER_ALPHA); //计算运动状态 int motion = abs(filteredGx) + abs(filteredGy); //控制基础舵机(0-3) baseServoControl(motion); //控制云台舵机(4-5) cloudServoControl(); //调试输出 Serial.print("Base Servos: "); for(int i = 0; i < 4; i++) { Serial.print(currentAngles[i]); Serial.print(" "); } Serial.print("| Cloud Yaw:"); Serial.print(currentAngles[4]); Serial.print(" Pitch:"); Serial.println(currentAngles[5]); delay(20); //控制周期 }