极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 2240|回复: 0

如何实现4自由度串联机械臂按颜色分拣物品?

[复制链接]
发表于 2023-3-9 09:14:04 | 显示全部楼层 |阅读模式
本帖最后由 机器谱 于 2023-3-9 09:14 编辑

1. 功能说明
本实验要实现的功能是:将黑、白两种颜色的工件分别放置在传感器上时,机械臂会根据检测到的颜色,将工件搬运至写有相应颜色字样区域。


2. 使用样机
本实验使用的样机为R204样机4自由度串联机械臂。
3. 运动功能实现
3.1 电子硬件
在这个示例中,我们采用了以下硬件,请大家参考:

主控板Basra(兼容Arduino Uno)
扩展板Bigfish2.1
传感器TCS3200颜色识别
电池7.4V锂电池

在4自由度串联机械臂底座上安装一个TCS3200颜色识别传感器,用于检测工件的RGB值。

3.2 编写程序
编程环境:Arduino 1.8.19
可以事先利用TCS3200颜色识别传感器检测一下两个工件的的颜色数据,根据颜色数据的特征来确定判断语句的写法,尽量利用R、G、B数值中差别最大的那个作为区分颜色的主要依据。TCS3200颜色识别传感器的检测方法可以参考 TCS3200颜色识别传感器
编写并烧录以下程序(Color_test.ino),该程序将实现演示视频中的动作。
  1. /*------------------------------------------------------------------------------------

  2.   版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

  3.            Distributed under MIT license.See file LICENSE for detail or copy at

  4.            https://opensource.org/licenses/MIT

  5.            by 机器谱 2023-02-03 https://www.robotway.com/

  6.   ------------------------------

  7.   实验接线:舵机接口依次D4、D7、D11、D3、D8; 颜色传感器接在A0、A2、A3口上                                    

  8. ------------------------------------------------------------------------------------*/

  9. /*I will sorting black and white things according to the color_test Code.

  10. Put the black things in the left.Put the white thing int the right*/

  11. #include <MsTimer2.h>

  12. //把TCS3200颜色传感器各控制引脚连到Arduino数字端口

  13. #define S0     A0   //物体表面的反射光越强,TCS3002D的内置振荡器产生的方波频率越高,

  14. #define S1     A1   //S0和S1的组合决定输出信号频率比率因子,,比例因子为2%

  15.                    //比率因子为TCS3200传感器OUT引脚输出信号频率与其内置振荡器频率之比

  16. #define S2     A2   //S2和S3的组合决定让红、绿、蓝,哪种光线通过滤波器

  17. #define S3     0

  18. #define OUT    2   //TCS3200颜色传感器输出信号输入到Arduino中断0引脚,并引发脉冲信号中断

  19.                   //在中断函数中记录TCS3200输出信号的脉冲个数

  20. #define LED    A3   //控制TCS3200颜色传感器是否点亮

  21. int   g_count = 0;    // 计算与反射光强相对应TCS3200颜色传感器输出信号的脉冲数

  22. // 数组存储在1s内TCS3200输出信号的脉冲数,它乘以RGB比例因子就是RGB标准值

  23. int   g_array[3];   

  24. int   g_flag = 0;     //滤波器模式选择顺序标志

  25. float g_SF[3];       // 存储从TCS3200输出信号的脉冲数转换为RGB标准值的RGB比例因子

  26. int color=0;



  27. // 初始化TSC3200各控制引脚的输入输出模式

  28. //设置TCS3002D的内置振荡器方波频率与其输出信号频率的比例因子为2%





  29. int a=0,b=0,c=0,d=0,e=0,f=0;

  30. #include <Servo.h>

  31. Servo servo_pin_4;

  32. Servo servo_pin_7;

  33. Servo servo_pin_11;

  34. Servo servo_pin_3;

  35. Servo servo_pin_8;

  36. void TSC_Init()

  37. {

  38.   pinMode(S0, OUTPUT);

  39.   pinMode(S1, OUTPUT);

  40.   pinMode(S2, OUTPUT);

  41.   pinMode(S3, OUTPUT);

  42.   pinMode(OUT, INPUT);

  43.   pinMode(LED, OUTPUT);

  44.   digitalWrite(S0, LOW);  

  45.   digitalWrite(S1, HIGH);

  46. }

  47. //选择滤波器模式,决定让红、绿、蓝,哪种光线通过滤波器

  48. void TSC_FilterColor(int Level01, int Level02)

  49. {

  50.   if(Level01 != 0)

  51.     Level01 = HIGH;

  52.   if(Level02 != 0)

  53.     Level02 = HIGH;

  54.   digitalWrite(S2, Level01);

  55.   digitalWrite(S3, Level02);

  56. }

  57. //中断函数,计算TCS3200输出信号的脉冲数

  58. void TSC_Count()

  59. {

  60.   g_count ++ ;

  61. }

  62. //定时器中断函数,每1s中断后,把该时间内的红、绿、蓝三种光线通过滤波器时,

  63. //TCS3200输出信号脉冲个数分别存储到数组g_array[3]的相应元素变量中

  64. void TSC_Callback()

  65. {

  66.   switch(g_flag)

  67.   {

  68.     case 0:

  69.          Serial.println("->WB Start");

  70.          TSC_WB(LOW, LOW);              //选择让红色光线通过滤波器的模式

  71.          break;

  72.     case 1:

  73.          Serial.print("->Frequency R=");

  74.          Serial.println(g_count);   //打印1s内的红光通过滤波器时,TCS3200输出的脉冲个数

  75.          g_array[0] = g_count;       //存储1s内的红光通过滤波器时,TCS3200输出的脉冲个数

  76.          TSC_WB(HIGH, HIGH);         //选择让绿色光线通过滤波器的模式

  77.          break;

  78.     case 2:

  79.          Serial.print("->Frequency G=");

  80.          Serial.println(g_count);   //打印1s内的绿光通过滤波器时,TCS3200输出的脉冲个数

  81.          g_array[1] = g_count;       //存储1s内的绿光通过滤波器时,TCS3200输出的脉冲个数

  82.          TSC_WB(LOW, HIGH);          //选择让蓝色光线通过滤波器的模式

  83.          break;

  84.     case 3:

  85.          Serial.print("->Frequency B=");

  86.          Serial.println(g_count);   //打印1s内的蓝光通过滤波器时,TCS3200输出的脉冲个数

  87.          Serial.println("->WB End");

  88.          g_array[2] = g_count;       //存储1s内的蓝光通过滤波器时,TCS3200输出的脉冲个数

  89.          TSC_WB(HIGH, LOW);             //选择无滤波器的模式   

  90.          break;

  91.    default:

  92.          g_count = 0;    //计数值清零

  93.          break;

  94.   }

  95. }

  96. //设置反射光中红、绿、蓝三色光分别通过滤波器时如何处理数据的标志

  97. //该函数被TSC_Callback( )调用

  98. void TSC_WB(int Level0, int Level1)     

  99. {

  100.   g_count = 0;   //计数值清零

  101.   g_flag ++;     //输出信号计数标志

  102.   TSC_FilterColor(Level0, Level1); //滤波器模式

  103. // Timer2.setPeriod(100000);      //设置输出信号脉冲计数时长1s

  104. }

  105. //初始化

  106. void grab_put_left()

  107. {   for(e=70;e>=50;e-=1)           

  108.   {servo_pin_8.write(e);delay(30);}  

  109.    

  110.   for(d=158;d>=36;d-=3)

  111.   {servo_pin_3.write(d);delay(30);}

  112.    

  113.   for(c=68;c<142;c+=3)

  114.   {servo_pin_11.write(c);delay(30);}

  115.    

  116.    for(e=50;e<=70;e+=1)

  117.   {servo_pin_8.write(e);delay(30);}

  118.    

  119.   for(c=142;c>=103;c-=3)

  120.   {servo_pin_11.write(c);delay(30);}



  121.   for(a=76;a<=120;a+=3)

  122.   {servo_pin_4.write(a); delay(30);}





  123.   for(e=70;e>=50;e-=1)

  124.   {servo_pin_8.write(e);delay(30);}



  125.   for(a=120;a>=76;a-=3)

  126.   {servo_pin_4.write(a); delay(30);}



  127.    for(c=103;c>=68;c-=3)

  128.   {servo_pin_11.write(c);delay(30);}



  129.     for(d=36;d<=157;d+=3)

  130.   {servo_pin_3.write(d);delay(30);}



  131.     for(e=50;e<=70;e+=1)

  132.   {servo_pin_8.write(e);delay(30);}

  133. delay(1000);



  134. }

  135. void grab_put_right()

  136. {   for(e=70;e>=50;e-=1)           

  137.   {servo_pin_8.write(e);delay(30);}  

  138.    

  139.   for(d=158;d>=36;d-=3)

  140.   {servo_pin_3.write(d);delay(30);}

  141.    

  142.   for(c=68;c<142;c+=3)

  143.   {servo_pin_11.write(c);delay(30);}

  144.    

  145.    for(e=50;e<=70;e+=1)

  146.   {servo_pin_8.write(e);delay(30);}

  147.    

  148.   for(c=142;c>=103;c-=3)

  149.   {servo_pin_11.write(c);delay(30);}



  150.   for(a=76;a>=32;a-=3)

  151.   {servo_pin_4.write(a); delay(30);}





  152.   for(e=70;e>=50;e-=1)

  153.   {servo_pin_8.write(e);delay(30);}



  154.   for(a=32;a<=76;a+=3)

  155.   {servo_pin_4.write(a); delay(30);}



  156.    for(c=103;c>=68;c-=3)

  157.   {servo_pin_11.write(c);delay(30);}



  158.     for(d=36;d<=157;d+=3)

  159.   {servo_pin_3.write(d);delay(30);}



  160.     for(e=50;e<=70;e+=1)

  161.   {servo_pin_8.write(e);delay(30);}



  162. delay(1000);

  163. }

  164. void setup()

  165. {servo_pin_4.attach(4);

  166.   servo_pin_4.write( 76);

  167.   servo_pin_7.attach(7);

  168.   servo_pin_7.write( 110);

  169.   servo_pin_11.attach(11);

  170.   servo_pin_11.write(68);

  171.   servo_pin_3.attach(3);

  172.   servo_pin_3.write(157);

  173.   servo_pin_8.attach(8);

  174.   servo_pin_8.write(71);

  175.   delay(3000);//set up the initial posotion.Each servo is different,

  176.               // so u must use Software of Processing to monitor your initial positions of servo.

  177.   TSC_Init();

  178.   Serial.begin(9600); //启动串行通信

  179.   MsTimer2::set(2000,TSC_Callback); // 500ms period

  180.   MsTimer2::start();

  181. // Timer1.initialize();   // defaulte is 1s

  182. //   Timer1.attachInterrupt(TSC_Callback); //设置定时器1的中断,中断调用函数为TSC_Callback()

  183.   //设置TCS3200输出信号的上跳沿触发中断,中断调用函数为TSC_Count()

  184.   attachInterrupt(0, TSC_Count,CHANGE);  

  185.   digitalWrite(LED, HIGH);//点亮LED灯

  186.   delay(2000); //延时4s,以等待被测物体红、绿、蓝三色在1s内的TCS3200输出信号脉冲计数

  187.   //通过白平衡测试,计算得到白色物体RGB值255与1s内三色光脉冲数的RGB比例因子

  188.   g_SF[0] =0.04800;//255.0/ g_array[0];     //红色光比例因子

  189.   g_SF[1] =0.05065;// 255.0/ g_array[1] ;    //绿色光比例因子

  190.   g_SF[2] =0.04104;// 255.0/ g_array[2] ;    //蓝色光比例因子

  191.   //打印白平衡后的红、绿、蓝三色的RGB比例因子

  192.   Serial.println(g_SF[0],5);

  193.   Serial.println(g_SF[1],5);

  194.   Serial.println(g_SF[2],5);

  195.   //红、绿、蓝三色光对应的1s内TCS3200输出脉冲数乘以相应的比例因子就是RGB标准值

  196.   //打印被测物体的RGB值

  197.   for(int i=0; i<3; i++)

  198.     Serial.println(int(g_array[i] * g_SF[i]));

  199. //   int color=g_array[2] * g_SF[2];

  200. }

  201. //主程序

  202. void loop()

  203. { int a=76;b=110;c=68;d=157;e=81;

  204.   servo_pin_4.write(a);

  205.   servo_pin_7.write(b);

  206.   servo_pin_11.write(c);

  207.   servo_pin_3.write(d);

  208.   servo_pin_8.write(e);

  209.    g_flag = 0;

  210.    //每获得一次被测物体RGB颜色值需时4s

  211.   // delay(4000);

  212.    //打印出被测物体RGB颜色值

  213.    for(int i=0; i<3; i++)     

  214.     Serial.println(int(g_array[i] * g_SF[i]));  

  215.    int color=g_array[2] * g_SF[2];           

  216. Serial.println(color);

  217. if(color>100)



  218. grab_put_right();

  219. if(color<=100)



  220. grab_put_left();

  221. //delay(5000);

  222. }
复制代码

4. 资料下载

资料内容:【R204】-按颜色分拣-例程源代码
​资料下载地址:https://www.robotway.com/h-col-189.html

想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com

回复

使用道具 举报

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

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 20:53 , Processed in 0.047523 second(s), 17 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表