Arduino BLDC机器人基础运动学解算的核心是将机器人整体运动意图精确分解为各电机独立指令的纯几何映射，而简易PID闭环则是通过实时反馈与误差修正确保电机精准执行指令的闭环控制机制；两者协同工作，构成了机器人底层运动控制的基础架构。
一、 机器人基础运动学解算
. 核心原理
机器人运动学解算的本质是建立机器人整体运动意图（如线速度、角速度）与各驱动轮/关节的独立运动指令之间的数学映射关系。它不涉及力、质量等动力学因素，是纯几何关系的计算。
运动学解算分为两个方向：
正向运动学（Forward Kinematics）：已知各轮/关节的转速，推算机器人整体运动状态（位姿、速度）。
逆向运动学（Inverse Kinematics）：给定机器人期望的运动状态，反向求解各轮/关节应达到的目标转速——这是控制系统的核心环节。
. 典型底盘模型与解算公式
（1）双轮差速底盘（Differential Drive）
这是最经典的移动机器人底盘构型。机器人运动状态由线速度 v 和角速度 ω 两个参数完全描述：
逆向运动学（控制核心）：
左轮目标转速：ω_L = (v - ω × D/2) / R
右轮目标转速：ω_R = (v + ω × D/2) / R
（R 为车轮半径，D 为两轮轮距）
正向运动学（里程计推算）：
线速度：v = (ω_R × R + ω_L × R) / 2
角速度：ω = (ω_R × R - ω_L × R) / D
（2）麦克纳姆轮全向底盘（Mecanum Wheel）
四轮全向移动底盘通过独特的45°滚子排列，实现二维平面内任意方向的自由度。其逆运动学解算为：
左前轮：ω₁ = vx - vy - ω
右前轮：ω₂ = vx + vy + ω
左后轮：ω₃ = vx + vy - ω
右后轮：ω₄ = vx - vy + ω
（vx 为前进速度，vy 为横向速度，ω 为旋转速度，输入值归一化至 [-1, 1]）
（3）多关节机械臂
对于串联机械臂，运动学建模通常采用 D-H 参数法（Denavit-Hartenberg），通过连杆长度 a、连杆扭转角 α、连杆偏距 d、关节角 θ 四个参数描述相邻连杆坐标系间的相对位姿，构建 4×4 齐次变换矩阵，各连杆矩阵依次连乘得到末端总位姿。逆运动学则从笛卡尔空间反推关节空间，属于非线性超越方程组求解问题，可能存在多解或无解。
. 主要特点
纯几何关系：解算过程不涉及质量、摩擦力、惯量等动力学参数，计算量小，适合嵌入式平台实时运行。
精度依赖硬件参数：解算精度直接取决于轮径 R、轮距 D、连杆长度等物理参数的测量精度，制造和装配误差会直接传导至运动精度。
存在奇异性问题：在特定构型下（如雅可比矩阵行列式为零），机器人可能丧失特定方向的运动能力或导致关节速度失控。
. 应用场景
AGV/AMR 自动导引运输车：仓储物流中的差速底盘路径跟踪与导航。
全向移动竞技平台：如 RoboMaster 中的麦克纳姆轮机器人，实现高速全向机动。
桌面级机械臂：Delta 并联机器人、多自由度串联臂的拾放与装配任务。
自平衡机器人：通过差速模型实现姿态调节与运动控制。
. 注意事项
参数标定至关重要：轮径、轮距、连杆长度等参数必须精确测量，建议通过实际运动标定进行校准，消除制造公差。
归一化处理：当解算出的各轮速度超出最大能力时，需进行等比例缩放（归一化），防止PWM溢出。
坐标系一致性：全局坐标系与机器人本体坐标系的转换（如通过 atan2 计算角度误差）必须严格统一，否则会导致路径跟踪偏差。
里程计累积误差：正向运动学用于里程计推算时，积分运算会随时间累积误差，需结合 IMU 或外部定位传感器进行校正。
二、 简易 PID 闭环控制
. 核心原理
PID 控制器通过实时比较目标值（Setpoint）与传感器反馈的实际值（Process Variable）之间的误差 e(t)，计算出控制输出（通常为 PWM 占空比），驱动电机消除误差。
三项的物理意义：
P（比例）：提供与误差成正比的恢复力，Kp 越大响应越快，但过大会导致超调和振荡。
I（积分）：累积历史误差，消除稳态误差（如重力、摩擦导致的静差），但过大易引起积分饱和。
D（微分）：感知误差变化率，起阻尼作用，抑制超调和振荡，但对噪声敏感。
. 在 Arduino BLDC 系统中的实现架构
根据控制目标不同，PID 闭环可分为多种形态：
速度闭环：编码器/霍尔传感器反馈实际转速 → PID 计算 → 输出 PWM 调节电机转速。
位置/角度闭环：磁编码器（如 AS5600）反馈实际角度 → PID 计算 → 驱动电机到达目标角度。
串级控制（推荐）：外环为位置/速度 PID，内环为电流/扭矩 PID（通常由 FOC 驱动器实现），形成级联结构，显著提升响应速度和抗干扰能力。
. 主要特点
算法简洁高效：仅涉及加减乘除运算，资源占用低，Arduino Uno/Nano 即可胜任。
闭环反馈，鲁棒性强：相比开环控制，能有效抵抗负载突变、电压波动、地面摩擦不均等外部扰动。
参数物理意义明确：Kp、Ki、Kd 各有直观的工程含义，便于调试和理解。
灵活可扩展：目标值可来自电位器、串口、蓝牙、IMU 等多种来源，便于构建复杂系统。
. 应用场景
轮式机器人速度控制：差速底盘左右轮的独立速度闭环，确保精确轨迹跟踪。
机械臂关节伺服：各关节的角度闭环控制，实现精确重复定位。
自平衡机器人：通过 IMU 反馈倾角，PID 调节电机维持平衡。
智能跟随/避障：根据超声波/UWB 测距误差，PID 调节跟随距离和速度。
无人机姿态控制：调节各电机转速维持飞行稳定。
. 注意事项
（1）PID 参数整定——核心难点
参数整定是 PID 控制中最具挑战性的环节，推荐遵循以下顺序：
先调 P：将 Ki、Kd 设为 0，从小值逐步增大 Kp，直到系统出现持续振荡但不失稳，以此确定 Kp 的上限参考。
再加 D：增大 Kd 抑制超调和振荡，起到阻尼作用。
最后引入 I：缓慢增大 Ki 消除稳态误差，同时适当减小 Kp。
切勿直接套用他人参数——不同电机特性、负载惯量、供电电压下，最优参数差异极大。
（2）工程实现关键要求
固定采样周期：PID 计算必须置于定时器中断中（如 Timer1，周期 1-10ms），严禁使用 delay() 或放在 loop() 中，否则 Δt 不恒定会导致积分和微分项失真。
积分抗饱和（Anti-windup）：必须对积分项设置限幅，防止电机堵转或大偏差时积分无限累积，导致恢复时严重超调。
输出限幅：对 PID 输出进行 PWM 范围限制（0-255），防止电机卡死时电流激增损坏驱动板。
微分项滤波：编码器噪声会被微分项放大，建议对微分项加入低通滤波或变化率限幅。
死区处理：设置微小偏差死区（如 < 0.5），当误差极小时强制输出为 0，避免系统在目标值附近持续振荡（“画龙"现象）。
（3）传感器与硬件匹配
编码器分辨率不足会导致速度估算噪声大，影响控制精度。
磁编码器必须与电机轴刚性同轴安装，否则引入机械间隙误差。
普通航模 ESC 仅支持速度开环，无法用于位置闭环；需选用支持 FOC 的驱动器（如 SimpleFOC + B-G431B-ESC1）或集成编码器的一体化模组。
（4）Arduino 平台资源限制
Arduino Uno/Nano（16MHz, 2KB RAM）难以同时运行多轴高频 PID（>1kHz），多关节或复杂系统建议升级至 ESP32、STM32 或 Teensy 4.0。
电机动力电源与 Arduino 逻辑电源必须共地但建议独立供电，避免电机启动电流导致 Arduino 重启。
三、 两者的协同关系
在实际的 Arduino BLDC 机器人系统中，运动学解算与 PID 闭环是上下游协同的关系：
上层指令（如"以 0.5m/s 前进，0.2rad/s 左转”）→ 运动学逆解（分解为左右轮目标转速）→ PID 闭环（确保电机实际转速精确跟踪目标值）→ 运动学正解（里程计反馈实际位姿，形成外层闭环）
运动学解算负责"算得准"，PID 闭环负责"执行稳"。两者缺一不可——没有运动学解算，PID 不知道该追踪什么目标；没有 PID 闭环，运动学解算的结果只是纸上谈兵，无法抵抗实际物理世界的各种扰动。

1、差速底盘运动学逆解算与PID速度闭环
此案例聚焦于运动学解算与闭环控制的基础框架。通过将机器人整体的线速度和角速度需求，反向解算为左右轮各自的目标转速，再通过编码器反馈和PID算法进行闭环修正，这是所有差速驱动机器人的核心算法原型。

#include <Encoder.h>
#include <PID_v1.h>

// --- 硬件参数配置 ---
const float WHEEL_BASE = 0.35;      // 轮距 (米)
const float WHEEL_DIAMETER = 0.10;  // 轮径 (米)
const float TICKS_PER_REV = 4000.0; // 编码器每转脉冲数 (四倍频后)

// --- 电机与编码器引脚定义 ---
#define ENC_LEFT_A 2
#define ENC_LEFT_B 3
#define ENC_RIGHT_A 4
#define ENC_RIGHT_B 5
#define PWM_LEFT 9
#define PWM_RIGHT 10

// --- 全局变量 ---
float target_v = 0.0;      // 目标线速度 (m/s)
float target_omega = 0.0;  // 目标角速度 (rad/s)

volatile long left_ticks = 0;
volatile long right_ticks = 0;
unsigned long last_time = 0;

Encoder encLeft(ENC_LEFT_A, ENC_LEFT_B);
Encoder encRight(ENC_RIGHT_A, ENC_RIGHT_B);

// --- 左右轮PID控制器定义 ---
double left_setpoint = 0, left_input = 0, left_output = 0;
double right_setpoint = 0, right_input = 0, right_output = 0;
double Kp = 0.5, Ki = 0.1, Kd = 0.01;

PID pidLeft(&left_input, &left_output, &left_setpoint, Kp, Ki, Kd, DIRECT);
PID pidRight(&right_input, &right_output, &right_setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
    Serial.begin(115200);
    
    // 初始化PID
    pidLeft.SetMode(AUTOMATIC);
    pidLeft.SetOutputLimits(-255, 255);
    pidRight.SetMode(AUTOMATIC);
    pidRight.SetOutputLimits(-255, 255);
    
    pinMode(PWM_LEFT, OUTPUT);
    pinMode(PWM_RIGHT, OUTPUT);
    
    last_time = micros();
}

void loop() {
    // 1. 计算控制周期 dt
    unsigned long current_time = micros();
    float dt = (current_time - last_time) / 1000000.0;
    last_time = current_time;
    if (dt <= 0) return;

    // 2. 运动学逆解算：将线速度/角速度转换为左右轮目标转速
    // 差速模型公式：v_left = v - omega * L/2, v_right = v + omega * L/2
    float v_left_req = target_v - (target_omega * WHEEL_BASE / 2.0);
    float v_right_req = target_v + (target_omega * WHEEL_BASE / 2.0);
    
    // 线速度 (m/s) 转换为转速 (RPM)
    float rpm_left_req = (v_left_req / (PI * WHEEL_DIAMETER)) * 60.0;
    float rpm_right_req = (v_right_req / (PI * WHEEL_DIAMETER)) * 60.0;
    
    // 3. 读取编码器反馈，计算实际转速 (RPM)
    left_ticks = encLeft.read();
    right_ticks = encRight.read();
    float rpm_left_act = (left_ticks / TICKS_PER_REV) / dt * 60.0;
    float rpm_right_act = (right_ticks / TICKS_PER_REV) / dt * 60.0;
    encLeft.write(0);
    encRight.write(0);

    // 4. PID运算：目标RPM vs 实际RPM
    left_setpoint = rpm_left_req;
    right_setpoint = rpm_right_req;
    left_input = rpm_left_act;
    right_input = rpm_right_act;
    pidLeft.Compute();
    pidRight.Compute();

    // 5. 输出PWM驱动电机
    analogWrite(PWM_LEFT, constrain(abs(left_output), 0, 255));
    analogWrite(PWM_RIGHT, constrain(abs(right_output), 0, 255));
    
    // 方向控制 (需根据硬件设计实现)
    // digitalWrite(DIR_LEFT, left_output >= 0 ? HIGH : LOW);
    // digitalWrite(DIR_RIGHT, right_output >= 0 ? HIGH : LOW);
    
    // 调试输出
    if (millis() % 100 < 10) {
        Serial.print("V: "); Serial.print(target_v);
        Serial.print(" Omega: "); Serial.print(target_omega);
        Serial.print(" | L_RPM: "); Serial.print(rpm_left_req);
        Serial.print(" -> "); Serial.print(rpm_left_act);
        Serial.print(" | R_RPM: "); Serial.print(rpm_right_req);
        Serial.print(" -> "); Serial.println(rpm_right_act);
    }
    
    delay(10); // 100Hz控制频率
}

关键逻辑：案例一展示了完整的“目标指令→运动学解算→闭环反馈”链路。差速底盘的运动学逆解公式 v_left = v - ω·L/2 和 v_right = v + ω·L/2 是核心，它将机器人的宏观运动需求映射为左右电机的独立控制指令。PID控制器则根据编码器反馈持续修正偏差，实现速度的精准跟随。

2、单轴BLDC电机PID速度闭环（使用霍尔传感器）
此案例聚焦于单电机轴的PID速度控制基础。通过霍尔传感器（或编码器）获取电机的实时转速反馈，利用PID算法计算控制量并输出PWM驱动电机。这是理解和调试PID参数的最小可行单元，适合在搭建完整机器人底盘前先验证单电机控制性能。

#include <SoftwareSerial.h>

// --- 电机1引脚定义 ---
#define DIR_PIN_M1 6
#define PWM_PIN_M1 5
#define HALL_SENSOR_M1 3  // 霍尔传感器反馈引脚 (中断)

// --- PID参数与变量 ---
float kp = 0.15, ki = 0.7, kd = 0.001;
float targetSpeed = 20;        // 目标速度 (需根据系统标定)
float feedback1 = 0.0;         // 实际速度反馈
float error1 = 0.0;
float prevErr1 = 0.0;
float errSum1 = 0.0;
float pid1 = 0.0;
float maxPWM = 255;
unsigned long lastTime = 0;

// --- 霍尔传感器中断计数 ---
volatile float tic1 = 0, tac1 = 0;

void measureSpeed1() {
    // 霍尔传感器上升沿触发，记录时间
    tic1 = micros();
    // 计算速度：频率 = 1 / (两次脉冲间隔)
    if (tic1 - tac1 > 100) {
        feedback1 = 1.0 / ((tic1 - tac1) / 1000000.0); // 频率 (Hz)
    }
    tac1 = tic1;
}

void setup() {
    Serial.begin(115200);
    
    pinMode(DIR_PIN_M1, OUTPUT);
    pinMode(PWM_PIN_M1, OUTPUT);
    attachInterrupt(digitalPinToInterrupt(HALL_SENSOR_M1), measureSpeed1, RISING);
    
    digitalWrite(DIR_PIN_M1, LOW); // 初始方向
    lastTime = millis();
}

void loop() {
    unsigned long now = millis();
    float dt = (now - lastTime) / 1000.0;
    lastTime = now;
    if (dt <= 0) return;

    // 1. 计算误差
    error1 = targetSpeed - feedback1;
    
    // 2. PID计算 (位置式PID)
    errSum1 += error1 * dt;
    // 抗积分饱和
    errSum1 = constrain(errSum1, -100, 100);
    
    float derivative = (error1 - prevErr1) / dt;
    pid1 = kp * error1 + ki * errSum1 + kd * derivative;
    
    // 输出限幅
    pid1 = constrain(pid1, -maxPWM, maxPWM);
    prevErr1 = error1;
    
    // 3. 驱动电机 (方向+占空比)
    if (pid1 >= 0) {
        digitalWrite(DIR_PIN_M1, LOW);
        analogWrite(PWM_PIN_M1, pid1);
    } else {
        digitalWrite(DIR_PIN_M1, HIGH);
        analogWrite(PWM_PIN_M1, -pid1);
    }
    
    // 调试输出
    Serial.print("Target: "); Serial.print(targetSpeed);
    Serial.print(" | Feedback: "); Serial.print(feedback1);
    Serial.print(" | Error: "); Serial.print(error1);
    Serial.print(" | PID Out: "); Serial.println(pid1);
    
    delay(50);
}

关键逻辑：本案例的核心是使用霍尔传感器中断测量转速。通过测量相邻脉冲的时间间隔来计算瞬时速度，在中断服务函数中更新反馈值，确保测量的实时性和准确性。PID计算采用位置式PID，加入了抗积分饱和处理防止失控。

3、双电机PID协同控制与扭矩自适应
此案例将单轴控制扩展到双电机协同，并引入扭矩自适应逻辑。在复杂地形中，左右轮可能因地面摩擦差异而出现滑移，通过监测滑移率并动态调整扭矩分配，可以提高机器人在崎岖路面的通过能力。

#include <SimpleFOC.h>
#include <Encoder.h>

// --- BLDC电机定义 (使用SimpleFOC库) ---
BLDCMotor motorL = BLDCMotor(11);
BLDCMotor motorR = BLDCMotor(11);
BLDCDriver3PWM driverL = BLDCDriver3PWM(5, 6, 10, 9);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 4, 8, 7);

// --- 参数配置 ---
float wheelRadius = 0.06;        // 轮半径 (m)
float wheelTrack = 0.45;         // 左右轮距 (m)
float targetLinearSpeed = 0.5;   // 目标车速 (m/s)
float slipThreshold = 0.3;       // 滑移阈值
float maxTorque = 2.5;           // 最大扭矩

// --- PID控制器 (扭矩修正) ---
float L_output = 0, R_output = 0;
float L_error = 0, R_error = 0;
double Kp_torque = 0.8, Ki_torque = 0.05, Kd_torque = 0.1;
PID torquePID_L(&L_error, &L_output, &targetLinearSpeed, Kp_torque, Ki_torque, Kd_torque, DIRECT);
PID torquePID_R(&R_error, &R_output, &targetLinearSpeed, Kp_torque, Ki_torque, Kd_torque, DIRECT);

void setup() {
    Serial.begin(115200);
    
    // 初始化BLDC电机与FOC
    driverL.voltage_power_supply = 24;
    driverL.init();
    motorL.linkDriver(&driverL);
    motorL.init();
    motorL.initFOC();
    motorL.controller = MotionControlType::torque;  // 扭矩控制模式
    
    driverR.voltage_power_supply = 24;
    driverR.init();
    motorR.linkDriver(&driverR);
    motorR.init();
    motorR.initFOC();
    motorR.controller = MotionControlType::torque;
    
    // 初始化扭矩PID
    torquePID_L.SetMode(AUTOMATIC);
    torquePID_L.SetOutputLimits(0, maxTorque);
    torquePID_R.SetMode(AUTOMATIC);
    torquePID_R.SetOutputLimits(0, maxTorque);
}

// 计算轮速 (m/s)
float calculateWheelSpeed(BLDCMotor& motor) {
    return motor.shaft_velocity * wheelRadius;
}

// 计算滑移率
float calculateSlip(float targetSpeed, float actualSpeed) {
    if (fabs(targetSpeed) < 0.001) return 0;
    return (targetSpeed - actualSpeed) / targetSpeed;
}

void loop() {
    // 1. 读取实际轮速
    float wheelSpeedL = calculateWheelSpeed(motorL);
    float wheelSpeedR = calculateWheelSpeed(motorR);
    
    // 2. 计算目标轮速 (差速模型)
    float targetOmega = 0.0; // 假设直线运动，角速度为0
    float targetWheelSpeedL = targetLinearSpeed - (targetOmega * wheelTrack / 2.0);
    float targetWheelSpeedR = targetLinearSpeed + (targetOmega * wheelTrack / 2.0);
    float targetWheelSpeed = (targetWheelSpeedL + targetWheelSpeedR) / 2.0;
    
    // 3. 计算滑移率
    float slipL = calculateSlip(targetWheelSpeed, wheelSpeedL);
    float slipR = calculateSlip(targetWheelSpeed, wheelSpeedR);
    
    // 4. 扭矩自适应逻辑：滑移轮降扭矩，对侧轮适当增扭
    float torqueL = maxTorque;
    float torqueR = maxTorque;
    
    if (slipL > slipThreshold) {
        // 左轮打滑，降低左轮扭矩
        torqueL = maxTorque * (1.0 - slipL);
    }
    if (slipR > slipThreshold) {
        torqueR = maxTorque * (1.0 - slipR);
    }
    
    // 5. 通过PID微调扭矩
    L_error = targetLinearSpeed - wheelSpeedL;
    R_error = targetLinearSpeed - wheelSpeedR;
    torquePID_L.SetOutputLimits(0, torqueL);
    torquePID_R.SetOutputLimits(0, torqueR);
    torquePID_L.Compute();
    torquePID_R.Compute();
    
    // 6. 应用扭矩指令
    motorL.target = L_output;
    motorR.target = R_output;
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 调试输出
    if (millis() % 100 < 10) {
        Serial.print("L: "); Serial.print(wheelSpeedL);
        Serial.print(" | R: "); Serial.print(wheelSpeedR);
        Serial.print(" | SlipL: "); Serial.print(slipL);
        Serial.print(" | SlipR: "); Serial.println(slipR);
    }
    
    delay(20);
}

关键逻辑：本案例在双电机独立PID闭环的基础上，增加了滑移监测与扭矩自适应机制。当检测到某侧轮子滑移率超过阈值时，自动降低该轮扭矩，并将功率更多分配给抓地力更好的对侧轮，提高复杂地形的通过能力。这种扭矩分配策略在越野机器人中尤为重要。

要点解读

1. 运动学逆解是实现机器人级控制的核心桥梁
   机器人运动控制的指令通常表达为宏观的“线速度”和“角速度”（如向前走0.5m/s、原地旋转0.3rad/s）。而电机只能理解“转速”或“扭矩”这样的底层指令。运动学逆解就是连接这两者的桥梁。对于差速底盘，核心公式就是 v_left = v - ω·L/2 和 v_right = v + ω·L/2，其中 L 是轮距。Arduino算力有限，需对算法做简化与优化：使用 constrain() 约束输出范围，避免关节角度超出物理极限。

2. 反馈传感器的选择与精度直接影响闭环性能
   PID闭环控制依赖可靠的反馈信号。常用的反馈方式包括编码器（精度高，可测速度和位置）、霍尔传感器（测速，成本低）和磁编码器（抗污染能力强）。编码器每转脉冲数越高，速度估算精度越高，建议选择至少4000脉冲/转（四倍频后）的编码器。反馈信号的噪声问题可通过移动平均滤波或卡尔曼滤波处理。

3. PID参数整定是闭环控制中最关键的调试环节
   PID的三个参数各有分工：P项决定响应速度，P过大导致超调震荡；I项消除稳态误差，但积分饱和会引发振荡，需要设置输出限幅；D项抑制超调，但对噪声敏感，编码器信号需先滤波再使用。推荐使用Ziegler-Nichols法进行整定：先调P使系统临界振荡，记录振荡周期和增益，再按经验公式计算Ki和Kd。参数整定建议在单轴调试时完成，再扩展到多轴协同。

4. 控制周期固定是PID稳定运行的前提
   PID计算必须在固定的时间间隔内完成，否则积分项和微分项的计算基准会混乱。工程实践中通常采用以下方法确保固定周期：使用定时器中断触发控制计算（如1ms或10ms定时器），或将主循环中所有可能阻塞运行的代码（如 delay()、串口打印）移除，改用 millis() 计时器控制执行频率。编码器计数建议使用硬件中断，避免主循环延迟导致脉冲丢失。

5. 力矩控制模式是实现柔顺运动的关键
   在基础的速度闭环之上，更高级的控制策略需要切换到力矩（扭矩）控制模式。SimpleFOC库支持将电机控制模式设置为 MotionControlType::torque，此时PID输出直接作为扭矩指令。这种模式配合阻抗控制或滑移自适应算法，能让机器人表现出“柔顺”的特性——遇到障碍时不是硬碰硬地堵转，而是像弹簧一样柔顺地退让，在足式机器人、机械臂和越野机器人中都有广泛应用。

4、BLDC 单关节角度 PID 闭环控制（SimpleFOC + AS5600 磁编码器）
场景：机械臂单关节精确角度定位，通过串口发送目标角度，电机自动闭环到达。

#include <SimpleFOC.h>

// 磁编码器（I2C，AS5600，12位分辨率）
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);

// BLDC电机（极对数根据实际电机调整，2804/2208为7，4015为11）
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8); // PWM引脚 + 使能引脚

float target_angle = 0;
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

void setup() {
  sensor.init();
  motor.linkSensor(&sensor);

  driver.voltage_power_supply = 12;
  driver.init();
  motor.linkDriver(&driver);

  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.controller = MotionControlType::angle; // 角度闭环模式

  // 速度环 PID
  motor.PID_velocity.P = 0.05f;
  motor.PID_velocity.I = 0.02f;
  motor.PID_velocity.D = 0;
  motor.voltage_limit = 6;
  motor.LPF_velocity.Tf = 0.01f;

  // 角度环 P
  motor.P_angle.P = 20;
  motor.velocity_limit = 15;

  Serial.begin(115200);
  motor.useMonitoring(Serial);
  motor.init();
  motor.initFOC(); // 传感器对齐 + 启动FOC

  command.add('T', doTarget, "target angle");
  Serial.println("Motor ready. Send T<angle> (e.g. T6.28 = 1 revolution)");
}

void loop() {
  motor.loopFOC();       // FOC核心循环（必须高频调用）
  motor.move(target_angle);
  command.run();         // 监听串口指令
}

运行方式：上传后在串口监视器输入 T3.14，电机旋转至 π 弧度（180°）位置并锁定。

5、双轮差速底盘运动学解算 + 双电机速度 PID 闭环
场景：AGV/移动机器人通过线速度 v 和角速度 ω 控制底盘运动，逆向运动学分解为左右轮目标转速，各自独立速度闭环执行。

#include <SimpleFOC.h>

// ===== 底盘参数 =====
float wheel_radius = 0.032;  // 车轮半径 (m)
float wheelbase    = 0.20;   // 两轮轮距 (m)

// ===== 左电机 =====
MagneticSensorI2C sensorL = MagneticSensorI2C(AS5600_I2C);
BLDCMotor motorL = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(3, 5, 6, 11);

// ===== 右电机 =====
// 注：若使用同一I2C总线，第二块AS5600需修改I2C地址
// 此处简化为使用编码器中断方案
volatile long encoderR_pos = 0;
void encoderR_ISR() { encoderR_pos++; }

float target_v = 0;    // 目标线速度 (m/s)
float target_omega = 0; // 目标角速度 (rad/s)

void setup() {
  Serial.begin(115200);

  // 左电机初始化
  sensorL.init();
  motorL.linkSensor(&sensorL);
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.controller = MotionControlType::velocity;
  motorL.PID_velocity.P = 0.2f;
  motorL.PID_velocity.I = 0.05f;
  motorL.voltage_limit = 6;
  motorL.init();
  motorL.initFOC();

  // 右编码器中断
  attachInterrupt(digitalPinToInterrupt(2), encoderR_ISR, RISING);

  Serial.println("Differential Drive Ready.");
  Serial.println("Send: v<speed> or w<omega> (e.g. v0.5 w0.3)");
}

void loop() {
  // 串口解析目标速度
  if (Serial.available()) {
    char cmd = Serial.read();
    if (cmd == 'v') target_v = Serial.parseFloat();
    if (cmd == 'w') target_omega = Serial.parseFloat();
  }

  // ===== 逆向运动学解算 =====
  float w_left  = (target_v - target_omega * wheelbase / 2.0) / wheel_radius;
  float w_right = (target_v + target_omega * wheelbase / 2.0) / wheel_radius;

  // 左电机：SimpleFOC 速度闭环
  motorL.move(w_left);
  motorL.loopFOC();

  // 右电机：简易PID速度闭环（基于编码器）
  static long prevPos = 0;
  static unsigned long prevTime = 0;
  unsigned long now = millis();
  float dt = (now - prevTime) / 1000.0;

  if (dt > 0.005) { // 至少5ms采样间隔
    float actualSpeed = ((encoderR_pos - prevPos) / 400.0 * 2 * PI) / dt;
    float error = w_right - actualSpeed;

    // 简易 PI 控制
    static float integral = 0;
    integral += error * dt;
    integral = constrain(integral, -5.0, 5.0); // 积分抗饱和
    float output = 0.3 * error + 0.1 * integral;

    // 输出到右电机PWM（简化示意）
    analogWrite(9, constrain(abs(output * 40), 0, 255));

    prevPos = encoderR_pos;
    prevTime = now;

    // 调试输出
    Serial.printf("v=%.2f w=%.2f | L=%.2f R=%.2f\n",
                  target_v, target_omega, motorL.shaft_velocity, actualSpeed);
  }

  delay(10);
}

运行方式：串口发送 v0.5 设定前进速度 0.5m/s，w0.3 设定转向角速度 0.3rad/s，两者可叠加。

6、两轮自平衡机器人——互补滤波姿态解算 + PD 直立环 PID 闭环
场景：倒立摆型自平衡小车，IMU 融合姿态角，PD 控制器驱动 BLDC 维持直立。

#include <Wire.h>

// ===== 硬件引脚 =====
int motorPin1 = 5;  // 左电机PWM
int motorPin2 = 6;  // 右电机PWM
int motorDir1 = 7;  // 方向控制
int motorDir2 = 8;

// ===== PID 参数 =====
float Kp = 35.0;   // 角度比例（需根据实际调参）
float Kd = 1.5;    // 角速度微分

// ===== 互补滤波参数 =====
float alpha = 0.98; // 陀螺仪权重
float angle = 0;    // 融合后的姿态角

// ===== MPU6050 I2C =====
const int MPU_ADDR = 0x68;

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // 唤醒MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); Wire.write(0);
  Wire.endTransmission();

  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorDir1, OUTPUT);
  pinMode(motorDir2, OUTPUT);

  Serial.println("Balance Robot Ready.");
}

void loop() {
  // 1. 读取IMU原始数据
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // 加速度计起始地址
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true);
  int16_t ax = Wire.read() << 8 | Wire.read();
  Wire.read() << 8 | Wire.read(); // 跳过 ay
  Wire.read() << 8 | Wire.read(); // 跳过 az

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x43); // 陀螺仪起始地址
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 2, true);
  int16_t gx = Wire.read() << 8 | Wire.read();

  // 2. 计算加速度计角度 & 陀螺仪角速度
  float accAngle = atan2(ax, /*az*/ 16384) * 180.0 / PI; // 简化
  float gyroRate = gx / 131.0; // 灵敏度 131 LSB/(°/s)

  // 3. 互补滤波融合
  float dt = 0.01; // 10ms 控制周期
  angle = alpha * (angle + gyroRate * dt) + (1 - alpha) * accAngle;

  // 4. PD 直立环控制
  float targetAngle = 0; // 目标：垂直（0°）
  float error = targetAngle - angle;
  float output = Kp * error - Kd * gyroRate; // 微分项直接用陀螺仪数据

  // 5. 电机输出
  int pwm = constrain((int)output, -255, 255);
  if (pwm > 0) {
    analogWrite(motorPin1, pwm); analogWrite(motorPin2, pwm);
    digitalWrite(motorDir1, HIGH); digitalWrite(motorDir2, HIGH);
  } else {
    analogWrite(motorPin1, -pwm); analogWrite(motorPin2, -pwm);
    digitalWrite(motorDir1, LOW); digitalWrite(motorDir2, LOW);
  }

  // 调试输出
  Serial.printf("Angle: %.1f | Gyro: %.1f | PWM: %d\n", angle, gyroRate, pwm);
  delay(10); // 固定10ms控制周期
}

运行方式：将小车扶至接近垂直位置后松手，系统自动维持平衡。Kp 和 Kd 需根据实际硬件反复调参。

要点解读

1. 运动学解算是"指令翻译器"，PID 闭环是"执行保障器"
   三个案例体现了同一架构：上层运动意图 → 运动学逆解分解为各电机目标值 → PID 闭环确保电机精确跟踪。案例一中目标角度直接作为 PID 设定点；案例二中线速度/角速度经差速逆解分解为左右轮转速后再各自闭环；案例三中姿态角误差经 PD 控制转化为电机 PWM。没有运动学解算，PID 不知道追踪什么；没有 PID，运动学解算的结果无法抵抗负载、摩擦等实际扰动。
2. PID 参数整定必须"从内到外、逐环调参"
   案例一展示了典型的串级控制结构：内环为速度 PI，外环为角度 P。调参顺序必须严格遵循：先调内环（速度/电流），再调外环（位置/角度）。案例三中直立环采用 PD 控制（无积分项），因为自平衡系统本身是开环不稳定的，积分项会加剧发散。切勿直接套用他人参数——不同电机极对数、负载惯量、供电电压下最优参数差异极大。
3. 固定采样周期是 PID 稳定运行的生命线
   三个案例中控制循环均通过 delay(10) 或时间戳判断维持固定周期（10ms）。在实际部署中，必须使用定时器中断替代 delay()，因为 delay() 会被串口通信、传感器读取等操作阻塞，导致 Δt 不恒定，积分项和微分项失真，严重时引发系统振荡。案例三中的互补滤波公式 angle = α × (angle + gyro × dt) + (1-α) × accAngle 对 dt 的精度极为敏感。
4. 传感器选型与安装直接决定控制精度上限
   案例一的 AS5600 磁编码器提供 12 位分辨率（≈0.088°），通过 I2C 读取，必须与电机轴刚性同轴安装，否则引入机械间隙误差。
   案例二的编码器用于速度估算，分辨率不足会导致速度噪声大，建议配合低通滤波。
   案例三的 MPU6050 通过互补滤波融合陀螺仪（高频响应好但有漂移）和加速度计（长期稳定但怕振动），alpha 值（0.98）需根据实际振动环境调整。
5. Arduino 平台资源限制与升级路径
   标准 Arduino Uno（16MHz, 2KB RAM）运行案例一、三基本可行，但运行案例二的双电机高频 FOC + 运动学解算会吃力。关键限制与建议：
   FOC 运算：motor.loopFOC() 需要 ≥1kHz 调用频率，Uno 勉强胜任单电机，双电机建议升级至 ESP32（双核 240MHz）或 STM32。
   浮点运算：运动学解算涉及大量浮点乘除，Uno 无硬件 FPU，运算延迟较高；ESP32/STM32 支持硬件浮点，性能提升 10 倍以上。
   多轴扩展：若需控制 3 个以上 BLDC 电机（如机械臂），强烈建议使用 STM32 或 Teensy 4.1（600MHz），配合 SimpleFOC 库的多电机管理功能。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。