Mega2560同时控制三个步进电机

参考

https://blog.csdn.net/weixin_43272272/article/details/107836477

硬件接线

ZD-42S驱动器接线图

代码

#include 
#include   //队列在这里很重要，串口接收的控制指令会分配给队列，其他任务再从队列里获取，避免多任务同时访问一个数据
/************定义4个任务，一个串口收发任务，3个电机任务**************/
void TaskSerial( void *pvParameters );//任务名称可以自己定
void Task_servo1( void *pvParameters );
void Task_servo2( void *pvParameters );
void Task_servo3( void *pvParameters );

QueueHandle_t  a_dir, a_num,  //定义servo1的指令的队列，a_dir是控制电机方向，a_num是控制电机步数
               b_dir, b_num, 
               c_dir, c_num;

void setup()
{
  Serial.begin(115200, SERIAL_8N1);
  
/************定义各电机的控制指令队列**************/
  /*..Y..*/
  a_dir = xQueueCreate( 1, sizeof( char ) );//队列数据类型定义，1是队列长度，char是Yc的数据类型
  a_num = xQueueCreate( 1, sizeof( long ) );

  /*..ZX..*/
  b_dir = xQueueCreate( 1, sizeof( char ) );
  b_num = xQueueCreate( 1, sizeof( long ) );

  /*..YX..*/
  c_dir = xQueueCreate( 1, sizeof( char ) );
  c_num = xQueueCreate( 1, sizeof( long ) );
  
/************创建4个任务（也可以在任务中创建任务），一个串口收发任务，3个电机任务**************/
  xTaskCreate(TaskSerial, "Serial" , 256 , NULL, 2 , NULL );//“Serial”是任务名称，256是该任务分配的存储空间，优先级为2（数字越大优先级越高）
  xTaskCreate(Task_servo1, "a_servo" , 128 , NULL, 1 , NULL );
  xTaskCreate(Task_servo2, "b_servo" , 128 , NULL, 1 , NULL );
  xTaskCreate(Task_servo3, "c_servo" , 128 , NULL, 1 , NULL );
 
  vTaskStartScheduler(); //启动任务调度
  
}

void loop()
{}

/*---------------------- Tasks Serial---------------------*/
void TaskSerial(void *pvParameters)
{
  Serial.println("二级任务"); 
  (void) pvParameters;
  //下面写串口任务需要的初始化参数
  char buffer1[22] = " ";//这个用来储存串口数据
  char data1 = ' ';//这个用来储存电机判断指令
  char data2 = ' ';//这个用来储存电机方向判断指令
  long number = 0;//这个用来储存电机运行步数
 
  for (;;)
  {
    /*---------------------接收数据---------------------*/
    if (Serial.available() > 0) //说明串口接收到数据
    {
      Serial.println("接收到数据");
      data1 = Serial.read();//串口接收指令，将第一个字符给data1,(用来判断需要控制哪电机)
      data2 = Serial.read();//串口接收指令，将第2个字符给data2,(用来判断电机方向)
    
      Serial.readBytes(buffer1, 22);//串口接收指令，将剩余字符数组buffer1,(用来储存电机运行步数),22是读取的字节数是22


      /****servo1****/
      if (data1 == 'a')
      {
        
        data1= ' ';
        number = atol(buffer1);//将数组buffer1的值强制转换为long型，把buffer1储存的步数给number
        xQueueSend( a_num, &number, 1);//把步数发给a_num
        number = 0;
        xQueueSend( a_dir, &data2, 1);//把方向发给a_dir
        data2= ' ';
      }

      /****servo2****/
      if (data1 == 'b')
      {
        Serial.println("进入b选择");
        data1= ' ';
        number = atol(buffer1);
        xQueueSend( b_num, &number, 1);
        //Serial.println("number");
        //Serial.println(number);
        number = 0;
        xQueueSend( b_dir, &data2, 1);
        //Serial.println("data2");
        //Serial.println(data2);
        data2= ' ';
      }

      /****servo3****/
      if (data1 == 'c')
      {
        data1= ' ';
        number = atol(buffer1);
        xQueueSend( c_num, &number, 1);
        number = 0;
        xQueueSend( c_dir, &data2, 1);
        data2 = ' ';
      }

      /****清空缓冲****/
      for (int k = 0; k < 22; k++)
      {
        buffer1[k] = ' ';
      }
    }
    vTaskDelay(3); // 等待1ms,避免任务拥挤
  }
}

/*------------------------------------------servo1----------------------------------------- */
void Task_servo1(void *pvParameters)  
{
  (void) pvParameters;
    /*----------- servo1电机任务初始化配置 -----------*/
  char a_data = ' ';//这是电机方向数据，它会去队列中获取方向数据
  long a_number = 0;//这是电机步数数据，它会去队列中获取步数数据
  long a_cp = 0;//这个是电机步数对比值，它与a_number对比  

  /*-----------servo1引脚接线-----------*/
  pinMode(22, OUTPUT);    //DIR，电机驱动器DIR接Arduino 2560的D22，后面接线照着接
  pinMode(23, OUTPUT);    //CP
  pinMode(24, OUTPUT);    //EN
  digitalWrite(24, LOW);  //EN     为了使电机不发热，先脱机
  
  for (;;)
  {
    if (xQueueReceive(a_dir,&a_data,3) )//判断队列a-dir是否有数据进来,并获取数据（方向数据）给a_data
    {
      
      xQueueReceive( a_num, &a_number, 3);//获取电机步数a_num的数据给a_number，因为串口任务里先发送a_num数据，再发送a_dir数据，所以用a_dir判断数据是否进来，a-dir数据进来，则a_num数据已进来

      
      if (a_data == 'f')//电机顺时针
      {
        
        digitalWrite(24, HIGH);     //电机使能
        digitalWrite(22, LOW);      //电机顺时针
        while (a_cp < a_number)//电机走Ynumber个步数
        {
          digitalWrite(23, HIGH);
          vTaskDelay( 0.001 );
          digitalWrite(23, LOW);
          vTaskDelay( 0.001  );
          a_cp++;
        }
        a_cp = 0;//对比数据清零
      }
      
      if (a_data == 'i')//电机逆时针
      {
        digitalWrite(24, HIGH);     
        digitalWrite(22, HIGH);      //电机逆时针
        while (a_cp < a_number)
        {
          digitalWrite(23, HIGH);
          vTaskDelay(0.001);
          digitalWrite(23, LOW);
          vTaskDelay(0.001);
          a_cp++;
        }
        a_cp = 0;
      }

      /*------------复位------------*/
      if (a_data == 'F')
      {
        digitalWrite(24, LOW);  //EN     
      }
    }
    digitalWrite(24, LOW);
    vTaskDelay( 0.03); // 等待1ms
  }
}

/*------------------------------------------servo2-----------------------------------------*/
void Task_servo2(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  char b_data = ' ';
  long b_number = 0;
  long b_cp = 0;

  /*----------- servo2引脚接线 -----------*/
  pinMode(26, OUTPUT);    //DIR
  pinMode(27, OUTPUT);    //CP
  pinMode(28, OUTPUT);    //EN
  digitalWrite(28, LOW);  //EN OFF

  for (;;)
  {
    if (xQueueReceive( b_dir, &b_data, 3) )
    {
      xQueueReceive( b_num, &b_number, 3);
      Serial.println(b_data);

      if (b_data == 'f')
      {
        digitalWrite(28, HIGH);     //EN
        digitalWrite(26, LOW);      //电机顺时针
        while (b_cp < b_number)
        {
          digitalWrite(27, HIGH);
          vTaskDelay( 0.001  );
          digitalWrite(27, LOW);
          vTaskDelay( 0.001 );
          b_cp++;
          //Serial.println(b_cp);
        }
        b_cp = 0;
      }
      
      if (b_data == 'i')
      {
        digitalWrite(28, HIGH);     //EN
        digitalWrite(26, HIGH);      //电机逆时针
        while (b_cp < b_number)
        {
          digitalWrite(27, HIGH);
          vTaskDelay( 0.001 );
          digitalWrite(27, LOW);
          vTaskDelay( 0.001 );
          b_cp++;
        }
        b_cp = 0;

      }

      /*------------复位------------*/
      if (b_data == 'F')
      {
        digitalWrite(28, LOW);  //EN

      }
    }
    digitalWrite(28, LOW);
    vTaskDelay(0.03); // 等待2ms
  }
}

/*------------------------------------------servo3-----------------------------------------*/
void Task_servo3(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  char c_data = ' ';
  long c_number = 0;
  long c_cp = 0;
   
  /*----------- servo3引脚接线 -----------*/
  pinMode(30, OUTPUT);    //DIR
  pinMode(31, OUTPUT);    //CP
  pinMode(32, OUTPUT);    //EN
  digitalWrite(32, LOW);  //EN OFF

  for (;;)
  {
    if (xQueueReceive( c_dir, &c_data, 3) )
    {
      xQueueReceive( c_num, &c_number, 3);

      if (c_data == 'f')
      {
        digitalWrite(32, HIGH);     //EN
        digitalWrite(30, LOW);      //远离电机端
        while (c_cp < c_number)
        {
          digitalWrite(31, HIGH);
          vTaskDelay( 3 / portTICK_PERIOD_MS );
          digitalWrite(31, LOW);
          vTaskDelay( 3 / portTICK_PERIOD_MS );
          c_cp++;
        }
        c_cp = 0;
      }
      if (c_data == 'i')
      {
        digitalWrite(32, HIGH);     //EN
        digitalWrite(30, HIGH);      //靠近电机端
        while (c_cp < c_number)
        {
          digitalWrite(31, HIGH);
          vTaskDelay( 3 / portTICK_PERIOD_MS );
          digitalWrite(31, LOW);
          vTaskDelay( 3 / portTICK_PERIOD_MS );
          c_cp++;
        }
        c_cp = 0;
      }

      /*------------复位------------*/
      if (c_data == 'F')
      {
        digitalWrite(32, LOW);  //EN
      }
    }
    digitalWrite(32, LOW);
    vTaskDelay(3); // 等待2ms
  }
}

注意

1

驱动器的细分数要选择合适，不能太小，太小的话，电机转起来就会一顿一顿的。另外，细分数和程序里驱动电机发脉冲的时候的延时时间有关，细分数越大，延时时间越短。延时过短，电机不转并且发出尖锐的声音，说明脉冲频率过高，此时要增大延时时间；延时过长，电机转动很慢。

2

调试的时候，通过串口给板子发信号

即点右上角的图案。输入的脉冲数要大一点，不然有的时候电机一下就转完了还发现不了，以为是程序问题。

代码更新

最终版，用在跟ros通讯联合调试三并联平台之中

#include 
#include 
#include 
#include   //队列在这里很重要，串口接收的控制指令会分配给队列，其他任务再从队列里获取，避免多任务同时访问一个数据
/************定义4个任务，一个串口收发任务，3个电机任务**************/
void TaskSerial( void *pvParameters );//任务名称可以自己定
void Task_servo1( void *pvParameters );
void Task_servo2( void *pvParameters );
void Task_servo3( void *pvParameters );

char a_data = 'q';//这是电机方向数据，
long a_number = 0;//这是电机步数数据，
char b_data = 'q';
long b_number = 0;
char c_data = 'q';
long c_number = 0;

ros::NodeHandle  nh;

void servoCb( const std_msgs::Float32MultiArray &angle_msg)  //angle_msg其实是脉冲数
{
  Serial.println("进入回调函数");
  if (angle_msg.data[0] == 0)
  {
    a_data = 'q'; a_number = 0;
    }
  if (angle_msg.data[0] > 0)
  {
    a_data = 'i'; a_number = long(angle_msg.data[0]);
    }
  if (angle_msg.data[0] < 0)
  {
    a_data = 'f'; a_number = long(-angle_msg.data[0]);
    }
// ------------------------------------------------
  if (angle_msg.data[1] == 0)
  {
    b_data = 'q'; b_number = 0;
    }
  if (angle_msg.data[1] > 0)
  {
    b_data = 'i'; b_number = long(angle_msg.data[1]);
    }
  if (angle_msg.data[1] < 0)
  {
    b_data = 'f'; b_number = long(-angle_msg.data[1]);
    }
// ---------------------------------------------------------
  if (angle_msg.data[2] == 0)
  {
    c_data = 'q'; c_number = 0;
    }
  if (angle_msg.data[2] > 0)
  {
    c_data = 'i'; c_number = long(angle_msg.data[2]);
    }
  if (angle_msg.data[2] < 0)
  {
    c_data = 'f'; c_number = long(-angle_msg.data[2]);
    }

}


ros::Subscriber<std_msgs::Float32MultiArray> sub_angle("/servo_angle", servoCb);


void setup()
{
  Serial.begin(115200, SERIAL_8N1);
  nh.initNode();
  nh.subscribe(sub_angle);

/************创建4个任务（也可以在任务中创建任务），一个串口收发任务，3个电机任务**************/
  xTaskCreate(TaskSerial, "Serial" , 256 , NULL, 2 , NULL );
  xTaskCreate(Task_servo1, "a_servo" , 128 , NULL, 1 , NULL );
  xTaskCreate(Task_servo2, "b_servo" , 128 , NULL, 1 , NULL );
  xTaskCreate(Task_servo3, "c_servo" , 128 , NULL, 1 , NULL );
 
  vTaskStartScheduler(); //启动任务调度
 
}

void loop()
{}

void TaskSerial(void *pvParameters)
{
  (void) pvParameters;
  for(;;)
  {
    nh.spinOnce();
    vTaskDelay( 25 / portTICK_PERIOD_MS ); // 等待3ms
    }
  
  
}

/*------------------------------------------servo1----------------------------------------- */
void Task_servo1(void *pvParameters)  
{
  (void) pvParameters;
    /*----------- servo1电机任务初始化配置 -----------*/
 
  long a_cp = 0;//这个是电机步数对比值，它与a_number对比  

  /*-----------servo1引脚接线-----------*/
  pinMode(22, OUTPUT);    //DIR，电机驱动器DIR接Arduino 2560的D22，后面接线照着接
  pinMode(23, OUTPUT);    //CP
  pinMode(24, OUTPUT);    //EN
  digitalWrite(24, HIGH);  //EN  
  
  for (;;)
  {
    if (a_data == 'f')//电机顺时针
    {
    
      digitalWrite(22, LOW);      //电机顺时针
      while (a_cp < a_number)//电机走Ynumber个步数
      {
        digitalWrite(23, HIGH);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        digitalWrite(23, LOW);
        vTaskDelay( 2 / portTICK_PERIOD_MS  );
        a_cp++;
      }
      a_cp = 0;//对比数据清零
      a_data= 'q';
    }
    
    if (a_data == 'i')//电机逆时针
    {
      digitalWrite(24, HIGH);     
      digitalWrite(22, HIGH);      //电机逆时针
      while (a_cp < a_number)
      {
        digitalWrite(23, HIGH);
        vTaskDelay(2 / portTICK_PERIOD_MS);
        digitalWrite(23, LOW);
        vTaskDelay(2 / portTICK_PERIOD_MS);
        a_cp++;
      }
      a_cp = 0;
      a_data='q';
    }
    if (a_data == 'q')
    {
      digitalWrite(24, HIGH);  //EN
    }
    vTaskDelay( 3 / portTICK_PERIOD_MS ); // 等待3ms
  }
    
  
}

/*------------------------------------------servo2-----------------------------------------*/
void Task_servo2(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  
  long b_cp = 0;

  /*----------- servo2引脚接线 -----------*/
  pinMode(26, OUTPUT);    //DIR
  pinMode(27, OUTPUT);    //CP
  pinMode(28, OUTPUT);    //EN
  digitalWrite(28, HIGH);  //EN on

  for (;;)
  {
    if (b_data == 'f')
    {
      
      digitalWrite(26, LOW);      //电机顺时针
      while (b_cp < b_number)
      {
        digitalWrite(27, HIGH);
        vTaskDelay( 2 / portTICK_PERIOD_MS  );
        digitalWrite(27, LOW);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        b_cp++;
        b_data='q';
      }
      b_cp = 0;
    }
    
    if (b_data == 'i')
    {
      
      digitalWrite(26, HIGH);      //电机逆时针
      while (b_cp < b_number)
      {
        digitalWrite(27, HIGH);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        digitalWrite(27, LOW);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        b_cp++;
      }
      b_cp = 0;
      b_data='q';
    }

    if (b_data == 'q')
    {
      digitalWrite(28, HIGH);  //EN on
    }
  
  vTaskDelay( 3 / portTICK_PERIOD_MS ); // 等待3ms
  }
}

/*------------------------------------------servo3-----------------------------------------*/
void Task_servo3(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  
  long c_cp = 0;
   
  /*----------- servo3引脚接线 -----------*/
  pinMode(30, OUTPUT);    //DIR
  pinMode(31, OUTPUT);    //CP
  pinMode(32, OUTPUT);    //EN
  digitalWrite(32, HIGH);  //EN on

  for (;;)
  {
    if (c_data == 'f')
    {
      
      digitalWrite(30, LOW);      //远离电机端
      while (c_cp < c_number)
      {
        digitalWrite(31, HIGH);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        digitalWrite(31, LOW);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        c_cp++;
      }
      c_cp = 0;
      c_data='q';
    }
    if (c_data == 'i')
    {
    
      digitalWrite(30, HIGH);      //靠近电机端
      while (c_cp < c_number)
      {
        digitalWrite(31, HIGH);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        digitalWrite(31, LOW);
        vTaskDelay( 2 / portTICK_PERIOD_MS );
        c_cp++;
      }
      c_cp = 0;
      c_data='q';
    }

    if (c_data == 'q')
    {
      digitalWrite(32, HIGH);  //EN on
    }
  
  vTaskDelay(3 / portTICK_PERIOD_MS); // 等待2ms
  }
}

说明：
每次指令是24位，每个电机都是8位；
1号电机逆时针转1000个脉冲，2 3号电机不转，指令：100010000000000000000000