单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 253|回复: 2
收起左侧

基于Arduino Mega2560工程训练竞赛物流小车设计

[复制链接]
阿拉景 发表于 2019-10-25 19:06 | 显示全部楼层 |阅读模式
去年夏天,我和队友参加了浙江省工程训练综合能力竞赛,取得了校赛第一名的成绩,随后代表学校参加浙江省赛,也取得满意的成绩。主控采用的是Mega2560,配合16路舵机控制板、条码识别传感器等。        
首先奉上关键动作的动图

                                                                     物流车.gif

                                                                                                           自主抓取过程


                                                                                  物流车放置.gif

                                                                                                   自主放置过程
程序流程图如左图所示。
  
思路面向任务要求:巡线——定位调整——装载物料——避障——防止物块——终点停车。
  
程序流程相合于顺序控制系统,前进过程中通过我们自己组合的激光点阵巡线系统,不断检测前方的赛道特征,实时反馈,并在不同的子程序中做出相应的处理,如进行高精度巡线、定位调整、弯道检测处理等。
  
流程图中多次出现“黑线是否为弯道特征”判断框,处理方法是:通过串口通讯的方式,实时反馈激光点阵的数据组,将得到的数据组进行均值滤波,并将滤波处理后数组的众数确定为弯道特征。
  
转向并高精度定位式巡线是保障装载卸载执行稳定的基础。高精度的定位才能确保吸盘抓取到最优的位置。同样依托于激光点阵系统,在上坡完成后即开始高精度的巡线,依托侧边传感器检测三条黑线用于定位停车,将定位的误差降低到极限。



    条码扫描及装载及卸载是整个程序的重中之重。
                              
结构图.png
                                      程序结构图
比赛结束一年啦,整理文件的时候,发现在这一过程中存了很多资料,这里可以给参与的小伙伴查阅.

单片机源程序如下:
  1. int speed  = 255, speed_for_avoid  = 255;
  2. int speed_for_judge = speed;

  3. /*//设定六个目标抓取物编号*/
  4. #define TARGET_DATA_ONE       "ZJGXDS01"                           //假定六个目标抓取物编号
  5. #define TARGET_DATA_TWO       "ZJGXDS03"
  6. #define TARGET_DATA_THREE     "ZJGXDS04"
  7. #define TARGET_DATA_FOUR      "ZJGXDS06"
  8. #define TARGET_DATA_FIVE      "ZJGXDS07"
  9. #define TARGET_DATA_SIX       "ZJGXDS09"

  10. #define SERVO_CONTROL_SPEED   10                                 //舵机速度

  11. #define FRAME_HEADER            0xFF              //帧头
  12. #define CMD_SERVO_SPEED         0x01              //设置舵机速度
  13. #define CMD_SERVO_PLACE         0x02              //设置舵机位置
  14. #define CMD_ACTION_GROUP_RUN    0x09              //运行动作组
  15. #define CMD_STOP_REFRESH        0x0b              //急停、恢复指令
  16. #define GET_LOW_BYTE(A) (uint8_t)((A))            //宏函数 获得A的低八位
  17. #define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)      //宏函数 获得A的高八位


  18. #define signal_light A15
  19. #define TrigPin2 A2    //定义第二超声波模块接口
  20. #define EchoPin2 A3

  21. /*-------------------定义前排灰度传感器接口 ---------------------*/

  22. #define sensor1 41
  23. #define sensor2 37
  24. #define sensor3 40
  25. #define sensor4 36
  26. #define sensor5 39
  27. #define sensor6 35
  28. #define sensor7 38
  29. #define sensor8 34


  30. /*------------------宏定义侧边传感器接口-----------------------*/
  31. #define side_r1 22
  32. #define side_r2 23

  33. /*--------------------宏定义双排激光传感器---------------------*/
  34. #define first_sensor1 52
  35. #define first_sensor2 28
  36. #define first_sensor3 48
  37. #define first_sensor4 33

  38. #define second_sensor1 50
  39. #define second_sensor2 51
  40. #define second_sensor3 24
  41. #define second_sensor4 32
  42. #define second_sensor5 27
  43. #define second_sensor6 29
  44. #define second_sensor7 26

  45. /*-----------------宏定义终点线检测传感器接口--------------------*/
  46. #define test_redline_r1 46
  47. #define test_redline_r2 47

  48. /*----------------宏定义气泵及电磁阀使能接口----------------------*/
  49. #define bump 30//低电平开机
  50. #define Electromagnetic_valve 31 //低电平吹出

  51. /*-------------------宏定义电机驱动接口----------------------------*/
  52. #define L1_IN1 8
  53. #define L1_IN2 9
  54. #define L2_IN1 10
  55. #define L2_IN2 11
  56. #define R1_IN1 12
  57. #define R1_IN2 13
  58. #define R2_IN1 6
  59. #define R2_IN2 7


  60. int com = 0;
  61. int fs1,fs2,fs3,fs4,ss1,ss2,ss3,ss4,ss5,ss6,ss7,all_fs,all_ss;

  62. unsigned char flag_for_judge = 0;
  63. int S1, S2, S3, S4, S5, S6, S7, S8, all_sensor, previous_all_sensor; //巡线传感器的值
  64. int SR1, SR2,BR1,BR2;                                 //定义侧边传感器及终点检测 传感器值

  65. int lineT = 0, unload_lineT = 0;                  //是否到达T字路口标志位
  66. int avo = 0;                                  //判断是否完成避障标志位
  67. int finish_all_task = 0;                        //判断比赛是否结束标志位
  68. int arrive_unload_place = 0;                          //判断是否到达卸货区
  69. unsigned long Time = 0, endtime = 0;                    //定时
  70. int ready_judge = 0;                            //判断是否前排红外是否经过T字路口

  71. const int BLACK = 0, WHITE = 1, t = 3;                  //定义黑白及小延时(用于稳定)




  72. unsigned int servo_init[7] = {135, 115, 20, 120, 140, 50, 75};                //舵机位置初始化数组 车在运行时的稳定位置
  73. unsigned int servo_previous_position[7] = {135, 115, 20, 120, 140, 50, 75};   //该数组用于存放之前的数据  初值与舵机初始化位置相同
  74. unsigned char servo_data[10];                                    //舵机指令存放数组
  75. //unsigned long endtime;                                           //与millis()配合使用
  76. int fifth_angle;
  77. int seventh_angle ;
  78. char target_count = -1;
  79. char scanner_count = 0;
  80. char servo_temp;                                                 //串口接收测试值
  81. String target_data[6] = {};                                      //目标取物数组



  82. String chstring = "";                                            //扫到的字符串
  83. int ch ;                                                         //数据临时缓存
  84.                                                    //数据临时缓存

  85. /***************************************************************************
  86.    函数功能:舵机串口命令输送
  87.    入口参数:舵机编号 舵机响应速度 舵机位置
  88.    返回值:无
  89. ***************************************************************************/
  90. void servo_command(unsigned char servo_ID, unsigned int servo_speed, unsigned int servo_position)
  91. {
  92.   servo_position = servo_position * 100 / 9 + 500;              //servo_position 对应舵机位置 换算公式 原始范围 0~180
  93.   //servo_speed 范围1~20   单位(9°/ s)
  94.   servo_data[0] = FRAME_HEADER;
  95.   servo_data[1] = CMD_SERVO_SPEED;
  96.   servo_data[2] = servo_ID;
  97.   servo_data[3] = GET_LOW_BYTE(servo_speed);
  98.   servo_data[4] = GET_HIGH_BYTE(servo_speed);
  99.   servo_data[5] = FRAME_HEADER;
  100.   servo_data[6] = CMD_SERVO_PLACE;
  101.   servo_data[7] = servo_ID;
  102.   servo_data[8] = GET_LOW_BYTE(servo_position);
  103.   servo_data[9] = GET_HIGH_BYTE(servo_position);
  104.   Serial3.write(servo_data, 10);
  105. }


  106. /***************************************************************************
  107.    函数功能:舵机控制
  108.    入口参数:无
  109.    返回值:无
  110. ***************************************************************************/

  111. /***************************************************************************
  112.    函数功能:舵机控制
  113.    入口参数:无
  114.    返回值:无
  115. ***************************************************************************/
  116. void servo_control(int D0, int D1, int D2, int D3, int D4, int D5, int D6)
  117. {
  118.   int servo_speed;                                                                           //舵机速度
  119.   int servo_control_counter;                                                                 //舵机控制次数
  120.   int servo_position[7] = {D0, D1, D2, D3, D4, D5, D6};                                         //存放一组舵机位置
  121.   int servo_position_D_value[7];                                                             //六个舵机 存放前后舵机位置差值
  122.   int servo_delay_time[7];                                                                   //舵机运行到位时间数组
  123.   float servo_position_D_value_max = 0;                                                      //舵机位置差值最大值
  124.   float servo_delay_time_max = 0 ;                                                           //舵机最大延时时间
  125.   fifth_angle = D4;
  126.   seventh_angle = D6;
  127.   for (servo_control_counter = 0; servo_control_counter < 7; servo_control_counter++)
  128.   {
  129.     servo_position_D_value[servo_control_counter] = servo_position[servo_control_counter] - servo_previous_position[servo_control_counter]; //得到前后两次位置的差值
  130.     if (servo_position_D_value[servo_control_counter] > 0)
  131.       servo_position_D_value[servo_control_counter] = servo_position_D_value[servo_control_counter];
  132.     else
  133.       servo_position_D_value[servo_control_counter] = - servo_position_D_value[servo_control_counter];
  134.     if (servo_position_D_value[servo_control_counter] > servo_position_D_value_max)                                        //得到差值最大值
  135.       servo_position_D_value_max = servo_position_D_value[servo_control_counter];
  136.     servo_previous_position[servo_control_counter] = servo_position[servo_control_counter];                                //使该次使用过的数据放入“先前数组”中
  137.   }

  138.   for (servo_control_counter = 0; servo_control_counter < 7; servo_control_counter++)
  139.   {
  140.     if (servo_position_D_value_max >= 60)
  141.       servo_speed = (servo_position_D_value[servo_control_counter] / servo_position_D_value_max) * 16;
  142.     else
  143.       servo_speed = (servo_position_D_value[servo_control_counter] / servo_position_D_value_max) * 10;
  144.     if (servo_position_D_value[servo_control_counter] > 140 && servo_control_counter == 1)
  145.       servo_speed = 20;
  146.     if (servo_speed > 20)
  147.       servo_speed = 20;
  148.     if (servo_speed <= 5)
  149.       servo_speed = 5;

  150.     servo_command(servo_control_counter, servo_speed, servo_position[servo_control_counter]);                               //发送舵机指令

  151.     servo_delay_time[servo_control_counter] =  (servo_position_D_value[servo_control_counter] * 1.00) / (9 * servo_speed) * 1000;
  152.     if (servo_delay_time[servo_control_counter] > servo_delay_time_max)
  153.       servo_delay_time_max = servo_delay_time[servo_control_counter];
  154.   }
  155.   delay(servo_delay_time_max);
  156. }
  157. /***************************************************************************
  158.    函数功能:舵机位置初始化
  159.    入口参数:无
  160.    返回值:无
  161. ***************************************************************************/
  162. void Servo_init()
  163. {
  164.   servo_control(135, 115, 20, 120, 140, 50, 75);                        //{135,135,0,175,161,80};
  165. }
  166. /***************************************************************************
  167.   函数功能:扫码触发命令
  168.   入口参数:无
  169.   返回值:无
  170. ***************************************************************************/

  171. void scanner_begin()
  172. {
  173.   Serial2.write(0x7E);              //串口2 发送触发指令使扫描仪开始工作
  174.   Serial2.write(0x00);
  175.   Serial2.write(0x08);
  176.   Serial2.write(0x01);
  177.   Serial2.write(0x00);
  178.   Serial2.write(0x02);
  179.   Serial2.write(0x01);
  180.   Serial2.write(0xAB);
  181.   Serial2.write(0xCD);
  182. }

  183.   /***************************************************************************
  184. * 函数功能:扫码功能
  185. * 入口参数:无
  186. * 返回值:无
  187. ***************************************************************************/
  188. void scanner_work2()
  189. {
  190.   scanner_begin();                        //触发扫描仪工作
  191.   endtime = millis();                     //开始扫描的时间点
  192.   while (1) {
  193.     if (millis() - endtime < 4000) {//直到扫到条形码 退出循环
  194.       if (millis() - endtime < 3200) {
  195.         if (millis() - endtime < 1500)
  196.           delay(300);
  197.         if (Serial2.available() - 7 > 0) { //如果串口接收到数据了
  198.           scanner_count++;                  //若检测到
  199.           break;
  200.         }
  201.         else {
  202.           servo_command(4, 2, fifth_angle + 2);
  203.           delay(300);
  204.           servo_command(4, 2, fifth_angle - 2);
  205.           delay(300);
  206.         }
  207.       }
  208.       else {
  209.         servo_command(6, 3, seventh_angle + 8);
  210.         delay(800);
  211.         servo_command(6, 3, seventh_angle - 8);
  212.         delay(800);
  213.       }
  214.     }
  215.     else
  216.     { scanner_count++;                       //若3秒内经调整依然无法扫到 下一个
  217.       chstring = "";
  218.       break;
  219.     }
  220.   }


  221.   while (Serial2.available() > 0)      //发送缓存区数据
  222.   {
  223.     ch = Serial2.read();
  224.     chstring += char(ch);
  225.     delay(2);
  226.   }
  227. }




  228. void scanner_work1()
  229. {
  230.   scanner_begin();                        //触发扫描仪工作
  231.   endtime = millis();                     //开始扫描的时间点
  232.   while (1) {
  233.     if (millis() - endtime < 4000) {//直到扫到条形码 退出循环
  234.       if (millis() - endtime < 3200) {
  235.         if (millis() - endtime < 1500)
  236.           delay(600);
  237.         if (Serial2.available() - 7 > 0) { //如果串口接收到数据了
  238.           scanner_count++;                  //若检测到
  239.           break;
  240.         }
  241.         else {
  242.           servo_command(4, 2, fifth_angle + 3);
  243.           delay(300);
  244.           servo_command(4, 2, fifth_angle - 3);
  245.           delay(300);
  246.         }
  247.       }
  248.       else {
  249.         servo_command(6, 3, seventh_angle + 8);
  250.         delay(500);
  251.         servo_command(6, 3, seventh_angle - 8);
  252.         delay(500);
  253.       }
  254.     }
  255.     else
  256.     { scanner_count++;                       //若3秒内经调整依然无法扫到 下一个
  257.       chstring = "";
  258.       break;
  259.     }
  260.   }


  261.   while (Serial2.available() > 0)      //发送缓存区数据
  262.   {
  263.     ch = Serial2.read();
  264.     chstring += char(ch);
  265.     delay(2);
  266.   }
  267. }
  268. /**————————
  269. 函数功能:测距
  270. 返回值:距离
  271. ————————*/

  272. float measure2() {
  273.   float d;
  274.   digitalWrite(TrigPin2, LOW);
  275.   delayMicroseconds(2);
  276.   digitalWrite(TrigPin2, HIGH);
  277.   delayMicroseconds(10);
  278.   digitalWrite(TrigPin2, LOW);
  279.   d = pulseIn(EchoPin2, HIGH) / 58.00;
  280.   return d;
  281. }



  282. /*——————————————————
  283. 函数功能:上料区高精度巡线
  284. ——————————————————*/
  285. void high_accuracy_stright_test_for_load()
  286. {
  287.         int flag = 0;
  288.         int minority_speed = 255,max_max = 255,max_min = 120;
  289.    while (lineT != 1)
  290.   {
  291.           if(flag == 0 && measure2() < 15 && measure2() < 15)               
  292.             {
  293.                         Time = millis();
  294.                         flag = 1;

  295.             }
  296.                 if(flag == 1 && ((millis()-Time) > 70))
  297.             {
  298.                     motor(-200,-200);
  299.                     delay(50);
  300.                     motor(0,0);
  301.                     //delay(1000);//停车1S用于观察
  302.                         minority_speed = 90;
  303.                         max_max = 200;
  304.                         max_min = 0;
  305.                         flag = 2;
  306.             }
  307.             
  308.         read_laser();
  309.     /*------第二排传感器已经到达目标状态-----小幅度调整------*/
  310.     if (ss1 == BLACK && ss2 == BLACK )
  311.     {
  312.       read_laser();
  313.        if (fs1 == WHITE && fs2 == BLACK) //当前状态微小左偏
  314.       {
  315.         motor(max_max, minority_speed);
  316.       }
  317.       else if (fs1 == BLACK && fs2 == WHITE) //当前状态微小右偏
  318.       {
  319.         motor(minority_speed, max_max );
  320.       }
  321.       else
  322.           {
  323.                   motor(minority_speed, minority_speed);
  324.       }
  325.     }
  326.    
  327.         /*—————大幅度调整———————————*/
  328.     else if (ss1 == WHITE && ss2 == BLACK) //当前状态较大左偏
  329.     {
  330.       motor( max_max,max_min );
  331.     }
  332.     else if (ss1 == BLACK && ss2 == WHITE ) //当前状态较大右偏
  333.     {
  334.       motor(max_min, max_max);
  335.     }
  336.     SR1 = !digitalRead(side_r1);
  337.     SR2 = !digitalRead(side_r2);
  338.     if (SR1 == BLACK && SR2 == BLACK && lineT == 0)
  339.       lineT = 1;       
  340.         }
  341. }



  342. void loading2()
  343. {
  344.   while (1)
  345.   {
  346.     if ((scanner_count > 9) || (target_count >= 5))           //若有效扫描次数超过10次,或已经抓取到6个对应物块,则退出该循环,机械臂回到初始位置
  347.     {
  348.       Servo_init();
  349.       break;
  350.     }

  351.     switch (scanner_count)
  352.     {
  353.       case (0): servo_control(108, 65, 17, 55, 138, 25, 74); delay(100); break; // 一号扫描位(112,81,23,89,118,28,75)
  354.       case (1): servo_control(114, 60, 18, 48, 130, 30, 75); delay(100);  break; //六号扫描位(117,55,15,50,140,30,75)
  355.       case (2): servo_control(125, 70, 39, 29, 149, 40, 80); delay(100);  break; // 七号扫描位(127,66,26,50,145,40,75);
  356.       case (3): servo_control(121, 82, 24, 78, 128, 39, 73); delay(100);  break; //  二号扫描位(123,110,45,100,151,57)
  357.       case (4): servo_control(137, 90, 22, 81, 134, 53, 78) ; delay(100);  break; //三号扫描位(138,119,45,110,152,81))
  358.       case (5): servo_control(136, 66, 20, 50, 144, 51, 78); delay(100); break; //八号扫描位(139,90,33,85,159,81)
  359.       case (6): servo_control(148, 65, 20, 50, 140, 65, 80); delay(100); break; //九号扫描位(152,94,42,80,155,104);
  360.       case (7): servo_control(151, 85, 18, 81, 132, 70, 78); delay(100); break; //四号扫描位(153,110,45,100,154,109)
  361.       case (8): servo_control(164, 78, 30, 56, 132, 83, 84); delay(100);  break; // 五号扫描位(168,102,50,80,158,125)
  362.       case (9): servo_control(161, 66, 35, 36, 131, 75, 90); delay(100); break; //十号扫描位(162,75,14,107,134,116)
  363.     }
  364.     //  delay(500);
  365.     scanner_work1();
  366.     if (chstring.length() > 7)
  367.     {
  368.       Serial.println(chstring.substring(7, 15));
  369.       chstring = chstring.substring(7, 15);
  370.       if (chstring.startsWith(TARGET_DATA_ONE) || chstring.startsWith(TARGET_DATA_TWO) || chstring.startsWith(TARGET_DATA_THREE)
  371.           || chstring.startsWith(TARGET_DATA_FOUR) || chstring.startsWith(TARGET_DATA_FIVE) || chstring.startsWith(TARGET_DATA_SIX)) //若为匹配数据
  372.       {
  373.         Serial.println("One of the equal string!");
  374.         target_count++;                                                             //target_count用于 决定存放于载物架的位置
  375.         target_data[target_count] = chstring;                                      //将取到的字符串放入 目标取物数组中
  376.         chstring = "";                                                             //字符串清零 等待下一次接收


  377.         switch (scanner_count)
  378.         {
  379.           case (1): {
  380.               //1号位置抓取
  381.               servo_control(109, 65, 18, 57, 139, 22, 74);
  382.               servo_control(107, 65, 23, 60, 140, 22, 74);
  383.               servo_control(107, 68, 28, 60, 141, 22, 74);
  384.               servo_control(106, 68, 33, 58, 143, 22, 74);
  385.               servo_control(105, 68, 36, 58, 140, 22, 74);
  386.               servo_control(105, 68, 40, 58, 136, 22, 74);
  387.               servo_control(105, 68, 45, 58, 136, 22, 74);
  388.               digitalWrite(Electromagnetic_valve, HIGH);
  389.               //1号位置放置
  390.               servo_control(106, 72, 45, 56, 136, 22, 74);
  391.               servo_control(106, 72, 39, 58, 134, 22, 74);
  392.               servo_control(106, 70, 36, 58, 136, 22, 74);
  393.               servo_control(106, 68, 33, 58, 139, 22, 74);
  394.               servo_control(107, 68, 28, 60, 139, 22, 74);
  395.               servo_control(107, 65, 23, 60, 139, 22, 74);            //回去
  396.               servo_control(102, 65, 23, 60, 139, 22, 74);
  397.               servo_control(97, 65, 23, 60, 139, 22, 74);             //向右转
  398.               servo_control(97, 75, 23, 75, 139, 22, 74);              //收到下一个置物架旋转点
  399.               servo_control(97, 90, 23, 90, 139, 22, 74);
  400. ……………………

  401. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
物流车省赛源码等资料.zip (54.41 KB, 下载次数: 13)
回复

使用道具 举报

1261887391 发表于 2019-11-5 01:03 来自手机 | 显示全部楼层
方便仔细了解一下吗?
回复

使用道具 举报

 楼主| 阿拉景 发表于 2019-11-5 09:52 | 显示全部楼层
1261887391 发表于 2019-11-5 01:03
方便仔细了解一下吗?

下载一下,先看一看能不能理解
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|单片机论坛 |51黑电子论坛技术交流 QQ 管理员QQ:125739409;技术交流QQ群636986012

Powered by 单片机教程网

快速回复 返回顶部 返回列表
妞干网免费观看频2018