ESP32硬件，arduino架构，双螺旋桨玩具船，无舵机，写一下双级PID代码，第一级是角度环，第二级是角速度环，角度环和角速度参数环由MPU6050芯片反馈，玩具船的角速度由双螺旋桨转速差控制

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// PID参数
float Kp_angle = 1.0;
float Ki_angle = 0.0;
float Kd_angle = 0.0;

float Kp_rate = 1.0;
float Ki_rate = 0.0;
float Kd_rate = 0.0;

// 目标角度和角速度
float target_angle = 0.0;
float target_rate = 0.0;

// PID变量
float angle_error = 0.0;
float angle_integral = 0.0;
float angle_derivative = 0.0;

float rate_error = 0.0;
float rate_integral = 0.0;
float rate_derivative = 0.0;

// 控制信号
float control_signal = 0.0;

// 初始化函数
void setup() {
Wire.begin();
mpu.initialize();
}

// 主循环
void loop() {
// 读取加速度和角速度
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

// 计算角度和角速度
float angle = atan2(ay, az) * 180.0 / PI;
float rate = gx / 131.0;

// 计算角度误差
angle_error = target_angle - angle;

// 计算角速度误差
rate_error = target_rate - rate;

// 更新角度积分项
angle_integral += angle_error;

// 更新角速度积分项
rate_integral += rate_error;

// 计算角度微分项
angle_derivative = angle_error - angle_previous_error;
angle_previous_error = angle_error;

// 计算角速度微分项
rate_derivative = rate_error - rate_previous_error;
rate_previous_error = rate_error;

// 计算控制信号
control_signal = Kp_angle * angle_error + Ki_angle * angle_integral + Kd_angle * angle_derivative +
Kp_rate * rate_error + Ki_rate * rate_integral + Kd_rate * rate_derivative;

// 限制控制信号范围
if (control_signal > 100) {
control_signal = 100;
} else if (control_signal < -100) {
control_signal = -100;
}

// 根据控制信号调整螺旋桨转速差

// 延时一段时间
delay(20);
}

#### 引用[.reference_title]
- *1* *2* [ESP32使用Arduino驱动180 360度舵机](https://blog.csdn.net/weixin_42854045/article/details/127603120)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^control_2,239^v3^insert_chatgpt"}} ] [.reference_item]
- *3* [ESP32上手笔记 | 02 - ESP32-Arduino开发环境搭建](https://blog.csdn.net/Mculover666/article/details/123486323)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^control_2,239^v3^insert_chatgpt"}} ] [.reference_item]
[ .reference_list ]