【SimpleFOC】SimpleFOC的运动规划器(Motion Planner)和梯形速度规划

SimpleFOC 在位置（角度）模式时，达到目标位置过程中，是默认开启平滑转动的效果的。

SimpleFOC 的平滑转动是通过运动规划器(Motion Planner)实现的，而不是瞬间跳到目标位置。

运动规划器的工作原理

1. 默认的梯形速度规划

// SimpleFOC 内部自动生成的平滑运动轨迹
void loop() {
  motor.loopFOC();          // 执行FOC控制
  motor.move(target_position); // 这里包含运动规划！
}

2. 运动规划参数配置

void setup() {
  // ... 其他初始化代码 ...
  
  // 配置运动规划器 - 实现平滑转动的关键！
  motor.PID_velocity.P = 0.2;    // 速度环P增益
  motor.PID_velocity.I = 2;      // 速度环I增益
  motor.LPF_velocity.Tf = 0.01;  // 速度滤波时间常数
  
  // 运动规划器参数
  motor.velocity_limit = 10.0;     // 最大速度 (rad/s)
  motor.acceleration_limit = 50.0; // 最大加速度 (rad/s²)
  motor.jerk_limit = 100.0;        // 加加速度限制 (rad/s³)
}

3. 平滑运动的三阶段

当执行 motor.move(6*_PI) 时：

阶段1：加速阶段

// 内部计算示例
current_velocity += acceleration * dt;  // 逐渐加速
current_position += current_velocity * dt;

阶段2：匀速阶段

// 保持最大允许速度
current_velocity = velocity_limit;

阶段3：减速阶段

// 逐渐减速到目标位置
float distance_to_target = target_position - current_position;
if (distance_to_target < braking_distance) {
    current_velocity -= acceleration * dt;  // 平滑减速
}

4. 可视化运动曲线

速度 (rad/s)
   ▲
   │        *********
   │      **         **
   │    **             **
   │  **                 **
   │**                     **
   └─────────────────────────▶ 时间
     加速  匀速   减速

5. 自定义更平滑的运动

// 如果想要更平滑的运动，可以降低限制值
void setup() {
  // 更温和的运动参数
  motor.velocity_limit = 5.0;      // 降低最大速度
  motor.acceleration_limit = 10.0; // 降低加速度
  motor.jerk_limit = 20.0;         // 降低加加速度
}

6. 实时监控运动状态

void loop() {
  motor.loopFOC();
  motor.move(target_position);
  
  // 监控运动状态
  Serial.print("目标: ");
  Serial.print(target_position);
  Serial.print(" | 实际: ");
  Serial.print(motor.shaft_angle);
  Serial.print(" | 速度: ");
  Serial.println(motor.shaft_velocity);
}

对比：非平滑 vs 平滑

非平滑运动（直接设置位置）：

• 电机会剧烈启动/停止

• 产生较大机械冲击

• 可能丢失同步

平滑运动（使用运动规划）：

• 速度从0逐渐增加到最大值

• 接近目标时平滑减速

• 减少机械应力，运行更安静

这就是 SimpleFOC 实现"平滑转动"的核心机制！运动规划器确保了电机在任何目标位置变化时都能产生平滑的速度曲线。