ESP32 旋转倒立摆 SimpleFOC

整体架构分层：宏定义层 → 全局变量层 → 硬件实例层 → 串口指令回调层 → 工具函数层 → 初始化 Setup 层 → 主循环 Loop 控制层，覆盖每一行代码功能、原理、设计目的、工程意义。

                  编制：刘平安                                           Email: 185286490@qq.com

一、头文件与基础数学宏、符号函数

#include <SimpleFOC.h>

#include <math.h>

1. SimpleFOC.h：无刷电机 FOC 矢量控制开源库，实现 BLDC 磁场定向控制、电流 / 电压 / 速度闭环、磁编码器读取、3 路 PWM 驱动输出。
2. math.h：浮点数学库，提供fmodf、fabs、constrain等浮点运算函数。

圆周常数宏

#ifndef M_PI

#define M_PI 3.14159265358979323846

#endif

#ifndef _2PI

#define _2PI (2.0f * M_PI)

#endif

• 原理：部分 ESP32 Arduino 内核未默认定义圆周率M_PI，预编译判断缺失则手动定义，避免编译报错。
• _2PI = 2π，用于角度归一化、周期运算，提前宏定义减少重复计算。

符号函数 signf()

static inline float signf(float x) {

    if (x > 1e-6f) return 1.0f;

    if (x < -1e-6f) return -1.0f;

    return 0.0f;

}

1. static inline：内联函数，无函数调用开销，高速控制循环专用。
2. 功能：返回浮点数符号
   • x > 极小阈值 1e-6 → 正 1
   • x < -1e-6 → 负 1
   • 区间内视为 0（浮点噪声过滤）
3. 工程作用：电压限幅时保持输出极性，防止微小噪声造成频繁换向抖动。
4. 阈值 1e-6：规避浮点数计算误差，不直接用x>0。

二、硬件 & 参数宏定义（分模块解析）

1. 电机硬件参数

#define MOTOR_POLE_PAIRS    7        // 电机极对数

#define PWM_U 32

#define PWM_V 33

#define PWM_W 25

#define DRIVER_EN 22                 // 驱动使能引脚

#define SUPPLY_VOLTAGE      16.8f    // 母线供电电压（6串锂电池16.8V满电）

#define FOC_VOLT_LIMIT      16.8f    // FOC最大输出电压上限

#define LQR_OUT_SCALE       0.7f     // LQR稳摆输出电压缩放系数

#define SWING_UP_SCALE      0.8f     // 起摆输出电压缩放系数

• 极对数：SimpleFOC 做电角度换算、霍尔 / 磁编码器校准必须参数；
• PWM UVW：三相无刷电机正弦波输出引脚；
• DRIVER_EN：驱动芯片使能，低电平关闭驱动，硬件安全切断功率输出；
• 电压缩放系数：防止控制输出满电压冲击机械结构，LQR 稳摆更温和，起摆允许更大出力。

2. 双路 I2C 磁编码器（AS5600）

#define I2C_MOTOR_SDA 19

#define I2C_MOTOR_SCL 18    // 电机轴编码器I2C0

#define I2C_PEND_SDA  23

#define I2C_PEND_SCL  5     // 摆杆编码器I2C1

#define I2C_FREQ      400000UL  // I2C高速400kHz

• ESP32 硬件双 I2C 总线，电机、摆杆编码器物理分离，互不干扰；
• 400kHz 高速 I2C：提升编码器读取刷新率，满足倒立摆高频控制需求。

3. 控制逻辑阈值、周期参数

#define STABLE_ANGLE_THRESH 0.5f    // 弧度，区分起摆/稳摆分界角度

#define LOOP_CTRL_PERIOD    25      // 每25次loop执行一次控制计算

#define PEND_ANG_OFFSET     3.83f   // 摆杆机械零点偏移校准值

#define SERIAL_BAUD         115200  // 串口波特率

#define PRINT_PERIOD_MS     100     // 100ms打印一次数据，减少串口阻塞

• STABLE_ANGLE_THRESH：摆杆倾角绝对值＜0.5rad 执行 LQR 直立稳摆；大于则执行起摆算法；
• LOOP_CTRL_PERIOD：分频控制周期，降低 CPU 占用，固定控制刷新率；
• PEND_ANG_OFFSET：机械装配误差，编码器零点和摆杆垂直零点不重合，静态校准偏移量；

4. 滤波、安全、死区参数

#define FILTER_TAU_PEND 0.02f       // 摆杆角速度一阶低通滤波时间常数

#define FILTER_TAU_MOTOR 0.01f      // 电机转速滤波时间常数

#define PEND_SAFE_ANGLE 1.2f        // 安全保护阈值，超过直接停机

#define ANGLE_DEAD_ZONE 0.03f       // 摆角死区，微小角度不输出控制电压

• 时间常数 τ：一阶低通滤波核心，τ 越大滤波越强、延迟越高；电机转速噪声小，τ 更小；摆杆振动噪声大，τ 更大；
• PEND_SAFE_ANGLE：机械限位保护，摆杆倾倒过大断电保护，防止飞车；
• ANGLE_DEAD_ZONE：消除直立时微小编码器噪声带来高频抖动，小角度直接置 0 输入。

三、全局状态变量

1. 时序、标志、输出变量

uint16_t loop_cnt = 0;                 // loop循环计数器，控制分频

float target_voltage = 0.0f;           // FOC输出目标电压（控制输出量）

uint32_t last_print_time = 0;          // 上一次串口打印时间戳

volatile bool motor_enable_flag = false;// 电机总使能标志，volatile防止编译器优化

uint32_t last_ctrl_time = 0;           // 上一次控制计算时间戳，用于计算dt

• volatile：变量会在中断 / 循环频繁读写，强制 CPU 每次读取内存，不缓存寄存器；
• dt：两次控制循环时间间隔，滤波、微分计算必备。

2. LQR 控制增益与校准偏移

float gain_angle     = 60.0f;    // 摆角度误差增益 kθ

float gain_pend_vel  = 3.0f;     // 摆杆角速度增益 kθd

float gain_motor_vel = 0.18f;    // 电机转速阻尼增益 kωm

float total_angle_offset = 0.0f; // 动态摆角总偏移，串口可调校准

标准倒立摆 LQR 状态反馈控制律：

\(u = -k_\theta \cdot \theta -k_{\dot\theta}\cdot\dot\theta -k_{\omega}\cdot\omega_m\)

代码中符号根据机械极性做了适配。

3. 滤波缓存变量（一阶低通状态）

float filt_pend_vel = 0.0f;

float filt_motor_vel = 0.0f;

一阶低通滤波公式：

\(y_{k} = y_{k-1} + \frac{dt}{\tau+dt}(x_k - y_{k-1})\)

filt_xxx存储上一时刻滤波输出\(y_{k-1}\)。

4. 硬件对象实例

BLDCMotor motor = BLDCMotor(MOTOR_POLE_PAIRS);          // 无刷电机FOC控制对象

BLDCDriver3PWM driver = BLDCDriver3PWM(PWM_U, PWM_V, PWM_W, DRIVER_EN); // 3路PWM驱动

MagneticSensorI2C motor_sensor = MagneticSensorI2C(AS5600_I2C); // 电机轴AS5600编码器

MagneticSensorI2C pendulum_sensor = MagneticSensorI2C(AS5600_I2C); // 摆杆AS5600

TwoWire wire_motor = TwoWire(0);  // I2C0硬件总线

TwoWire wire_pend = TwoWire(1);   // I2C1硬件总线

Commander cmd = Commander(Serial); // SimpleFOC串口指令解析器

• BLDCMotor：封装 FOC 核心算法、角度读取、转矩 / 速度闭环；
• BLDCDriver3PWM：输出三路互补 PWM 控制三相桥；
• MagneticSensorI2C：读取 AS5600 磁编码器原始角度、转速；
• Commander：SimpleFOC 内置串口命令解析，无需手动解析字符串，支持浮点参数读写。

四、串口指令回调函数（每条指令功能 + 原理）

Commander 机制：串口收到指令字符自动触发对应回调，buf携带传入浮点参数。

1. 调参指令 A / P / M / T

void cmdGainAngle(char* buf){

  cmd.scalar(&gain_angle, buf);

  Serial.print("【参数】摆角增益 A = "); Serial.println(gain_angle);

}

• 指令：A60
• cmd.scalar(变量指针, buf)：自动把串口字符串转浮点数写入变量；
• 功能：实时修改 LQR 摆角反馈增益，增益越大直立刚度越强，过大会振荡；

同理：

• cmdGainPendVel：指令P3 → 修改摆角速度阻尼增益，抑制摆杆摆动；
• cmdGainMotorVel：指令M0.18 → 电机转速阻尼，抑制电机来回震荡；
• cmdTotalOffset：指令T3.83 → 动态补偿摆杆机械零点偏移，解决直立偏置；

2. 停机指令 cmdMotorOff 指令0

void cmdMotorOff(char* buf){

  motor_enable_flag = false;

  driver.disable();

  digitalWrite(DRIVER_EN, LOW);

  motor.move(0);

  target_voltage = 0;

  Serial.println("【保护】电机驱动已关闭");

}

执行流程：

1. 全局使能标志置 0；
2. SimpleFOC 驱动软件关闭 PWM 输出；
3. 拉低硬件使能引脚，硬件切断功率管；
4. 电机输出电压置 0，清空控制量；

硬件 + 软件双重保护停机。

3. 开机使能 cmdMotorOn 指令1

void cmdMotorOn(char* buf){

  float pend_raw_ang = pendulum_sensor.getAngle();

  float pend_angle_raw = pend_raw_ang - PEND_ANG_OFFSET + total_angle_offset;

  float pend_angle = constrainAngle(pend_angle_raw);

  if(fabs(pend_angle) > STABLE_ANGLE_THRESH * 1.5f){

    Serial.println("【警告】摆杆倾角过大，禁止启动电机！先手动扶正摆杆");

    return;

  }

  digitalWrite(DRIVER_EN, HIGH);

  driver.enable();

  motor_enable_flag = true;

  last_ctrl_time = millis();

  Serial.println("【运行】电机驱动已使能，启动稳摆控制");

}

安全启动逻辑：

1. 读取当前摆杆角度并校准归一化；
2. 判断摆杆倾角不能过大，防止上电直接飞车；
3. 拉高硬件使能、开启 PWM 驱动、置位使能标志；
4. 刷新控制时间戳，保证第一次 dt 计算准确。

4. 偏移清零 cmdResetOffset 指令R

void cmdResetOffset(char* buf){

  total_angle_offset = 0;

  Serial.println("【校准】全局摆角偏移清零 T=0");

}

快速重置摆杆角度补偿，用于重新静态标定零点。

五、核心工具函数（控制算法核心）

1. 角度归一化 constrainAngle(float x)

float constrainAngle(float x){

  x = fmodf(x, _2PI);

  if (x > M_PI) x -= _2PI;

  else if (x < -M_PI) x += _2PI;

  return x;

}

原理：AS5600 输出 0~2π 连续角度，倒立摆控制需要 **-π ~ +π** 对称区间（左右摆动正负区分）。

1. fmodf(x,2π) 将任意角度折叠到 0~2π；
2. 大于 π 减去 2π 得到负角度，小于 -π 加 2π；

作用：摆杆左右摆动角度连续无跳变，控制不会出现角度跳变冲击。

2. LQR 直立稳摆控制器 lqrController()

float lqrController(float p_angle, float p_vel, float m_vel){

  if(fabs(p_angle) < ANGLE_DEAD_ZONE) p_angle = 0;

  float u = gain_angle * p_angle + gain_pend_vel * p_vel + gain_motor_vel * m_vel;

  float max_out = SUPPLY_VOLTAGE * LQR_OUT_SCALE;

  if(fabs(u) > max_out) u = signf(u) * max_out;

  return u;

}

1. 死区处理：微小摆角直接置 0，消除高频抖动；
2. 状态反馈控制律：

输入三状态：摆杆角度 θ、摆杆角速度\(\dot\theta\)、电机旋转速度\(\omega_m\)；

线性加权求和得到控制电压 u；

1. 限幅：按缩放系数限制最大输出电压，用signf保留转向极性，软限幅无突变。

3. 起摆控制器 swingUpController()

float swingUpController(float p_angle, float p_vel, float m_vel){

  const float swing_kp = 13.0f;

  const float swing_kd_p = 1.3f;

  const float swing_kd_m = 0.35f;

  float swing_feed = p_angle * swing_kp;

  float swing_damp = p_vel * swing_kd_p;

  float swing_motor_damp = m_vel * swing_kd_m;

  float u = swing_feed - swing_damp - swing_motor_damp;

  float swing_max = SUPPLY_VOLTAGE * SWING_UP_SCALE;

  if(fabs(u) > swing_max) u = signf(u) * swing_max;

  return u;

}

能量起摆原理：

1. 角度项swing_feed：根据摆杆倾斜方向持续给电机激励，给摆杆持续注入摆动能量；
2. 阻尼项：减去摆杆自身速度、电机转速，抑制超调，防止摆动幅度无限增大；
3. 更大电压输出缩放系数 0.8，相比稳摆允许更大出力把摆杆甩到直立区域；
4. 当摆杆进入 ±0.5rad 区间，自动切换 LQR 稳摆。

六、Setup 初始化函数 完整流程解析

void setup() {

  Serial.begin(SERIAL_BAUD);

  delay(600);

  Serial.println("===== ESP32 旋转倒立摆 SimpleFOC 优化修复版 =====");

串口初始化，延时等待硬件上电稳定，打印版本信息。

pinMode(DRIVER_EN, OUTPUT);

digitalWrite(DRIVER_EN, LOW);

驱动使能引脚设为输出，默认低电平关闭驱动，上电安全状态。

wire_motor.begin(I2C_MOTOR_SDA, I2C_MOTOR_SCL, I2C_FREQ);

wire_pend.begin(I2C_PEND_SDA, I2C_PEND_SCL, I2C_FREQ);

motor_sensor.init(&wire_motor);

pendulum_sensor.init(&wire_pend);

Serial.println("双路AS5600磁编码器初始化完成");

两路硬件 I2C 分别初始化电机、摆杆编码器，绑定对应总线。

motor.controller = MotionControlType::torque;

motor.linkSensor(&motor_sensor);

电机控制模式：转矩电压模式，直接输出电压等效控制输出转矩，适合倒立摆直接 LQR 电压输出；绑定电机编码器用于 FOC 电角度解算。

driver.voltage_power_supply = SUPPLY_VOLTAGE;

driver.voltage_limit = FOC_VOLT_LIMIT;

driver.init();

motor.linkDriver(&driver);

配置驱动母线电压、输出限幅，初始化三路 PWM，电机绑定驱动硬件。

Serial.println("=== 电机FOC自动校准，请保持电机完全静止 ===");

motor.init();

motor.initFOC();

Serial.println("FOC相序、零点校准全部完成！");

SimpleFOC 核心校准步骤：

1. motor.init()：检测电机绕组、电阻基础检测；
2. motor.initFOC()：电机堵转校准电角度零点、三相相序，必须保持电机静止，否则 FOC 换向错误无力、抖动。

cmd.add('A', cmdGainAngle, "LQR摆角增益 A+数值");

cmd.add('P', cmdGainPendVel, "LQR摆角速度增益 P+数值");

cmd.add('M', cmdGainMotorVel, "LQR电机转速增益 M+数值");

cmd.add('T', cmdTotalOffset, "摆杆总角度偏移 T+数值");

cmd.add('0', cmdMotorOff, "关闭电机驱动 0");

cmd.add('1', cmdMotorOn, "使能电机驱动 1");

cmd.add('R', cmdResetOffset, "重置摆角偏移 R");

注册全部串口指令：字符、回调函数、帮助注释，串口输入对应字符自动执行函数。

Serial.println("\n==== 使用说明 ====");

// 打印操作提示

motor_enable_flag = false;

last_ctrl_time = millis();

last_print_time = millis();

}

打印操作手册，初始化所有时序时间戳，上电默认电机关闭。

七、Loop 主循环 实时控制逻辑逐段解析

loop 是 ESP32 无限循环，分为硬件刷新层 → 分频控制层 → 安全保护层 → 滤波计算层 → 控制算法切换层 → 输出驱动层 → 串口打印层。

1. 底层硬件实时刷新（每 loop 都执行）

motor_sensor.update();

pendulum_sensor.update();

motor.loopFOC();

cmd.run();

1. 双编码器刷新读取最新角度、转速；
2. motor.loopFOC()：高频刷新 FOC 正弦 PWM 输出，必须持续调用，否则电机失步；
3. cmd.run()：非阻塞轮询串口，检测是否收到调参指令，无数据不阻塞控制。

2. 分频定时控制（核心控制仅每 25 次 loop 运行一次）

if(loop_cnt >= LOOP_CTRL_PERIOD){

  loop_cnt = 0;

  uint32_t now = millis();

  float dt = (now - last_ctrl_time) / 1000.0f;

  last_ctrl_time = now;

  dt = constrain(dt, 1e-5f, 0.05f);

• loop_cnt 循环计数，达到 25 才进入控制计算，降低控制频率减轻 CPU 负载；
• dt = 本次与上次控制间隔，单位秒，用于一阶滤波微分计算；
• dt 限幅：防止卡死、延时过长导致 dt 突变，滤波发散。

3. 摆杆角度读取与校准归一化

float pend_raw_ang = pendulum_sensor.getAngle();

float pend_angle_raw = pend_raw_ang - PEND_ANG_OFFSET + total_angle_offset;

float pend_angle = constrainAngle(pend_angle_raw);

流程：原始编码器角度 → 减去固定机械偏移 + 串口动态偏移 → 归一化到 [-π,π]。

4. 机械安全保护逻辑

bool safe_flag = true;

if(fabs(pend_angle) > PEND_SAFE_ANGLE){

  motor_enable_flag = false;

  driver.disable();

  digitalWrite(DRIVER_EN, LOW);

  motor.move(0);

  target_voltage = 0;

  safe_flag = false;

  Serial.println("【安全保护触发】摆杆角度超限，电机紧急停机");

}

摆杆倾角超过安全阈值，自动切断软硬件驱动，停机防碰撞、飞车。

5. 一阶低通滤波（仅安全状态执行）

float raw_pend_vel = pendulum_sensor.getVelocity();

float raw_motor_vel = motor.shaftVelocity();

filt_pend_vel = filt_pend_vel + dt / (FILTER_TAU_PEND + dt) * (raw_pend_vel - filt_pend_vel);

filt_motor_vel = filt_motor_vel + dt / (FILTER_TAU_MOTOR + dt) * (raw_motor_vel - filt_motor_vel);

一阶低通离散实现：平滑编码器转速噪声，抑制微分噪声带来控制抖动；时间常数 τ 越大平滑效果越强。

6. 控制算法自动切换逻辑

if(fabs(pend_angle) < STABLE_ANGLE_THRESH){

  target_voltage = lqrController(pend_angle, filt_pend_vel, filt_motor_vel);

}else{

  target_voltage = swingUpController(pend_angle, filt_pend_vel, filt_motor_vel);

}

• 摆杆接近直立（|θ|<0.5rad）：LQR 线性稳摆；
• 摆杆倾倒大角度：起摆能量控制，自动过渡无手动切换。

7. 电压输出执行

if(motor_enable_flag){

  motor.move(target_voltage);

}else{

  motor.move(0.0f);

}

motor.move(u)：SimpleFOC 转矩模式下输入电压值，输出对应幅值三相正弦 PWM；电机关闭则输出 0 电压。

8. 非阻塞定时串口打印

if(millis() - last_print_time > PRINT_PERIOD_MS){

  last_print_time = millis();

  Serial.print("θ:");Serial.print(pend_angle,3);

  Serial.print(" θd:");Serial.print(filt_pend_vel,2);

  Serial.print(" ωm:");Serial.print(filt_motor_vel,2);

  Serial.print(" U:");Serial.println(target_voltage,2);

}

每 100ms 打印一组状态：摆角 θ、摆角速度 θd、电机转速 ωm、输出控制电压 U；非阻塞 millis 计时，不占用控制周期。

9. 循环计数器自增

loop_cnt++;

分频计数器累加，完成单次 loop 迭代。

整体系统工作流程总结

1. 上电 Setup：初始化双 I2C 编码器、无刷电机 FOC 校准、串口指令注册，默认电机关闭；
2. 主循环高频刷新编码器与 FOC 底层；
3. 每 25 次 loop 执行一次离散控制计算：

读取摆杆角度 → 安全判断 → 转速滤波 → 自动切换起摆 / LQR 控制 → 输出电压；

1. 串口实时支持在线调增益、校准零点、启停电机；
2. 多重保护：上电倾角检测、摆角超限紧急停机、硬件使能引脚切断驱动、输出电压限幅；
3. 滤波 + 角度死区双重抑制机械抖动，分段控制实现自动起摆 + 直立稳定。

#include <SimpleFOC.h> #include <math.h> // ========== 数学宏补全 + 符号函数 ========== #ifndef M_PI #define M_PI 3.14159265358979323846 #endif #ifndef _2PI #define _2PI (2.0f * M_PI) #endif static inline float signf(float x) { if (x > 1e-6f) return 1.0f; if (x < -1e-6f) return -1.0f; return 0.0f; } // ====================== 硬件&参数宏定义 ====================== #define MOTOR_POLE_PAIRS 7 #define PWM_U 32 #define PWM_V 33 #define PWM_W 25 #define DRIVER_EN 22 #define SUPPLY_VOLTAGE 16.8f #define FOC_VOLT_LIMIT 16.8f #define LQR_OUT_SCALE 0.7f #define SWING_UP_SCALE 0.8f #define I2C_MOTOR_SDA 19 #define I2C_MOTOR_SCL 18 #define I2C_PEND_SDA 23 #define I2C_PEND_SCL 5 #define I2C_FREQ 400000UL #define STABLE_ANGLE_THRESH 0.5f #define LOOP_CTRL_PERIOD 25 #define PEND_ANG_OFFSET 3.83f #define SERIAL_BAUD 115200 #define PRINT_PERIOD_MS 100 #define FILTER_TAU_PEND 0.02f #define FILTER_TAU_MOTOR 0.01f #define PEND_SAFE_ANGLE 1.2f #define ANGLE_DEAD_ZONE 0.03f // 摆角死区，抑制微小抖动 // ====================== 全局状态变量 ====================== uint16_t loop_cnt = 0; float target_voltage = 0.0f; uint32_t last_print_time = 0; volatile bool motor_enable_flag = false; uint32_t last_ctrl_time = 0; // LQR增益 float gain_angle = 60.0f; float gain_pend_vel = 3.0f; float gain_motor_vel = 0.18f; float total_angle_offset = 0.0f; // 滤波缓存 float filt_pend_vel = 0.0f; float filt_motor_vel = 0.0f; // 硬件实例 BLDCMotor motor = BLDCMotor(MOTOR_POLE_PAIRS); BLDCDriver3PWM driver = BLDCDriver3PWM(PWM_U, PWM_V, PWM_W, DRIVER_EN); MagneticSensorI2C motor_sensor = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C pendulum_sensor = MagneticSensorI2C(AS5600_I2C); TwoWire wire_motor = TwoWire(0); TwoWire wire_pend = TwoWire(1); Commander cmd = Commander(Serial); // ====================== 串口指令回调 ====================== void cmdGainAngle(char* buf){ cmd.scalar(&gain_angle, buf); Serial.print("【参数】摆角增益 A = "); Serial.println(gain_angle); } void cmdGainPendVel(char* buf){ cmd.scalar(&gain_pend_vel, buf); Serial.print("【参数】摆角速度增益 P = "); Serial.println(gain_pend_vel); } void cmdGainMotorVel(char* buf){ cmd.scalar(&gain_motor_vel, buf); Serial.print("【参数】电机转速增益 M = "); Serial.println(gain_motor_vel); } void cmdTotalOffset(char* buf){ cmd.scalar(&total_angle_offset, buf); Serial.print("【参数】摆角总偏移 T = "); Serial.println(total_angle_offset); } void cmdMotorOff(char* buf){ motor_enable_flag = false; driver.disable(); digitalWrite(DRIVER_EN, LOW); motor.move(0); target_voltage = 0; Serial.println("【保护】电机驱动已关闭"); } void cmdMotorOn(char* buf){ // 上电安全校验：仅摆角在安全区间才允许使能 float pend_raw_ang = pendulum_sensor.getAngle(); float pend_angle_raw = pend_raw_ang - PEND_ANG_OFFSET + total_angle_offset; float pend_angle = constrainAngle(pend_angle_raw); if(fabs(pend_angle) > STABLE_ANGLE_THRESH * 1.5f){ Serial.println("【警告】摆杆倾角过大，禁止启动电机！先手动扶正摆杆"); return; } digitalWrite(DRIVER_EN, HIGH); driver.enable(); motor_enable_flag = true; last_ctrl_time = millis(); Serial.println("【运行】电机驱动已使能，启动稳摆控制"); } void cmdResetOffset(char* buf){ total_angle_offset = 0; Serial.println("【校准】全局摆角偏移清零 T=0"); } // ====================== 工具函数 ====================== // 优化角度归一化 [-π, π] float constrainAngle(float x){ x = fmodf(x, _2PI); if (x > M_PI) x -= _2PI; else if (x < -M_PI) x += _2PI; return x; } // LQR控制律 float lqrController(float p_angle, float p_vel, float m_vel){ // 死区抑制微小抖动 if(fabs(p_angle) < ANGLE_DEAD_ZONE) p_angle = 0; float u = gain_angle * p_angle + gain_pend_vel * p_vel + gain_motor_vel * m_vel; float max_out = SUPPLY_VOLTAGE * LQR_OUT_SCALE; // 软限幅（渐变限制，无突变） if(fabs(u) > max_out) u = signf(u) * max_out; return u; } // 起摆控制器（增加极性可调系数，方便机械适配） float swingUpController(float p_angle, float p_vel, float m_vel){ const float swing_kp = 13.0f; // 起摆角度系数，可根据机械微调 const float swing_kd_p = 1.3f; // 摆杆阻尼 const float swing_kd_m = 0.35f; // 电机阻尼 float swing_feed = p_angle * swing_kp; float swing_damp = p_vel * swing_kd_p; float swing_motor_damp = m_vel * swing_kd_m; float u = swing_feed - swing_damp - swing_motor_damp; float swing_max = SUPPLY_VOLTAGE * SWING_UP_SCALE; if(fabs(u) > swing_max) u = signf(u) * swing_max; return u; } void setup() { Serial.begin(SERIAL_BAUD); delay(600); Serial.println("===== ESP32 旋转倒立摆 SimpleFOC 优化修复版 ====="); pinMode(DRIVER_EN, OUTPUT); digitalWrite(DRIVER_EN, LOW); // 双独立I2C初始化 wire_motor.begin(I2C_MOTOR_SDA, I2C_MOTOR_SCL, I2C_FREQ); wire_pend.begin(I2C_PEND_SDA, I2C_PEND_SCL, I2C_FREQ); motor_sensor.init(&wire_motor); pendulum_sensor.init(&wire_pend); Serial.println("双路AS5600磁编码器初始化完成"); // 电机FOC配置：转矩电压模式 motor.controller = MotionControlType::torque; motor.linkSensor(&motor_sensor); driver.voltage_power_supply = SUPPLY_VOLTAGE; driver.voltage_limit = FOC_VOLT_LIMIT; driver.init(); motor.linkDriver(&driver); Serial.println("=== 电机FOC自动校准，请保持电机完全静止 ==="); motor.init(); motor.initFOC(); Serial.println("FOC相序、零点校准全部完成！"); // 注册串口调参指令 cmd.add('A', cmdGainAngle, "LQR摆角增益 A+数值"); cmd.add('P', cmdGainPendVel, "LQR摆角速度增益 P+数值"); cmd.add('M', cmdGainMotorVel, "LQR电机转速增益 M+数值"); cmd.add('T', cmdTotalOffset, "摆杆总角度偏移 T+数值"); cmd.add('0', cmdMotorOff, "关闭电机驱动 0"); cmd.add('1', cmdMotorOn, "使能电机驱动 1"); cmd.add('R', cmdResetOffset, "重置摆角偏移 R"); Serial.println("\n==== 使用说明 ===="); Serial.println("A60 设置摆角增益60"); Serial.println("P3 设置摆角速度增益3"); Serial.println("M0.2 设置电机转速增益0.2"); Serial.println("T0.1 设置摆角偏移补偿0.1"); Serial.println("1 启动电机 | 0 停机保护 | R 清零偏移"); Serial.println("==================\n"); motor_enable_flag = false; last_ctrl_time = millis(); last_print_time = millis(); } void loop() { // 编码器实时读取 motor_sensor.update(); pendulum_sensor.update(); motor.loopFOC(); cmd.run(); // 定时控制逻辑 if(loop_cnt >= LOOP_CTRL_PERIOD){ loop_cnt = 0; uint32_t now = millis(); float dt = (now - last_ctrl_time) / 1000.0f; last_ctrl_time = now; // 限制dt上下限，防止滤波发散 dt = constrain(dt, 1e-5f, 0.05f); // 读取并校正摆杆角度 float pend_raw_ang = pendulum_sensor.getAngle(); float pend_angle_raw = pend_raw_ang - PEND_ANG_OFFSET + total_angle_offset; float pend_angle = constrainAngle(pend_angle_raw); // 安全保护：摆角超限强制停机 bool safe_flag = true; if(fabs(pend_angle) > PEND_SAFE_ANGLE){ motor_enable_flag = false; driver.disable(); digitalWrite(DRIVER_EN, LOW); motor.move(0); target_voltage = 0; safe_flag = false; Serial.println("【安全保护触发】摆杆角度超限，电机紧急停机"); } if(safe_flag) { // 一阶低通滤波平滑转速 float raw_pend_vel = pendulum_sensor.getVelocity(); float raw_motor_vel = motor.shaftVelocity(); filt_pend_vel = filt_pend_vel + dt / (FILTER_TAU_PEND + dt) * (raw_pend_vel - filt_pend_vel); filt_motor_vel = filt_motor_vel + dt / (FILTER_TAU_MOTOR + dt) * (raw_motor_vel - filt_motor_vel); // 分段控制：直立稳摆 / 大角度起摆 if(fabs(pend_angle) < STABLE_ANGLE_THRESH){ target_voltage = lqrController(pend_angle, filt_pend_vel, filt_motor_vel); }else{ target_voltage = swingUpController(pend_angle, filt_pend_vel, filt_motor_vel); } // 电机电压输出 if(motor_enable_flag){ motor.move(target_voltage); }else{ motor.move(0.0f); } } // 非阻塞串口打印状态 if(millis() - last_print_time > PRINT_PERIOD_MS){ last_print_time = millis(); Serial.print("θ:");Serial.print(pend_angle,3); Serial.print(" θd:");Serial.print(filt_pend_vel,2); Serial.print(" ωm:");Serial.print(filt_motor_vel,2); Serial.print(" U:");Serial.println(target_voltage,2); } } loop_cnt++; }