如何让小型云台机械手实现按颜色分拣物品？
1. 功能说明
在小型云台机械手附近设置一个工作台，并安装一个颜色识别传感器。将红色、蓝色工件分别放置在传感器上，如果检测的物料的颜色为红色，机械臂将物体放在机械臂的左侧，如果检测的物料的颜色为蓝色，机械臂将物体放在机械臂的右侧，否则，机械臂不动作。
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 myservo[3]; //舵机声明
#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[3]={4,7,11}; //定义舵机引脚号
float value_init[3]={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[3];
int g_flag = 0; //滤波器模式选择顺序标志
float g_sf[3]; // 存储从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[3]的相应元素变量中
void tsc_callback()
{
switch(g_flag)
{
case 0:
tsc_wb(low, low); //选择让红色光线通过滤波器的模式
break;
case 1:
g_array[0] = g_count; //存储1s内的红光通过滤波器时，tcs3200输出的脉冲个数
tsc_wb(high, high); //选择让绿色光线通过滤波器的模式
break;
case 2:
g_array[1] = g_count; //存储1s内的绿光通过滤波器时，tcs3200输出的脉冲个数
tsc_wb(low, high); //选择让蓝色光线通过滤波器的模式
break;
case 3:
g_array[2] = 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 1s
timer1.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] = 0.53; //红色光比例因子
g_sf[1] = 0.65; //绿色光比例因子
g_sf[2] = 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[3];
g_flag = 0;
for(int i=0; i color[1]) && (color[0] >color[2]) && ( (color[1]+color[2])
return 1; //如果检测到的颜色为红色，返回1；
}
else if( (color[2] > color[1]) && (color[2] >color[0]) ){
return 2; //如果检测到的颜色为蓝色，返回2；
}
else { return 3; } //否则，机械臂不动作；
}
void reset() //舵机角度初始化
{
for(int i=0;i
{
myservo[i].attach(servo_pin[i]);
myservo[i].write(map(value_init[i],0,180,500,2500));
}
}
void servo_move(float value0, float value1, float value2) //舵机转动
{
float value_arguments[3] = {value0, value1, value2};
float value_delta[servo_num];
for(int i=0;i
{
value_delta[i] = (value_arguments[i] - value_init[i]) / f;
}
for(int i=0;i
{
for(int k=0;k
{
value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];
}
for(int j=0;j
{
myservo[j].write(map(value_init[j],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);
}
小米推出8周年纪念版手机
真的来了！官方曝定制版小米手环3腕带上将镭雕“ARE YOU OK？”
借助AI在网络边缘训练设备的价值
INA116基本连接电路
XP Power推出新款500W-650W AC-DC电源，满足医疗设备（包括BF）应用
如何让小型云台机械手实现按颜色分拣物品？
华为5G专利申请量全球第一
2022~2028年LED照明行业市场研究及发展趋势
PCB：PCB领域的加税主要针对电视机的PCB及生产设备
iphone8被曝光提前6月量产 三星S8渲染图曝光
PCB电路板和芯板的制作
ADI直接数字频率合成器有哪些，其应用领域是什么
gsm是什么网络_gsm网络是什么意思
DP7344 192K 双通道24位DA转换器芯片
重磅发布！浪潮高性能分布式存储平台AS15000G7加速产业数智化变革
西门子PLC S7-300出现通讯故障及远程维护办法
特拉维夫大学使用RFID和GPS技术了解蝙蝠行为 从而帮助人类思维研究
三星S8再曝光，超高屏占比无Home键
DC-DC变换器ADP3000的引脚功能介绍
辅助蓝牙网关Iot-Box组网 实现低成本低精度区域定位蓝牙覆盖
在小型云台机械手附近设置一个工作台，并安装一个颜色识别传感器。将红色、蓝色工件分别放置在传感器上，如果检测的物料的颜色为红色，机械臂将物体放在机械臂的左侧，如果检测的物料的颜色为蓝色，机械臂将物体放在机械臂的右侧，否则，机械臂不动作。
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 myservo[3]; //舵机声明
#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[3]={4,7,11}; //定义舵机引脚号
float value_init[3]={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[3];
int g_flag = 0; //滤波器模式选择顺序标志
float g_sf[3]; // 存储从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[3]的相应元素变量中
void tsc_callback()
{
switch(g_flag)
{
case 0:
tsc_wb(low, low); //选择让红色光线通过滤波器的模式
break;
case 1:
g_array[0] = g_count; //存储1s内的红光通过滤波器时，tcs3200输出的脉冲个数
tsc_wb(high, high); //选择让绿色光线通过滤波器的模式
break;
case 2:
g_array[1] = g_count; //存储1s内的绿光通过滤波器时，tcs3200输出的脉冲个数
tsc_wb(low, high); //选择让蓝色光线通过滤波器的模式
break;
case 3:
g_array[2] = 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 1s
timer1.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] = 0.53; //红色光比例因子
g_sf[1] = 0.65; //绿色光比例因子
g_sf[2] = 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[3];
g_flag = 0;
for(int i=0; i color[1]) && (color[0] >color[2]) && ( (color[1]+color[2])
return 1; //如果检测到的颜色为红色，返回1；
}
else if( (color[2] > color[1]) && (color[2] >color[0]) ){
return 2; //如果检测到的颜色为蓝色，返回2；
}
else { return 3; } //否则，机械臂不动作；
}
void reset() //舵机角度初始化
{
for(int i=0;i
{
myservo[i].attach(servo_pin[i]);
myservo[i].write(map(value_init[i],0,180,500,2500));
}
}
void servo_move(float value0, float value1, float value2) //舵机转动
{
float value_arguments[3] = {value0, value1, value2};
float value_delta[servo_num];
for(int i=0;i
{
value_delta[i] = (value_arguments[i] - value_init[i]) / f;
}
for(int i=0;i
{
for(int k=0;k
{
value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];
}
for(int j=0;j
{
myservo[j].write(map(value_init[j],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);
}
小米推出8周年纪念版手机
真的来了！官方曝定制版小米手环3腕带上将镭雕“ARE YOU OK？”
借助AI在网络边缘训练设备的价值
INA116基本连接电路
XP Power推出新款500W-650W AC-DC电源，满足医疗设备（包括BF）应用
如何让小型云台机械手实现按颜色分拣物品？
华为5G专利申请量全球第一
2022~2028年LED照明行业市场研究及发展趋势
PCB：PCB领域的加税主要针对电视机的PCB及生产设备
iphone8被曝光提前6月量产 三星S8渲染图曝光
PCB电路板和芯板的制作
ADI直接数字频率合成器有哪些，其应用领域是什么
gsm是什么网络_gsm网络是什么意思
DP7344 192K 双通道24位DA转换器芯片
重磅发布！浪潮高性能分布式存储平台AS15000G7加速产业数智化变革
西门子PLC S7-300出现通讯故障及远程维护办法
特拉维夫大学使用RFID和GPS技术了解蝙蝠行为 从而帮助人类思维研究
三星S8再曝光，超高屏占比无Home键
DC-DC变换器ADP3000的引脚功能介绍
辅助蓝牙网关Iot-Box组网 实现低成本低精度区域定位蓝牙覆盖