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();

// 设置PID参数和目标值
// ...

// 设置舵机引脚和舵机库
// ...
}

// 主循环
void loop() {
// 读取MPU6050数据
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;
angle_integral += angle_error;

// 计算角速度误差和积分项
rate_error = target_rate - rate;
rate_integral += rate_error;

// 计算角度和角速度的微分项
angle_derivative = angle - previous_angle;
rate_derivative = rate - previous_rate;

// 计算PID控制量
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;

// 输出控制量到舵机或双螺旋桨转速差控制
// ...

// 更新上一次的角度和角速度
previous_angle = angle;
previous_rate = rate;

// 延时
delay(20);
}

#### 引用[.reference_title]
- *1* *3* [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]
- *2* [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 ]