[项目] 【花雕】AI 迷你小龙虾 MimiClaw 自主闭环控制机器人

#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// 1. 硬件定义 (根据实际接线修改)
BLDCMotor motor = BLDCMotor(7); // 7为极对数
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 14); // 相线PWM引脚, 使能引脚
MagneticSensorI2C sensor = MagneticSensorI2C(0x36); // AS5600 地址

// 2. 共享变量 (用于 Core 0 和 Core 1 通信)
volatile float target_velocity = 0.0; 

// 3. Core 1 任务：实时 FOC 控制
void FOC_Control_Task(void * parameter) {
  for(;;) {
    motor.loopFOC();
    motor.move(target_velocity);
    delay(1); // 约 1kHz 控制频率
  }
}

void setup() {
  Serial.begin(115200);
  
  // 初始化传感器
  sensor.init();
  motor.linkSensor(&sensor);

  // 初始化驱动
  driver.voltage_power_supply = 12;
  driver.init();
  motor.linkDriver(&driver);

  // 配置闭环控制
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.5;
  motor.PID_velocity.I = 10;
  motor.PID_velocity.D = 0;
  motor.PID_velocity.output_ramp = 1000;
  motor.PID_velocity.limit = 6; // 电压限制

  motor.init();
  motor.initFOC();

  // 启动 Core 1 控制任务
  xTaskCreatePinnedToCore(
    FOC_Control_Task,   // 任务函数
    "FOC_Task",         // 任务名
    4096,               // 堆栈大小
    NULL,               // 参数
    5,                  // 优先级
    NULL,               // 任务句柄
    1                   // 绑定到 Core 1
  );
}

void loop() {
  // Core 0 主循环 (MimiClaw 逻辑)
  // 模拟接收指令：如果串口收到数字，更新目标速度
  if (Serial.available()) {
    target_velocity = Serial.parseFloat();
    Serial.print("新目标速度: ");
    Serial.println(target_velocity);
  }
  delay(10);
}
复制代码

#include <SimpleFOC.h>
#include <WiFi.h>
#include <BluetoothSerial.h> // 模拟 MimiClaw 通信接口

// --- 硬件定义同上 ---
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 14);
MagneticSensorI2C sensor = MagneticSensorI2C(0x36);

volatile float target_velocity = 0.0;

// --- MimiClaw 模拟部分 ---
BluetoothSerial BT; // 使用蓝牙模拟与 MimiClaw 后端通信

// 解析 MimiClaw 发来的 JSON 或 文本指令
void parseMimiClawCommand(String cmd) {
  if (cmd.indexOf("前进") >= 0) {
    target_velocity = 5.0;
    Serial.println("MimiClaw 决策: 前进");
  } else if (cmd.indexOf("停止") >= 0) {
    target_velocity = 0.0;
    Serial.println("MimiClaw 决策: 停止");
  } else if (cmd.indexOf("左转") >= 0) {
    // 假设是差速转向或调整偏航角
    Serial.println("MimiClaw 决策: 左转逻辑");
  }
}

// --- FOC 任务 (Core 1) ---
void FOC_Task(void * parameter) {
  sensor.init();
  motor.linkSensor(&sensor);
  driver.voltage_power_supply = 12;
  driver.init();
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.5; 
  motor.PID_velocity.I = 10;
  motor.init();
  motor.initFOC();

  for(;;) {
    motor.loopFOC();
    motor.move(target_velocity);
    delay(1);
  }
}

void setup() {
  Serial.begin(115200);
  BT.begin("MimiClaw-Robot"); // 蓝牙名称
  Serial.println("等待 MimiClaw 连接...");

  // 启动 Core 1
  xTaskCreatePinnedToCore(FOC_Task, "FOC", 4096, NULL, 5, NULL, 1);
}

void loop() {
  // Core 0: MimiClaw 通信循环
  if (BT.available()) {
    String command = BT.readStringUntil('\n');
    command.trim();
    if (command.length() > 0) {
      parseMimiClawCommand(command);
    }
  }
  delay(20);
}
复制代码

#include <SimpleFOC.h>

// 硬件定义
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 14);
MagneticSensorI2C sensor = MagneticSensorI2C(0x36);

volatile float target_velocity = 0.0;
volatile bool safety_lock = false; // 安全锁标志位

// 安全监控阈值
const float MAX_CURRENT = 3.0; // 假设最大电流 3A
const float MAX_TEMP = 60.0;   // 假设最高温度 60度

void Safety_Monitor_Task(void * parameter) {
  for(;;) {
    // 1. 读取传感器数据 (示例：读取电流估算值)
    // 注意：SimpleFOC 需配置电流检测硬件才能获取真实电流
    float current_q = motor.current_q; 
    
    // 2. 安全检查逻辑
    if (abs(current_q) > MAX_CURRENT) {
      safety_lock = true;
      Serial.println("⚠️ 警报：过流保护触发！");
      // 这里可以添加代码通过串口向 MimiClaw 发送 "ERROR_OVERCURRENT"
    }
    
    // 3. 恢复逻辑 (简单示例)
    if (safety_lock && abs(current_q) < 0.5) {
       // 需要 MimiClaw 确认后才能解锁，这里仅做演示自动复位
       // safety_lock = false; 
    }
    
    delay(10);
  }
}

void FOC_Task(void * parameter) {
  // ... (初始化代码同上) ...
  sensor.init(); motor.linkSensor(&sensor);
  driver.voltage_power_supply = 12; driver.init(); motor.linkDriver(&driver);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.5; motor.PID_velocity.I = 10;
  motor.init(); motor.initFOC();

  for(;;) {
    motor.loopFOC();
    
    // 核心安全闭环：如果安全锁激活，强制停止电机
    if (safety_lock) {
      motor.move(0); 
    } else {
      motor.move(target_velocity);
    }
    
    delay(1);
  }
}

void setup() {
  Serial.begin(115200);
  // 启动 FOC 任务 (Core 1)
  xTaskCreatePinnedToCore(FOC_Task, "FOC", 4096, NULL, 5, NULL, 1);
  // 启动 安全监控 (Core 1 或 Core 0 均可，建议 Core 1 以保证实时性)
  xTaskCreatePinnedToCore(Safety_Monitor_Task, "Safety", 2048, NULL, 4, NULL, 1);
}

void loop() {
  // Core 0: MimiClaw 逻辑
  // 模拟 MimiClaw 发送速度指令
  if (Serial.available()) {
     if (safety_lock) {
       Serial.println("拒绝指令：系统处于安全锁定状态");
     } else {
       target_velocity = Serial.parseFloat();
     }
  }
  delay(10);
}
复制代码