如何让小型云台机械手实现按颜色分拣物品？
1. 功能说明 在小型云台机械手附近设置一个工作台，并安装一个颜色识别传感器【https://www.robotway.com/h-col-137.html】。将红色、蓝色工件分别放置在传感器上，如果检测的物料的颜色为红色，机械臂将物体放在机械臂的左侧，如果检测的物料的颜色为蓝色，机械臂将物体放在机械臂的右侧，否则，机械臂不动作。 2. 使用样机本实验使用的样机是用探索者兼容零件制作的。3. 功能实现3.1 电子硬件在这个示例中，采用了以下硬件，请大家参考：
将夹爪、腕关节、底座关节的舵机分别接在扩展板的D4、D7以及D11舵机接口上，颜色传感器接在A0、A4、A3口上。
3.2 编写程序编写并烧录以下程序（Color_Sorting_Robot.ino），该程序将实现演示视频中的动作。编程环境：Arduino 1.8.19
/*******************************************************************************************版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved. Distributed under MIT license.See file LICENSE for detail or copy at https://opensource.org/licenses/MIT by 机器谱 2022-12-21 https://www.robotway.com/---------------------------------------------------------------------------------------实验需求: 用颜色传感器实现颜色识别。
实现思路： 程序的整体思路为：在机械臂前方安装颜色传感器，如果检测的物料的颜色为红色，机械臂将 物体放在机械臂的左侧，如果检测的物料的颜色为蓝色，机械臂将物体放在机械臂的右侧， 否则，机械臂不动作。实验接线: 最上端的机械爪舵机接D4; 中间的机械身躯舵机接D7； 最下端的机械底座舵机接D11； 颜色传感器的接线为 S1 S2 5V GND S3 S2 5V GND OUT LED 5V GND | | | | | | | | | | | | A0 A1 5V GND A5 A4 5V GND D2 A3 5V GND********************************************************************************************/
//颜色传感器原理/*首先进行白平衡，把一个白色物体放置在TCS3200颜色传感器之下，两者相距10mm左右，点亮传感器上的4个白光LED灯，用Arduino控制器的定时器设置一固定时间1s，然后选通三种颜色的滤波器，让被测物体反射光中红、绿、蓝三色光分别通过滤波器，计算1s时间内三色光分别对应的TCS3200的输出脉冲数，再通过算式得到白色物体RGB值255与三色光脉冲数的比例因子。有了白平衡后，得到的RGB比例因子，则其他颜色物体反射光中红、绿、蓝三色光对应的1s内TCS3200输出信号脉冲数乘以R、G、B比例因子，就可换算出被测物体的RGB标准值。*/
#include "TimerOne.h" //颜色传感器需要用到的定时函数库#include<ServoTimer2.h> //舵机驱动需要的函数库ServoTimer2 myservo; //舵机声明#define servo_num 3 //舵机数量#define Servo_Speed 20 //舵机速度
#define Upward_servo_close 66 //机械爪闭合的角度值#define Upward_servo_open 115 //机械爪张开的角度值
#define Middle_servo_down 105 //机械臂的初始角#define Middle_servo_init 85 //机械臂的初始角#define Middle_servo_left 10 //机械臂向左偏的角度#define Middle_servo_left1 50 //机械臂向左偏的角度
#define Down_servo_middle 75 //机械底座初始角度值#define Down_servo_left 5 //机械底座向左偏的角度值#define Down_servo_right 145 //机械底座向右偏的角度值
int servo_pin={4,7,11}; //定义舵机引脚号float value_init={Upward_servo_open, Middle_servo_left, Down_servo_middle};//舵机初始角度int f=20; //舵机从角度A转到角度B分的分数
//把TCS3200颜色传感器各控制引脚连到Arduino数字端口#define S0 A0 //物体表面的反射光越强，TCS3002D的内置振荡器产生的方波频率越高，#define S1 A1 //S0和S1的组合决定输出信号频率比率因子，比例因子为2% //比率因子为TCS3200传感器OUT引脚输出信号频率与其内置振荡器频率之比#define S2 A4 //S2和S3的组合决定让红、绿、蓝，哪种光线通过滤波器#define S3 A5#define OUT 2 //TCS3200颜色传感器输出信号输入到Arduino中断0引脚，并引发脉冲信号中断 //在中断函数中记录TCS3200输出信号的脉冲个数#define LED A3 //控制TCS3200颜色传感器是否点亮int g_count = 0; // 计算与反射光强相对应TCS3200颜色传感器输出信号的脉冲数// 数组存储在1s内TCS3200输出信号的脉冲数，它乘以RGB比例因子就是RGB标准值int g_array; int g_flag = 0; //滤波器模式选择顺序标志float g_SF; // 存储从TCS3200输出信号的脉冲数转换为RGB标准值的RGB比例因子
// 初始化TSC3200各控制引脚的输入输出模式//设置TCS3002D的内置振荡器方波频率与其输出信号频率的比例因子为2%void TSC_Init(){pinMode(S0, OUTPUT);pinMode(S1, OUTPUT);pinMode(S2, OUTPUT);pinMode(S3, OUTPUT);pinMode(OUT, INPUT);pinMode(LED, OUTPUT);digitalWrite(S0, LOW);digitalWrite(S1, HIGH);}
//选择滤波器模式，决定让红、绿、蓝，哪种光线通过滤波器void TSC_FilterColor(int Level01, int Level02){if(Level01 != LOW) Level01 = HIGH;if(Level02 != LOW) Level02 = HIGH;digitalWrite(S2, Level01);digitalWrite(S3, Level02);}//中断函数，计算TCS3200输出信号的脉冲数void TSC_Count(){g_count ++ ;}//定时器中断函数，每1s中断后，把该时间内的红、绿、蓝三种光线通过滤波器时，//TCS3200输出信号脉冲个数分别存储到数组g_array的相应元素变量中void TSC_Callback(){switch(g_flag){ case 0: TSC_WB(LOW, LOW); //选择让红色光线通过滤波器的模式 break; case 1: g_array = g_count; //存储1s内的红光通过滤波器时，TCS3200输出的脉冲个数 TSC_WB(HIGH, HIGH); //选择让绿色光线通过滤波器的模式 break; case 2: g_array = g_count; //存储1s内的绿光通过滤波器时，TCS3200输出的脉冲个数 TSC_WB(LOW, HIGH); //选择让蓝色光线通过滤波器的模式 break;
case 3: g_array = g_count; //存储1s内的蓝光通过滤波器时，TCS3200输出的脉冲个数 TSC_WB(HIGH, LOW); //选择无滤波器的模式 break; default: g_count = 0; //计数值清零 break;}}//设置反射光中红、绿、蓝三色光分别通过滤波器时如何处理数据的标志//该函数被TSC_Callback( )调用void TSC_WB(int Level0, int Level1) {g_count = 0; //计数值清零g_flag ++; //输出信号计数标志TSC_FilterColor(Level0, Level1); //滤波器模式Timer1.setPeriod(100000); //设置输出信号脉冲计数时长1s}//初始化void setup(){TSC_Init();Serial.begin(9600); //启动串行通信Timer1.initialize(100000); // defaulte is 1sTimer1.attachInterrupt(TSC_Callback); //设置定时器1的中断，中断调用函数为TSC_Callback()//设置TCS3200输出信号的上跳沿触发中断，中断调用函数为TSC_Count()attachInterrupt(0, TSC_Count, RISING);digitalWrite(LED, HIGH);//点亮LED灯// delay(1500); //延时4s，以等待被测物体红、绿、蓝三色在1s内的TCS3200输出信号脉冲计数//通过白平衡测试，计算得到白色物体RGB值255与1s内三色光脉冲数的RGB比例因子
g_SF = 0.53; //红色光比例因子g_SF = 0.65; //绿色光比例因子g_SF = 0.54; //蓝色光比例因子//红、绿、蓝三色光对应的1s内TCS3200输出脉冲数乘以相应的比例因子就是RGB标准值 reset();}//主程序int Now_Color = 0; //存储上一次颜色传感器检测的数值int Last_Color = 0; //存储当前颜色传感器检测的数值
void loop(){ Last_Color = Color_Detection(); Now_Color = Color_Detection(); if( Last_Color == Now_Color) //如果两次检测的数值相同 //（这里是为了防止颜色传感器检测出错，所以检测了两次） { switch(Now_Color) { case 1: Serial.print("Red"); //如果检测到的物料为红色，将物料放到机械臂的左侧 Servo_Left(); Now_Color = 0; Last_Color = 0; break; case 2: Serial.print("Blue");//如果检测到的物料为蓝色，将物料放到机械臂的右侧 Servo_Right(); Now_Color = 0; Last_Color = 0; break; case 3: Serial.print("NONE");//否则，机械臂不动作； Serial.println(); Now_Color = 0; Last_Color = 0; break; } }}
int Color_Detection() //颜色检测函数{ int color; g_flag = 0; for(int i=0; i<3; i++) { color = g_array * g_SF; } Serial.println((String)(color) + '+' + (String)(color) + '+' + (String)(color) + '+'); delay(500); if( (color > color) && (color >color) && ( (color+color)<color ) ){ return 1; //如果检测到的颜色为红色，返回1； } else if( (color > color) && (color >color) ){ return 2; //如果检测到的颜色为蓝色，返回2； } else { return 3; } //否则，机械臂不动作；}
void reset() //舵机角度初始化{ for(int i=0;i<servo_num;i++){ myservo.attach(servo_pin); myservo.write(map(value_init,0,180,500,2500));}}
void servo_move(float value0, float value1, float value2) //舵机转动 {float value_arguments = {value0, value1, value2};float value_delta;
for(int i=0;i<servo_num;i++){ value_delta = (value_arguments - value_init) / f;}
for(int i=0;i<f;i++){ for(int k=0;k<servo_num;k++) { value_init = value_delta == 0 ? value_arguments : value_init + value_delta; }
for(int j=0;j<servo_num;j++) { myservo.write(map(value_init,0,180,500,2500)); delay(Servo_Speed); }}}
void Servo_Left() //将物料放到机械臂的左侧{servo_move(Upward_servo_open, Middle_servo_left, Down_servo_middle);//初始化动作servo_move(Upward_servo_open, Middle_servo_init, Down_servo_middle);//机械臂下降servo_move(Upward_servo_close, Middle_servo_init, Down_servo_middle);//机械爪闭合（抓取货物）servo_move(Upward_servo_close, Middle_servo_left1, Down_servo_middle);//机械臂上抬servo_move(Upward_servo_close, Middle_servo_down, Down_servo_left); //机械臂下降，机械底座向左转servo_move(Upward_servo_open, Middle_servo_down, Down_servo_left); //机械爪张开（释放货物）servo_move(Upward_servo_open, Middle_servo_left, Down_servo_middle);//机械臂回复到初始角度}
void Servo_Right() //将物料放到机械臂的右侧{servo_move(Upward_servo_open, Middle_servo_left, Down_servo_middle);//初始化动作servo_move(Upward_servo_open, Middle_servo_init, Down_servo_middle);servo_move(Upward_servo_close, Middle_servo_init, Down_servo_middle);servo_move(Upward_servo_close, Middle_servo_left1, Down_servo_middle);servo_move(Upward_servo_close, Middle_servo_down, Down_servo_right);servo_move(Upward_servo_open, Middle_servo_down, Down_servo_right);servo_move(Upward_servo_open, Middle_servo_left, Down_servo_middle);}
4. 资料内容① 按颜色分拣-例程源代码② 按颜色分拣-样机3D文件
资料下载链接 https://www.robotway.com/h-col-187.html
页:
[1]