一 功能原理

识别出不同颜色的物体，让机械臂进行夹取并放入不同位置。

1 硬件设备

2 arduino库

3 工作原理

---

二 代码设计

• 项目重点划分为三部分：颜色识别，机械臂抓取，机械臂复位。分别如下：

全局参数

#include <Servo.h>
#include "Wire.h"
#include <Adafruit_TCS34725.h>int pos1 = 0;  //各舵机角度初始角度值
int pos2 = 90;
int pos3 = 90; 
int pos4 = 90;
int pos5 = 0;
int speed = 20;//机械手速度，全局调控
int step = 1;//模拟舵机转动度数//创建舵机对象
Servo myservo1;//base:基座舵机
Servo myservo2;//big_arm:大臂舵机
Servo myservo3;//fore_arm:小臂舵机
Servo myservo4;//top:顶部舵机
Servo myservo5;//paw:爪子舵机//初始化颜色传感器对象tcs，相关参数参阅： https://learn.adafruit.com/adafruit-color-sensors/program-it
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_1X);//在setup中初始设定舵机引脚，舵机初始角度，波特率，颜色判定等等....，略

1 颜色识别

/*
循环执行颜色判定，对于不同的颜色，串口输出不同的提示
并在检测到不同颜色时，调用对应的方法实现不同的动作。
*/
void loop() {// 传感器返回 R、G、B 的值uint16_t clearcol, red, green, blue;float average, r, g, b;delay(100);  //颜色测量需要大约50mstcs.getRawData(&red, &green, &blue, &clearcol);// 我写了5种颜色，分别是红色 棕色 蓝色  绿色 和黄色// 求 RGB 的平均值average = (red + green + blue) / 3;r = red / average;g = green / average;b = blue / average;//串口输出//R  G B的平均值应该在0.5-1.5之间，如果检测到红色，则R应高于1.0，G B在0.5-1.0之间，Serial.print("\tClear:");Serial.print(clearcol);Serial.print("\tRed:");Serial.print(r);Serial.print("\tGreen:");Serial.print(g);Serial.print("\tBlue:");Serial.print(b);/*
通过上面的串口输出，我们可以把颜色放在颜色传感器去检测，
然后确定RGB的取值范围，就可以确定不同颜色的值，对应不
同的舵机角度了
*/if ((r > 1.4) && (g < 0.9) && (b < 0.9)) {Serial.print("\t红色");catchs(60);} else if ((r < 0.95) && (g > 1.4) && (b < 0.9)) {Serial.print("\t绿色");//green();} else if ((r < 0.8) && (g < 1.2) && (b > 1.2)) {Serial.print("\t蓝色");catchs(120);}else if ((r > 1.15) && (g > 1.15) && (b < 0.7)) {Serial.print("\t黄色");catchs(90);} else if ((r < 0.95) && (g > 1.1) && (b < 0.9)) {Serial.print("\t棕色");//brown();}// 当颜色符合上述的颜色else {Serial.print("\t未检测到红-棕-绿-蓝-黄");// myservo.write(nonePos);}Serial.println("");delay(100);
}

2 机械臂转动

机械臂的完整抓取过程可提取为：
下放大小顶臂接近物体—>夹取—>抬高大臂—>旋转—>下方大臂—>送爪—>复位。
其中不同颜色对应动作的不同之处仅仅在于底座舵机旋转角度，故可设置一个通用的夹取函数，内置形参表示旋转角度即可。多种颜色一个函数全部实现，精简明了。

拓展：可改进为联动执行，需要运动学逆向受力分析，建立数学模型求解三元一次方程组。这样就可以根据末端舵机的状态，去计算出其他舵机需要达到的角度，实现多个舵机联动的目的，效果更加丝滑流畅。

/*
机械臂抓取函数，spin为旋转度数，不同颜色，设置不同度数
多种颜色的抓取动作，只有旋转角度不同，一个函数全部搞定
注：参数未详细调整，仅做参考。另外不同舵机转动范围可能不同，必须先验证自己舵机的角度范围与转动方向
*/
void catchs(int spin) {//1 下放大臂小臂和顶部舵机for (pos2 = 90; pos2 <= 120; pos2 += 1) {myservo2.write(pos2);delay(speed);}for (pos3 = 90; pos3 >= 60; pos3 -= 1) {myservo3.write(pos3);delay(speed);}for (pos4 = 90; pos4 >= 60; pos4 -= 1) {myservo4.write(pos4);delay(speed);}//2 夹取物品// 3 仅抬起大臂，调转方向for (pos2 = 120; pos2 >= 90; pos2 -= 1) {myservo2.write(pos2);delay(speed);}for(int i=0;i<spin;i++){   //转动spin度，spin不同颜色可以手动设置转动区间myservo1.write(pos1++);delay(speed);} //4 大臂下放for (pos2 = 90; pos2 <= 120; pos2 += 1) {myservo2.write(pos2);delay(speed);}//5 松爪init_pos();//机械臂流畅复位
}

3 机械臂复位

用于机械臂一套夹取动作结束后，执行复位。不需要为每一个动作单独编写复位代码，使用servo库中read()方法能够读取舵机角度，从而实现舵机通用角度的复位效果。

/*
读取舵机角度参数，控制舵机丝滑流畅的回到初始位置
如果不想丝滑，直接4行myservo.write(x)，各舵机咻咻咻咻的就回去了
*/
void init_pos(){pos1=myservo1.read();  //读取当前各舵机角度值pos2=myservo2.read();  //注意：read函数只能读取对应舵机上一次write的数值pos3=myservo3.read();  //不支持及时读取，因为本项目使用的for循环不停的write角度，故可读取到上一次的角度pos4=myservo4.read();//pos5=myservo5.read();//爪子初始应该是打开状态；for(int i=pos1;i>=0;i-=step){  //1号舵机，归位0，逆时针移动，范围[0°,180°]，0在右myservo1.write(i);delay(speed);}//myservo1.write(0);//设置step时，杜绝误差for(int i=pos2;i>=90;i-=step){   //2号舵机，归位90，顺时针移动，范围[90°,180°]，0°在左myservo2.write(i);delay(speed);}if(pos2<90){                 //处理2号机在[0°,90°]时，2号机为大臂比较重要，所有情况全部判断for(int i=pos2;i<=90;i+=step){myservo2.write(i);delay(speed);}}//myservo2.write(90);//如果设置step时，杜绝误差for(int i=pos3;i<=90;i+=step){    //3号舵机，归位90°，逆时针移动，范围[0°,90°]，0°在右myservo3.write(i);delay(speed);}if(pos3>90){for(int i=pos3;i>=90;i-=step){    //处理3号机在[90°,180°]时myservo3.write(i);delay(speed);}}//myservo3.write(90);//如果设置step时，杜绝误差for(int i=pos4;i<=90;i+=step){    //4号舵机，归位90°，逆时针移动，范围[0°,90°]，0°在右myservo4.write(i);delay(speed);}  //处理4号机在[90°,180°]时if(pos4>90){for(int i=pos4;i>=90;i-=step){    //处理3号机在[90°,180°]时myservo4.write(i);delay(speed);}}//myservo4.write(90);//如果设置step时，杜绝误差//更新pos，可以将本函数中所有的i换成对应pos变量，此处就不用更新，但我觉得不如写i码量更短，哈哈哈pos1=0;pos2=90;pos3=90;pos4=90;}

---

三 展示效果

颜色识别机械臂