Mega2560同时控制三个步进电机

参考

硬件接线

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储存的步数给numberxQueueSend( a_num, &number, 1);//把步数发给a_numnumber = 0;xQueueSend( a_dir, &data2, 1);//把方向发给a_dirdata2= ' ';}/****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);    //CPpinMode(24, OUTPUT);    //ENdigitalWrite(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);    //DIRpinMode(27, OUTPUT);    //CPpinMode(28, OUTPUT);    //ENdigitalWrite(28, LOW);  //EN OFFfor (;;){if (xQueueReceive( b_dir, &b_data, 3) ){xQueueReceive( b_num, &b_number, 3);Serial.println(b_data);if (b_data == 'f'){digitalWrite(28, HIGH);     //ENdigitalWrite(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);     //ENdigitalWrite(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);    //DIRpinMode(31, OUTPUT);    //CPpinMode(32, OUTPUT);    //ENdigitalWrite(32, LOW);  //EN OFFfor (;;){if (xQueueReceive( c_dir, &c_data, 3) ){xQueueReceive( c_num, &c_number, 3);if (c_data == 'f'){digitalWrite(32, HIGH);     //ENdigitalWrite(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);     //ENdigitalWrite(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

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

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

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

代码更新

最终版，用在跟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);    //CPpinMode(24, OUTPUT);    //ENdigitalWrite(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);    //DIRpinMode(27, OUTPUT);    //CPpinMode(28, OUTPUT);    //ENdigitalWrite(28, HIGH);  //EN onfor (;;){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);    //DIRpinMode(31, OUTPUT);    //CPpinMode(32, OUTPUT);    //ENdigitalWrite(32, HIGH);  //EN onfor (;;){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号电机不转，指令：00