一.G力6轴电机控制

1.任务函数模拟生成脉冲

#include <Arduino.h>           // Arduino核心库，提供基础函数和常量
#include <freertos/FreeRTOS.h> // FreeRTOS实时操作系统核心头文件
#include <freertos/task.h>     // FreeRTOS任务管理相关函数
#include <freertos/semphr.h>   // FreeRTOS信号量和互斥锁功能
#include <Adafruit_MCP23X17.h> // MCP23017 GPIO扩展芯片驱动库
#include <U8g2lib.h>           // OLED显示屏驱动库
#include <Wire.h>              // I2C通信库

// ========== 硬件对象定义 ==========
Adafruit_MCP23X17 mcp; // MCP23017扩展芯片对象
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE); // OLED显示对象

// ========== 系统配置常量 ==========
const byte MOTOR_COUNT = 4;           // 电机数量
const unsigned long baudRate = 115200; // 串口通信波特率
const unsigned int xingcheng = 142;    // 行程系数（每转步数）
const unsigned int minPos = 0;         // 最小位置限制
const unsigned int maxPos = 200 * xingcheng; // 最大位置计算
const int MAX_SPEED_FREQ = 10000;      // 最大速度频率(Hz)
const int MIN_SPEED_FREQ = 300;        // 最小速度频率(Hz)
const int ACCELERATION = 1000;         // 加速度值

// ========== 硬件引脚定义 ==========
const int pulsePins[MOTOR_COUNT] = {0, 1, 2, 3};  // 4个电机的脉冲引脚
const int dirPins[MOTOR_COUNT] = {6, 7, 8, 9};    // 4个电机的方向引脚

// ========== FreeRTOS相关定义 ==========
SemaphoreHandle_t motorMutex;      // 电机控制互斥锁，防止多任务同时访问电机数据
SemaphoreHandle_t displayMutex;    // 显示互斥锁，防止显示刷新过程被打断
TaskHandle_t pulseTaskHandle = NULL; // 脉冲生成任务句柄

// ========== 电机状态结构体 ==========
struct MotorState {
    volatile int32_t currentPos;   // 当前位置（volatile确保多任务访问正确性）
    volatile int32_t targetPos;    // 目标位置
    volatile int32_t currentSpeed; // 当前速度
    volatile int32_t stepCounter;  // 步进计数器
    volatile bool isMoving;        // 运动状态标志
    bool direction;                // 运动方向
    volatile bool pulseState;      // 脉冲状态（高/低电平）
};

MotorState motors[MOTOR_COUNT];   // 4个电机的状态数组

// ========== 硬件初始化函数 ==========
void hardwareInit() {
  Wire.begin(5, 6);  // 初始化I2C，SDA=GPIO5, SCL=GPIO6
  
  // 初始化MCP23017扩展芯片，地址为0x20
  if (!mcp.begin_I2C(0x20, &Wire)) {
    Serial.println("mcp23x17: Error");
    while (1);  // 初始化失败则停止程序
  }
  
  // 初始化OLED显示屏
  if (!u8g2.begin()) {
    Serial.println("OLED init failed!");
    while(1);
  }
  u8g2.setFont(u8g2_font_wqy12_t_gb2312);  // 设置中文字体
  u8g2.clearBuffer();  // 清空显示缓冲区
  u8g2.drawStr(0, 12, "System Starting...");  // 显示启动信息
  u8g2.sendBuffer();    // 将缓冲区内容发送到显示屏
}

// ========== 脉冲生成任务（高优先级） ==========
volatile int64_t previousMicros = 0;    // 上次脉冲生成的时间戳(微秒)
const int64_t    BASE_INTERVAL = 50;    // 基准时间间隔(微秒)，控制脉冲频率
//---------------------------------------------
void pulseGenerationTask(void *pvParameters) {
  while (1) {  
    int64_t currentMicros = esp_timer_get_time();      // 获取当前时间(微秒)
    int64_t elapsed = currentMicros - previousMicros;  // 计算时间间隔
    
    // 达到基准时间间隔时生成脉冲
    if (elapsed >= BASE_INTERVAL) {
      previousMicros = currentMicros;    // 更新上次执行时间
      
      // 获取电机互斥锁，防止其他任务修改电机状态
      if (xSemaphoreTake(motorMutex, portMAX_DELAY) == pdTRUE) {
        uint16_t outputValue = 0;  // 16位输出值，对应MCP23017的16个GPIO
        
        // 遍历所有电机，生成脉冲信号
        for (int i = 0; i < MOTOR_COUNT; i++) {
          if (motors[i].isMoving && motors[i].stepCounter > 0) {
            motors[i].pulseState = !motors[i].pulseState;  // 翻转脉冲状态
            
            // 设置方向引脚输出
            if (motors[i].direction) {
              outputValue |= (1 << dirPins[i]);  // 设置方向位为高
            } else {
              outputValue &= ~(1 << dirPins[i]); // 设置方向位为低
            }
            
            // 在脉冲上升沿更新位置和步数计数器
            if (motors[i].pulseState) {
              // 根据方向更新当前位置
              motors[i].direction ? motors[i].currentPos++ : motors[i].currentPos--;
              motors[i].stepCounter--;  // 步数减1
              
              // 检查是否到达目标位置
              if (motors[i].stepCounter <= 0) {
                motors[i].isMoving = false;  // 停止运动
                motors[i].currentSpeed = 0;   // 速度归零
              }
            }
            
            // 设置脉冲引脚输出电平
            if (motors[i].pulseState) {
              outputValue |= (1 << pulsePins[i]);  // 脉冲引脚高电平
            } else {
              outputValue &= ~(1 << pulsePins[i]); // 脉冲引脚低电平
            }
          }
        }
        
        mcp.writeGPIOAB(outputValue);  // 一次性写入所有GPIO状态到MCP23017
        xSemaphoreGive(motorMutex);    // 释放互斥锁
      }
    }
    
    vTaskDelay(1 / portTICK_PERIOD_MS);  // 短暂延时，让出CPU给其他任务
  }
}

// ========== OLED显示任务 ==========
// 此任务负责定期更新OLED显示屏内容，显示系统状态
void displayTask(void *pvParameters) {
  char buffer[20];  // 显示缓冲区
  
  while (1) {
    // 获取显示互斥锁，确保显示内容的一致性
    if (xSemaphoreTake(displayMutex, portMAX_DELAY) == pdTRUE) {
      u8g2.clearBuffer();  // 清空显示缓冲区
      
      // 显示前两个电机的状态信息
      for (int i = 0; i < MOTOR_COUNT && i < 2; i++) {
        // 格式化显示电机当前位置和目标位置
        snprintf(buffer, sizeof(buffer), "M%d:%ld/%ld", i, motors[i].currentPos, motors[i].targetPos);
        u8g2.drawStr(0, (i+1)*12, buffer);
        
        // 显示电机运动状态和速度
        if (motors[i].isMoving) {
          snprintf(buffer, sizeof(buffer), "%s%dHz", motors[i].direction ? "UP:" : "DN:", motors[i].currentSpeed);
          u8g2.drawStr(70, (i+1)*12, buffer);
        } else {
          u8g2.drawStr(70, (i+1)*12, "STOP");
        }
      }
      
      // 显示系统状态信息
      u8g2.drawStr(0, 36, "Pulse:Software");
      u8g2.drawStr(0, 48, "Mode:4-Motor Ctrl");
      
      u8g2.sendBuffer();  // 更新显示
      xSemaphoreGive(displayMutex);  // 释放显示互斥锁
    }
    
    vTaskDelay(200 / portTICK_PERIOD_MS);  // 每200ms刷新一次显示
  }
}

// ========== 电机控制任务 ==========
// 此任务监控每个电机的目标位置，并启动运动过程
void motorControlTask(void *pvParameters) {
  uint8_t motorIndex = (uint8_t)(uintptr_t)pvParameters;  // 从参数获取电机索引
  
  while (1) {
    if (xSemaphoreTake(motorMutex, portMAX_DELAY) == pdTRUE) {
      MotorState* motor = &motors[motorIndex];
      
      // 如果电机处于静止状态但目标位置不等于当前位置，则启动运动
      if (!motor->isMoving && motor->currentPos != motor->targetPos) {
        int32_t steps = motor->targetPos - motor->currentPos;  // 计算需要移动的步数
        motor->direction = (steps > 0);      // 设置运动方向
        motor->stepCounter = abs(steps);     // 设置总步数
        motor->isMoving = true;              // 启动运动
      }
      
      xSemaphoreGive(motorMutex);  // 释放互斥锁
    }
    
    vTaskDelay(100 / portTICK_PERIOD_MS);  // 每100ms检查一次
  }
}

// ========== 串口通信任务 ==========
// 此任务处理来自串口的控制指令，更新电机目标位置
void uartCommunicationTask(void *pvParameters) {
  while (1) {
    // 检查是否有足够的数据可读（至少2字节头部）
    if (Serial.available() >= 2) {
      // 检查数据包头部（0xFF 0xFF）
      if (Serial.read() == 0xFF && Serial.read() == 0xFF) {
        // 等待直到收到完整的数据包（MOTOR_COUNT * 2字节的位置数据）
        while (Serial.available() < MOTOR_COUNT * 2) {
          vTaskDelay(1 / portTICK_PERIOD_MS);
        }
        
        // 解析每个电机的目标位置数据
        for (int i = 0; i < MOTOR_COUNT; i++) {
          // 读取16位原始位置数据（高位在前）
          uint16_t rawPos = (Serial.read() << 8) | Serial.read();
          // 将原始数据映射到实际位置范围
          int32_t mappedPos = rawPos * 200 / 461.5;
          // 限制位置在合法范围内并更新目标位置
          motors[i].targetPos = constrain(mappedPos, minPos, maxPos);
        }
      }
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);  // 短暂延时
  }
}

// ========== 主设置函数 ==========
void setup() {
  Serial.begin(baudRate);  // 初始化串口通信
  hardwareInit();          // 初始化硬件
  
  // 初始化GPIO和电机状态
  for (int i = 0; i < MOTOR_COUNT; i++) {
    mcp.pinMode(pulsePins[i], OUTPUT);  // 设置脉冲引脚为输出模式
    mcp.digitalWrite(pulsePins[i], LOW); // 初始化为低电平
    
    mcp.pinMode(dirPins[i], OUTPUT);    // 设置方向引脚为输出模式
    mcp.digitalWrite(dirPins[i], LOW);  // 初始化为低电平
    
    // 初始化电机状态变量
    motors[i].currentPos = 0;
    motors[i].targetPos = 0;
    motors[i].currentSpeed = 0;
    motors[i].stepCounter = 0;
    motors[i].isMoving = false;
    motors[i].direction = true;
    motors[i].pulseState = false;
  }
  
  // 创建互斥锁用于资源保护
  motorMutex = xSemaphoreCreateMutex();
  displayMutex = xSemaphoreCreateMutex();
  
  // 创建FreeRTOS任务（按优先级排序）
  xTaskCreate(pulseGenerationTask, "PulseGen", 4096, NULL, 4, &pulseTaskHandle); // 最高优先级
  xTaskCreate(displayTask, "Display", 4096, NULL, 2, NULL);     // 显示任务
  xTaskCreate(uartCommunicationTask, "UART", 4096, NULL, 2, NULL); // 串口任务
  
  // 为每个电机创建独立的控制任务
  for (int i = 0; i < MOTOR_COUNT; i++) {
    xTaskCreate(motorControlTask, "MotorCtrl", 4096, (void*)(uintptr_t)i, 3, NULL);
  }
}

void loop() {
  // FreeRTOS调度器已接管任务调度，此处只需短暂延时
  vTaskDelay(1000 / portTICK_PERIOD_MS);
}