设备控制 TB6600+42步进电机 -EPS32C3 -arduino

  42步进电机的一般最高速度可以达到4400RPM，但在实际应用中，为了平衡转速和力矩，通常建议选择90至900RPM的速度范围。同时，需要根据具体的应用需求和条件来选择合适的转速和驱动方案。

 

要达到900RPM的速度，即15RPS，必须有个加速过程控制。

这里使用两个脉冲距离，调整一次步进电机控制的驱动脉冲频率（ESP32C3定时器中断），按此设计如下程序。

实现 梯形曲线  即 加速 匀速 减速的距离控制  

减速至0控制。

 

#include <Arduino.h>
#include <cmath> // 包含数学库以使用sqrt函数
// 定义TB6600控制角
#define TB_DIR 10
#define TB_PUL 3
#define TB_EN 2
//两次中断一个脉冲
#define TimerFrequency 1000000
#define Mirosteps 800      //Step motor microstepping value  800个脉冲一圈
/////////////////////////////////////////////////////////////////////////
// 电机方向控制
void motorClockwise(){
    // 将TB6600的控制引脚设置为输出模式
    pinMode(TB_DIR, OUTPUT);
    pinMode(TB_EN, OUTPUT);
    pinMode(TB_PUL, OUTPUT); 
    // 设置转动方向
    digitalWrite(TB_DIR, HIGH); 
    digitalWrite(TB_EN, LOW); 
};
void motorCounterClockwise(){
    // 将TB6600的控制引脚设置为输出模式
    pinMode(TB_DIR, OUTPUT);
    pinMode(TB_EN, OUTPUT);
    pinMode(TB_PUL, OUTPUT); 
    // 设置转动方向
    digitalWrite(TB_DIR, LOW); 
    digitalWrite(TB_EN, LOW); 
};
/////////////////////////////////////////////////////////////////////////
// 定义队列项的数据结构
typedef struct {
    int data; // 要传递的数据
} QueueItem;
typedef struct {// 要传递的数据内容
    float V; 
    float S_now;
    float S_last; // 记录接收新命令时的S_now
    float S_acceleration;
    float S_deceleration;
    float S_start_deceeleration;
    int Period;// 定时器中断周期
    int State;// 0 静止， 1加速，匀速，3 减速至静止
    double S_absolute;// 绝对圈数

} IsrInforItem;
volatile IsrInforItem  StateItem;// 中断和线程的公共变量
// 定义共享数据结构
typedef struct {
  float S; // 转或圈
  float V; // RPS   
  float Accelerate;  // RPS^2
  float Decelerate;  // RPS^2
  int   Command; //  1加速，匀速，到减速至静止，3 减速至静止
  bool  DirIsClockwise= true;     //
  bool  resetState = true; // 清零状态量
} MotorCtrlItem;

volatile MotorCtrlItem CtrlItem; // 中断和线程的公共变量
volatile bool timerIsUse = false;// 控制中断程序是否实现功能

// 线程互斥量
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
// 线程信号量
volatile SemaphoreHandle_t timerSemaphore;
//计算加速或减少距离
float getAcceleratecCircles(float v, float a){
  return (v/a*v/2);
}
//加速或减速两个步进的定时时间
int getTimerPeriodForAccelerate(float Vmax,float Vsmall,float* pVbig, float a){
  float s1 = Vsmall/a*Vsmall/2;
  float s2 = s1 + 2.0/Mirosteps;
  float t = sqrt(s2*2/a);
  *pVbig = t*a;
  if (*pVbig> Vmax){*pVbig = Vmax;};
  t = 2.0/Mirosteps/(*pVbig);
  return(int(t*TimerFrequency/4));// 2个步进，4个定时溢出中断
}
//加速或减速两个步进的定时时间
int getTimerPeriodForDecelerate(float* pVsmall,float s, float a){
  if (s<0){s =0;};
  float s1 = s;
  float s2 = s1 - 2.0/Mirosteps;
  if(s2<0){s2 = s1;};
  float t = sqrt(s2*2/a);
  *pVsmall = t*a;
  t = 2.0/Mirosteps/(*pVsmall);
  return(int(t*TimerFrequency/4));// 2个步进，4个定时溢出中断
}
// 任务函数，用于处理队列中的数据
void MotorTask(void *pvParameters) {
    IsrInforItem item;
    bool  ret = true;
    bool  show = false;
    for (;;) {
        delay(10);
        if (xSemaphoreTake(timerSemaphore, 1000) == pdTRUE) {
          // 读写共享区命令
          portENTER_CRITICAL(&timerMux);
            // float S; // 转或圈
            // float V; // RPS   
            // float Accelerate;  // RPS^2
            // float Decelerate;  // RPS^2
            // int   Command; // 0 静止， 1加速，匀速，到减速至静止，2 匀速，3 减速至静止
              item.S_now = StateItem.S_now;
              item.V     = StateItem.V;
              item.S_acceleration=StateItem.S_acceleration;
              item.S_deceleration=StateItem.S_deceleration;
              item.S_start_deceeleration=StateItem.S_start_deceeleration;
              item.Period=StateItem.Period;// 定时器中断周期
              item.State=StateItem.State;// 0 静止， 1加速，匀速，到减速至静止，2 匀速，3 减速至静止
              item.S_absolute = StateItem.S_absolute;
          portEXIT_CRITICAL(&timerMux);
          show = true;
        };
        //从队列中接收数据
        if (show) {
            // 在这里处理接收到的数据
            //if (item.State == 3){
            Serial.print("Received data: ");
            Serial.println(item.S_now); 
            Serial.println(item.V); 
            Serial.println(item.S_acceleration);
            Serial.println(item.S_deceleration);
            Serial.println(item.S_start_deceeleration);
            Serial.println(item.State);
            Serial.println(item.Period);
            Serial.println(item.S_absolute);
            show = false;
            //};
        }
    }
}

/////////////////////////////////////////////////////////////////////////
// 初始化 定时器
// 定时器
hw_timer_t *timer = NULL;

volatile bool pulsState = LOW; // 初始TB_PUL状态

void ARDUINO_ISR_ATTR onTimer() {
  if (not timerIsUse){ return;};

  portENTER_CRITICAL_ISR(&timerMux);
    if (CtrlItem.resetState){
      CtrlItem.resetState = false;// 清标志
      // 复位 处理
      if(CtrlItem.DirIsClockwise){
        motorClockwise();
      }else{
        motorCounterClockwise();
      };
      //StateItem.V = 0;
      //StateItem.S_now =0;
      StateItem.S_last = StateItem.S_now; // 记录接收新命令时的S_now
      if(CtrlItem.Command == 1){
        // 计算加速距离
        StateItem.S_acceleration = getAcceleratecCircles(CtrlItem.V,CtrlItem.Accelerate);
        // 计算减速距离
        StateItem.S_deceleration = getAcceleratecCircles(CtrlItem.V,CtrlItem.Decelerate);
        // 计算减速开始的坐标点
        StateItem.S_start_deceeleration=CtrlItem.S - StateItem.S_deceleration;
      };
      if(CtrlItem.Command == 3){
        // 计算减速距离
        StateItem.S_deceleration = getAcceleratecCircles(StateItem.V,CtrlItem.Decelerate);
        // 修改目标距离
        CtrlItem.S = StateItem.S_now+StateItem.S_deceleration;
      };
    };
    if(CtrlItem.Command == 1){
      if(StateItem.S_now >= StateItem.S_start_deceeleration){
        StateItem.State  = 3;
      }else{StateItem.State  = 1;};
    }
    if(CtrlItem.Command == 3){
      StateItem.State  = 3;
    };
    //if(CtrlItem.Command == 2){StateItem.State  = 2;};
    // 计算周期，改变定时器中断频率，进而改变速度。
    int Period =0;
    if(StateItem.State  == 1){
      float  Vbig =0;
      Period = getTimerPeriodForAccelerate(CtrlItem.V,StateItem.V,&Vbig, CtrlItem.Accelerate);
      StateItem.V=Vbig;
    } 
    if(StateItem.State  == 3){
      float  Vsmall =0;
      Period = getTimerPeriodForDecelerate(&Vsmall,CtrlItem.S- StateItem.S_now, CtrlItem.Decelerate);
      StateItem.V=Vsmall;
    } 
    // if(StateItem.State  == 2){
    //   Period = int(2.0/Mirosteps / StateItem.V*TimerFrequency);
    //   StateItem.V=CtrlItem.V;
    // }     
    timerSetCount(Period);
    StateItem.Period = Period;
    // 记录距离
    StateItem.S_now = StateItem.S_now + 0.5/Mirosteps;

  portEXIT_CRITICAL_ISR(&timerMux);

  if(StateItem.S_now >= CtrlItem.S) {
    timerIsUse = false;

    portENTER_CRITICAL_ISR(&timerMux);
      StateItem.V=0;
      StateItem.State=0;//静止

      if(CtrlItem.DirIsClockwise){ // 记录距离
        StateItem.S_absolute =StateItem.S_absolute+StateItem.S_now; // 记录距离
      }else{
        StateItem.S_absolute =StateItem.S_absolute-StateItem.S_now; // 记录距离
      };
      StateItem.S_now =0;
    portEXIT_CRITICAL_ISR(&timerMux);

    return;
  };
  if(StateItem.V>0){
     // 设置TB6600 的脉冲引脚
     digitalWrite(TB_PUL, pulsState);
     pulsState =!pulsState;
  };
  // Give a semaphore that we can check in the loop
  xSemaphoreGiveFromISR(timerSemaphore, NULL);// 发信号，通知线程可以安全读写共享变量
}

void timerSetup()
{
  // 设定定时器频率 
  timer = timerBegin(TimerFrequency);// 1MHz
  // 绑定中断函数
  timerAttachInterrupt(timer, &onTimer);

  pulsState = LOW;   // 初始TB_PUL状态
  timerIsUse = true; // 控制中断程序是否实现功能
  timerSetCount(100);//us 
}
void timerSetCount(int count){
  // 计数达到count，调中断函数一次 即 count us中断一次
  timerAlarm(timer, count, true, 0); // 产生脉冲最高频率是125kHz  #define Freq_Interrupts 250000
}
void timerClose(){
  timerIsUse = false;// 控制中断程序是否实现功能
  // 关闭定时器中断
  timerDetachInterrupt(timer);
  timerEnd(timer);
}
/////////////////////////////////////////////////////////////////////////
void setup() {
    // put your setup code here, to run once:
    Serial.begin(115200);
    // 创建信号量 Create semaphore to inform us when the timer has fired
    timerSemaphore = xSemaphoreCreateBinary();
    // 创建队列
    // TimerQueue = xQueueCreate(queueLength, itemSize);
    // 创建任务来处理队列中的数据
    xTaskCreate(
        MotorTask,       // 任务函数
        "MotorTask",     // 任务名称
        configMINIMAL_STACK_SIZE * 2, // 任务堆栈大小
        NULL,                // 任务输入参数
        configMAX_PRIORITIES - 1, // 任务优先级（最高）
        NULL                 // 任务句柄（不需要时可以设置为NULL）
    );

    CtrlItem.S = 100;//圈
    CtrlItem.V =15;   //RPS
    CtrlItem.Accelerate =10;//RPS^2
    CtrlItem.Decelerate =10;//RPS^2
    CtrlItem.Command =1;//命令
    CtrlItem.resetState = true;//复位状态参数
    CtrlItem.DirIsClockwise = true;
    timerIsUse = true;// 执行运动控制

    timerSetup();// 启动定时器

    delay(10000);// 延时

    // 减速停止
    if (xSemaphoreTake(timerSemaphore, 1000) == pdTRUE) {
          // 读写共享区命令
          portENTER_CRITICAL(&timerMux);
          CtrlItem.Command =3;//命令 减速停止，程序自动修改目标距离CtrlItem.S
          CtrlItem.resetState = true;// 重新计算 距离CtrlItem.S
          portEXIT_CRITICAL(&timerMux);
    }
}

void loop() {
  // put your main code here, to run repeatedly:

}