本文详细介绍了智能机器人竞赛项目的全过程，从项目启动到比赛结束，包括团队如何从零开始构建机器人，解决寻迹、机械臂控制、颜色识别、二维码读取等问题。特别分享了代码逻辑、调试技巧和硬件设计心得，如步进电机与麦克纳姆轮的配合、舵机控制策略、电路板快速焊接技巧等。
程序和方案还有很多要改进的地方，自己以后也没有机会再做这个比赛了，当时自己苦思冥想的东西，如果能帮助到需要的人，那就再次发挥了价值！也很希望大家比完赛后，能分享自己的思考，想法和方案。
================================================================
比赛视频，大神勿喷：工程训练赛_哔哩哔哩_bilibili
================================================================
一开始我并不知道要参加这个比赛（老师偷偷给我报名了-_-!!），后来期末考试考完了，在楼下洗澡碰到实验室的同学，他说：“你是不是有一个比赛要参加”。我当时一脸懵逼，第二天老师就通知去实验室集合，说是有新项目。。。。。。
我们是第一次参加这个比赛项目，没啥经验，心里也没底，当时听完比赛规则，感觉还是挺简单的。（这种比赛项目难的不是做出来，而是做好，因为你不可能考虑到所有情况，比赛场地，光线啥的影响很大！！就算你在家里调试好了，到了比赛的地方，可能又会出现新问题，所以调试是最难的。）
我们是三月初开始做的，每天都花很多的时间在这上面，因为完全从零开始做，毫无头绪。比赛的内容听起来很简单，就是读取任务，然后按照任务顺序，抓取物块，放置到规定的地方。涉及到，电机控制，二维码读取，和一个路径的自主判断等，代码部分的主要难点在于各个模块之间的配合。特别是寻迹模块（黑标）与底盘的配合真的搞得头都大了！整个代码逻辑写出来做出来不难，就是细节很多，调试很麻烦，真的很麻烦！
而且这个比赛还有焊接电路版和拆装的环节，整个比赛我们一直处在疲于奔命的状态，我们负责装配的同学差点被逼疯了，田大头装机械臂的时候手都在抖，一边装一边骂（+_+!）。电路板在设计的时候可以把主控芯片和晶振分离出来，采用插拔式，这样焊接电路板会很省时间，只要焊接一个主控就好了，不然真的焊不完，就算焊完了几乎也用不了。。。
我们的方案是步进电机加麦克纳姆轮（超级稳（慢）的那种 ），但是寻迹做的不太好（黑标受光线影响太大！！！），底盘的运动速度不是很快，害怕速度给快了抖动（会飘）。
物块抓取是两块控制板通信。我们用了两块Arduino mega2560控制板，一块运行舵机控制程序，控制机械臂抓取物块，一块运行主程序，颜色识别是TCS230模块，二维码用的是二维码识别模块（微雪）。
整个项目思路得细节方面还是看代码吧，能注释的地方都注释了，变量命名也是有规律的（一个一个单词查的），一般是可以看懂的，这样可以直接理解思路！
下面是全部的代码（代码打包了，需要可以自取，和下面一样的）。
================================================================
程序重新打包了一下：
https://wwgw.lanzouu.com/i1Je0mxt86d 密码:6k6y
字符串读取希函数库：
https://wwgw.lanzouu.com/i19qq42y87a
用到的其他库：
https://wwgw.lanzouu.com/iTcAu6rju8f 密码:atk3
================================================================
舵机控制程序：
#include <SerialCommand.h> //字符串处理库函数
#include <Servo.h> //舵机控制库
SerialCommand SCmd;//定义字符处理对象
Servo myservo[3];//定义舵机控制对象
//#define DEBUG1
//#define DEBUG
/*---------------舵机抓取角度宏定义------------*/
//上部舵机角度
//BEGIN:开始状态
//CATCH : 抓取角度
//PUT : 放置角度
//DCATCH : 待抓取角度
#define SERVO_S_BEGIN 57
#define SERVO_S_CATCH 0
#define SERVO_S_PUT 20
#define SERVO_S_DCATCH 40
//中间舵机角度
//BEGIN : 开始角度
//CATCH : 夹取角度
#define SERVO_Z_BEGIN 25
#define SERVO_Z_CATCH 89 //斜一点居中
//底部角度
//MIDDLE : 中间角度
//PUT : 放置角度
//CATCH: 夹取角度
#define SERVO_X_MIDDLE 90
#define SERVO_X_PUT 20
#define SERVO_X_CATCH 150
/*----------------模式定义--------------------*/
#define BEGIN_MODEL 0
#define MIDDLE_MODEL 1
#define DCATCH_MODEL 2
#define CATCH_MODEL 3
#define PUT_MODEL 4
#define DPUT_MODEL 5
#define DMIDDLE_MODEL 6
//新添
#define _CATCH 7
#define _PUT 8
/*----------------舵机端口设置----------------*/
int servo_port[3] = {3, 8, 12}; //舵机端口 从上到下
/*-----------------舵机初始角度---------------*/
float servo_angle[3] = {40, 25, 90}; //舵机端口 从上到下
void setup() {
Serial.begin(9600);//启动串口
SCmd.addCommand("T", control_model);
//舵机初始化函数
servoInti();
#ifdef DEBUG1 // DEBUG1
beginModel();
dmiddleModel();
catchModel();
putModel();
dputModel();
dcatchModel();
middleModel();
#endif
#ifdef DEBUG // DEBUG
// beginModel();
// _catch();
// _put();
#endif
beginModel();
}
void loop() {
SCmd.readSerial(); //循环读取外部数值
}
//舵机初始化函数
void servoInti() {
for (int i = 0; i < 3; i++) {
myservo[i].attach(servo_port[i]); //舵机端口连接
myservo[i].write(servo_angle[i]); //运动到舵机初始角度
}
}
//控制模式
void control_model() {
char *arg; //字符指针
int action_model;//控制模式匹配
arg = SCmd.next();//读取控制模式
action_model = atoi(arg);
switch (action_model) {
case BEGIN_MODEL:
beginModel();
break;
case CATCH_MODEL:
catchModel();
break;
case PUT_MODEL:
putModel();
break;
case MIDDLE_MODEL:
middleModel();
break;
case DCATCH_MODEL:
dcatchModel();
break;
case DPUT_MODEL:
dputModel();
break;
case DMIDDLE_MODEL:
dmiddleModel();
break;
case _CATCH:
_catch();
break;
case _PUT:
_put();
break;
default:
break;
}
}
//开始姿态控制
inline void beginModel(){
myservo[0].write(SERVO_S_BEGIN);
delay(1000);
myservo[1].write(SERVO_Z_BEGIN);
delay(1000);
myservo[2].write(SERVO_X_MIDDLE);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//中立准备姿态控制
inline void dmiddleModel(){
myservo[0].write(SERVO_S_DCATCH);
delay(1000);
myservo[1].write(SERVO_Z_CATCH);
delay(1000);
myservo[2].write(SERVO_X_MIDDLE);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//抓取姿态控制
inline void catchModel(){
myservo[0].write(SERVO_S_CATCH);
delay(1000);
myservo[1].write(SERVO_Z_CATCH);
delay(1000);
myservo[2].write(SERVO_X_CATCH);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//放置姿态控制
inline void putModel(){
myservo[0].write(SERVO_S_DCATCH);
delay(1000);
myservo[1].write(SERVO_Z_CATCH);
delay(1000);
myservo[2].write(SERVO_X_PUT);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//待放置姿态控制
inline void dputModel(){
myservo[0].write(SERVO_S_CATCH);
delay(1000);
myservo[1].write(SERVO_Z_CATCH);
delay(1000);
myservo[2].write(SERVO_X_PUT);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//待抓取姿态控制
inline void dcatchModel(){
myservo[0].write(SERVO_S_DCATCH);
delay(1000);
myservo[1].write(SERVO_Z_CATCH);
delay(1000);
myservo[2].write(SERVO_X_CATCH);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//中立抓取姿态控制
inline void middleModel(){
myservo[0].write(SERVO_S_CATCH);
delay(1000);
myservo[1].write(SERVO_Z_CATCH);
delay(1000);
myservo[2].write(SERVO_X_MIDDLE);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//抓
inline void _catch() {
//中立张开
myservo[1].write(SERVO_Z_CATCH);
delay(1000);
//待抓
myservo[2].write(SERVO_X_CATCH);
delay(1000);
//抓
myservo[0].write(SERVO_S_CATCH);
delay(1000);
//中立抓取
myservo[2].write(SERVO_X_MIDDLE);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}
//放
inline void _put() {
// dputModel();//待放
myservo[2].write(SERVO_X_PUT);
delay(1000);
//轻放
myservo[0].write(17);
delay(2000);
// putModel();//全放
myservo[0].write(SERVO_S_DCATCH);
delay(1000);
// dmiddleModel();//中立张开
myservo[2].write(SERVO_X_MIDDLE);
delay(1000);
//Serial.println("Y");//反馈给2560，表明动作执行完毕
}主程序：
/******************************************************
OLED屏幕 ：
128 个像素横向排列在 X 轴上，分别以 0-127 来代表,
64个像素垂直排列在 Y 轴上，分别以 0-63 来代表。
******************************************************/
#include <Arduino.h>
#include <Wire.h>
#include "scanner.h"
#include <U8g2lib.h>
#define PUTORDER "123" //物料放置时从左至右为红,绿,蓝,对应序号为123
//物料放置时从左至右为蓝,绿,红,对应序号为321
enum Dir {X = 1, _X = 2, Y = 3, _Y = 4}; //延时寻迹用
/*=====================传感器端口设置==================*/
int S_FL = A0; //车头左侧传感器
int S_FR = A1; //车头右侧传感器
int S_LF = A5; //左侧前传感器
int S_LB = A4; //左侧后传感器
int S_RF = A3; //右前侧传感器
int S_RB = A2; //右后侧传感器
int S_BL = A15; //后左侧传感器
int S_BR = A11; //后右侧传感器
/*====================小车行驶的速度==================*/
float linear_v = 0.2; //速度控制
float runingSpeed=0.2;//前进速度
/*====================次序存储=======================*/
String qr_data = ""; //存储二维码顺序
String color_data = ""; //存储颜色顺序
String get_data = ""; //存储抓取顺序
int get_order[3];//存储抓取顺序
int put_order[3];//存储放置顺序
int count = 0; //Y向黑线计数
/*======================定义OLED对象=================*/
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);
/*===========定义二维码扫描对象和颜色扫描对象==========*/
Scanner myScanner0; //二维码
Scanner myScanner1; //颜色
/*=================Arduino初始化函数================*/
void setup() {
Serial.begin(9600);
Serial1.begin(9600);//二维码反馈端口
Serial2.begin(9600);//舵机通信
//引脚端口初始化
pinMode(S_FL, INPUT);
pinMode(S_FR, INPUT);
pinMode(S_LF, INPUT);
pinMode(S_LB, INPUT);
pinMode(S_RF, INPUT);
pinMode(S_RB, INPUT);
pinMode(S_BL, INPUT);
pinMode(S_BR, INPUT);
/*--------------OLED初始化----------------*/
ledInti();
/*-------------构建Scanner扫描对象--------*/
myScanner0.setType(0); //类型 0 为构建二维码扫描对象
myScanner1.setType(1); //类型 1 为构建颜色扫描对象
/*------------步进电机初始化--------------*/
intiMotors();
/*---------------等待触发开始-------------*/
//wait(); //端口为A10的触发传感器触发开始
/*----------------步骤1-------------------*/
stepOne();
/*----------------步骤2-------------------*/
stepTwo();
/*----------------步骤3-------------------*/
stepThree();
/*----------------步骤4-------------------*/
stepFour();
}
/*--------------------------------------*
Arduino循环函数
--------------------------------------*/
void loop() {
}
/*--------------------------------------*
步骤一：离开出发区
--------------------------------------*/
void stepOne() {
//穿门
moveTo(0.35, 0, 3); //向Y正方向平移出出发区
//舵机到待抓中立位置
servoMiddle();
moveTo(0.215, 0, 1);//向X方向平移0.23米
while (toDigital(S_FL) && toDigital(S_BL)) moveTo(0.001, 0, 4);//移动至黑线处
delay(200);
}
/*--------------------------------------*
步骤二：识别二维码和颜色
--------------------------------------*/
void stepTwo() {
const double dx = 0.24; //中部传感器触发时，颜色传感器到第一个物块的距离
while (1) {
if (count < 5) {
if (!toDigital(S_LB)) { //传感器触发,计数
count++;
while (!toDigital(S_LB)) moveTo(0.001, 0, 1); //将左前触发传感器，脱离触发黑线
switch (count) {
case 2:
//左后传感器触发两次后，移动到第一个物块，进行颜色识别
moveTo(dx, 0, 1);
stopHalSec();//停止
colorDetect(0); //第一次检测颜色
break;
case 3:
//左后传感器触发三次后，移动到第二个物块，进行颜色识别
moveTo(0.09, 0, 1);
stopHalSec();
colorDetect(1); //第二次检测颜色
break;
default:break;
}
} else {
trackingX(); //X正方向寻迹
}
} else {
//到达二维码区域
//扫描二维码,获取二维码数据
moveTo(0.035, 0, 2); //修正二维码扫描的位置
qrDetect();//二维码扫描
delay(100);
//扫描完成
break;
}
}
}
/*--------------------------------------*
步骤三：抓取放置
--------------------------------------*/
void stepThree() {
//调试用
//color_data = "321";
//qr_data = "321";
GetOrderDisplay(qr_data);//抓取顺序显示);//抓取顺序显示
//通过颜色识别与二维码要求信息比对，获取抓取顺序
for (int i = 0; i < qr_data.length(); i++) {
for (int j = 0; j < color_data.length(); j++) {
if (qr_data[i] == color_data[j]) {
get_data += j; //通过比对设置抓取顺序
}
}
}
//设置抓取顺序数组
GetOrderSet(get_data); //将字符串转化为数字存入抓取数组
//设置放置顺序数组
PutOrderCalc(); //将字符串转化为数字存入放置数组
//后退
moveTo(0.05, 0, 2); //向后退一小段距离让左后传感器离开黑线
delay(20);
//后退到中心位置区
int count = 0;
while (1) {
if (count < 2) {
if (!toDigital(S_LB)) { //传感器触发,计数
count++;
while (!toDigital(S_LB)) moveTo(0.001, 0, 2); //将左前触发传感器，脱离触发黑线
} else {
tracking_X();//向后寻迹
}
} else {
stopHalSec();//左前传感器触碰到两条黑线立即停止
break;
}
}
//调试看是否需要调整位置
//开始根据抓取数组判断抓取顺序
for (int i = 0; i < 3; i++) {
//抓取
switch (get_order[i]) {
case 0: //抓取位置一
moveTo(0.16, 0, 2); // 移动到位置一物块处
getT(0);
while (toDigital(S_LB) && toDigital(S_RB)) moveTo(0.001, 0, 1);
//调整车身
break;
case 1: //抓取位置二
//moveTo(0.01, 0, 1); //位置调整
getT(1);
break;
case 2: //抓取位置三
moveTo(0.16, 0, 1);//移动到位置三处
getT(2);
while (toDigital(S_LF) && toDigital(S_RF)) moveTo(0.001, 0, 2); //是否可用升级版延时寻迹?????
//调整车身
break;
}
//放置
switch (put_order[i]) {
//位置确定方法，从左到右
case 0: putT(0); break; //放置到位置一
case 1: putT(1); break; //放置到位置二
case 2: putT(2); break; //放置到位置三
}
}
}
/*--------------------------------------*
步骤四：返回出发区
--------------------------------------*/
void stepFour() {
int count = 0;
//调整车身远离中心黑线
moveTo(0.08, 0, 2);
linear_v = runingSpeed; //速度设置为 0.3m/s
while (count != 3) {
if (count < 3) {
if (!toDigital(S_LF)) {
count++;
while (!toDigital(S_LF)) moveTo(0.001, 0, 2);
}
else {
tracking_X();//向后循迹
//moveTo(0.001, 0, 2);
}
}
}
//返回开始区域
moveTo(0.15, 0, 2);
moveTo(0.20, 0, 4);
/*********************/
//lcdClear();
//u8g2.drawStr(15, 3, "Mission Complete!");
//u8g2.sendBuffer();//将缓存输出到屏幕
/*********************/
}
/*-------------------------------------*
OLED屏幕的初始化
-------------------------------------*/
void ledInti() {
u8g2.begin();
u8g2.clearBuffer(); //清除模组的缓存
u8g2.setFont(u8g2_font_ncenB14_tr); // 设置字体
delay(200);
}
/*--------------------------------------*
二维码扫描函数
--------------------------------------*/
void qrDetect() {
myScanner0.scan();//会一直读取知道得到二维码顺序
qr_data = myScanner0.getData();
}
/*--------------------------------------*
触碰传感器等待触发
--------------------------------------*/
/*
void wait() {
u8g2.drawStr(30, 1, "waiting...");
u8g2.sendBuffer(); // 将缓存输出到屏幕
while(toDigital(S_Begin)) delay(1); //等待触发
lcdClear();
u8g2.drawStr(0, 0, "begin!");
u8g2.sendBuffer(); // 将缓存输出到屏幕
}
*/
/*--------------------------------------*
颜色检测函数
--------------------------------------*/
void colorDetect(int i) {
switch (i) {
case 0: myScanner1.scan(); break;
case 1: myScanner1.scan(); break;
}
color_data = myScanner1.getData();
}
/*-------------------------------------*
抓取顺序计算
-------------------------------------*/
void GetOrderSet(String str) {
int n = str.toInt(); //字符串转化为整数型
get_order[0] = n / 100 % 10; //取第一个数字
get_order[1] = n / 10 % 10; //取第二个数字
get_order[2] = n % 10; //取第三个数字
}
/*--------------------------------------*
放置顺序计算
--------------------------------------*/
void PutOrderCalc() {
String str = PUTORDER; //物料放置时从左至右为红,绿,蓝,对应序号为123
for (int i = 0; i < qr_data.length(); i++) {
for (int j = 0; j < 3; j++) {
if (qr_data[i] == str[j])
put_order[i] = j;
}
}
}
/*-------------------------------------*
抓取函数
-------------------------------------*/
void getT(int i) {
linear_v = 0.1; //设置车身校准速度
//校准车身的位置
switch (i) {
case 0: moveTo(0.03, 0, 3);break; //位置一物块抓取校准
case 1: moveTo(0.03, 0, 3);break; //位置二物块抓取校准
case 2: moveTo(0.03, 0, 3);break; //位置三物块抓取校准
}
linear_v = runingSpeed;//速度设置
servoCatch();
//校准车身的位置
switch (i) {
case 0: moveTo(0.03, 0, 4); break; //位置一物块抓取校准
case 1: moveTo(0.03, 0, 4); break; //位置二物块抓取校准
case 2: moveTo(0.03, 0, 4); break; //位置三物块抓取校准
}
}
/*-----------------------------------------*
放置函数调用
-----------------------------------------*/
void putSet(int i) {
delayTracking(Y, 5);//延时寻迹越过两条线
//至物料放置区
while (toDigital(S_LF) || toDigital(S_LB)) trackingY();//moveTo(0.001,0,3);//
//位置调整
switch (i) {
case 0: moveTo(0, 0, 0); break;
case 1: moveTo(0, 0, 0); break;
case 2: moveTo(0, 0, 0); break;
}
//放置
servoPut();
//返回位置调整
switch (i) {
case 0: moveTo(0, 0, 0); break;
case 1: moveTo(0, 0, 0); break;
case 2: moveTo(0, 0, 0); break;
}
//返回
delayTracking(_Y, 5); //-Y方向延时寻迹越过两条线
while (toDigital(S_FL) && toDigital(S_BL))tracking_Y();// moveTo(0.001,0,4);
//move(0, 0.01, 0); //位置调整
}
/*---------------------------------------*
放置函数
---------------------------------------*/
void putT(int which) {
//速度设置函数
//延时寻迹函数
//车身右侧传感器同时触发，到达预定位置
//摆放角度调整
//舵机运动放置物块
//返回函数
//寻迹返回，触发前面
const double d = 0.2; //距路口距离
switch (which) {
case 0:
{
moveTo(d, 0, 2);
while (toDigital(S_LF)) {
moveTo(0.001, 0, 2);
}
//至物料区放置并返回
putSet(0);
//位置调整
//返回中心区域
moveTo(d, 0, 1); //是否可用升级版延时寻迹
while (toDigital(S_LB)) trackingX();
//位置调整
}
break;
case 1:
{
putSet(1);
}
break;
case 2:
{
moveTo(d, 0, 1);
while (toDigital(S_LB)) trackingX();
putSet(2);
//位置调整
moveTo(d, 0, 2); //是否可用升级版延时寻迹
while (toDigital(S_LF)) {
moveTo(0.001, 0, 2);
}
//调整位置
}
break;
}
}
/*---------------------------------------*
延时寻迹
---------------------------------------*/
void delayTracking(int axis, double time) {
unsigned long t = millis() + time* 1000 * 1.06; //参数可调
if (axis == X) while (millis() < t) moveTo(0.001,0,1);//trackingX();
if (axis == _X) while (millis() < t)moveTo(0.001,0,2) ;//tracking_X();
if (axis == Y) while (millis() < t)trackingY();//moveTo(0.001,0,3) ;//
if (axis == _Y) while (millis() < t)tracking_Y();// moveTo(0.001,0,4);//
}
/*---------------------------------------*
舵机动作函数
---------------------------------------*/
void servoPut() { //放置函数
Serial2.println("T 8");
//等待执行完毕
delay(5000);
}
void servoCatch() {//拿取函数
Serial2.println("T 7");
//等待执行完毕
delay(4100);
}
void servoMiddle() {//中立待抓取
Serial2.println("T 6");
//等待执行完毕
delay(2000);
}
/*---------------------------------------*
OLED清屏函数
---------------------------------------*/
void lcdClear() {
u8g2.clear();//清除屏幕所有信息
}
/*-------------------------------------
OLED抓取任务显示函数
-------------------------------------*/
void GetOrderDisplay(String str) {
lcdClear();//清除屏幕信息
char str1[4];
for (int i = 0; i < 3; i++) {
str1[i] = str[i];
}
u8g2.drawStr(0, 15, "Grab order:");
u8g2.drawStr(50, 35, str1); // 设置坐标,显示二维码读取信息
u8g2.sendBuffer(); // 将缓存输出到屏幕
delay(200);
}
/*-------------------------------------*
传感器状态反馈
-------------------------------------*/
inline boolean toDigital(int pin) {
if (analogRead(pin) >= 800) {
return true;
} else {
return false;
}
}
/*--------------------------------------*
前向寻迹行驶
--------------------------------------*/
void trackingX() {
const double dx = 0.001;
const double dw = 0.1;
if ((!toDigital(S_FL) && !toDigital(S_FR)) || (toDigital(S_FL) && toDigital(S_FR))) {
moveTo(dx, 0, 1); //+X平移
}
else if (!toDigital(S_FL)) {//前左传感器触发，左转
moveTo(0, dw, 5); //左偏
moveTo(dx, 0, 1);
}
else if (!toDigital(S_FR)) {//前右传感器触发，右转
moveTo(0, dw, 6); //右偏
moveTo(dx, 0, 1);
}
}
/*--------------------------------------*
后向寻迹行驶(平移)
--------------------------------------*/
void tracking_X() {
const double dx = 0.001;
const double dw = 0.1;
if ((!toDigital(S_BL) && !toDigital(S_BR)) || (toDigital(S_BL) && toDigital(S_BR))) {
moveTo(dx, 0, 2); //-X平移
}
else if (!toDigital(S_BL)) {//后左传感器触发，右转
moveTo(0,dw, 6); //右转
moveTo(dx, 0, 2);
}
else if (!toDigital(S_BR)) {//后右传感器触发，左转
moveTo(0,dw, 5); //左转
moveTo(dx, 0, 2);
}
linear_v=runingSpeed;
}
/*--------------------------------------*
左横向寻迹行驶
--------------------------------------*/
void trackingY() {
const double dx = 0.001;
const double dw = 0.1;
linear_v = 0.1;
if ((!toDigital(S_LB) && !toDigital(S_LF)) || (toDigital(S_LB) && toDigital(S_LF))) {
moveTo(dx, 0, 3); //+Y平移
}
else if (!toDigital(S_LB)) {
//moveTo(0, dw, 5); //左转
//moveTo(dx, 0, 3);
moveTo(dx, 0, 2); //后退
moveTo(dx, 0, 3);
}
else if (!toDigital(S_LF)) {
//moveTo(0, dw, 6); //右转
//moveTo(dx, 0, 3);
moveTo(dx, 0, 1); //前进
moveTo(dx, 0, 3);
}
linear_v = runingSpeed;
}
/*--------------------------------------*
右横向寻迹行驶(平移)
--------------------------------------*/
void tracking_Y() {
const double dx = 0.001;
const double dw = 0.1;
linear_v = 0.1;
if ((!toDigital(S_RB) && !toDigital(S_RF)) || (toDigital(S_RB) && toDigital(S_RF))) {
moveTo(dx, 0, 4); //-Y平移
}
else if (!toDigital(S_RF)) {
//moveTo(0, dw, 5); //左转
//moveTo(dx, 0, 4); //
moveTo(dx, 0, 1); //右前触发向左转
moveTo(dx, 0, 4);
}
else if (!toDigital(S_RB)) {
//moveTo(0, dw, 6); //右转
//moveTo(dx, 0, 4); //
moveTo(dx, 0, 2); //右后触发向右转
moveTo(dx, 0, 4);
}
linear_v = runingSpeed;
}步进电机控制程序：
/* 步进电机方向引脚：
* dir: x: 5, y: 6, z: 7, a: 13
* 步进电机步进引脚：
* stp: x: 2, y: 3, z: 4, a: 12
* 步进电机使能引脚(低电平有效)：
* en: 8
* 步进电机细分设置：0, 2, 4, 8, 16
* 各细分对应步进电机每周步数：
* 0 --> 200
* 2 --> 400
* 4 --> 800
* 8 --> 1600
* 16 --> 3200
*
* 车身位置及传感器\电机接线:
* Y轴
* | A4 A3
* | 1:X - - - - - - Y:3
* | | | A2
* | | | A0
* | 4:Z - - - - - - A:2
* |
* 0-- -- -- -- -- -- -- -- X轴
*
*
************************************************/
#include <Arduino.h>
#include <AccelStepper.h> //步进电机库
/*----------------------------------------------*
* 步进电机端口宏定义
*----------------------------------------------*/
#define En_Pin 8 //步进电机使能端口
#define A_Dir 13 //A电机方向引脚
#define A_Step 12 //A电机步进引脚
#define X_Dir 5
#define X_Step 2
#define Y_Dir 6
#define Y_Step 3
#define Z_Dir 7
#define Z_Step 4
/*------------------------------------------*
* 步进电机参数定义
*------------------------------------------*/
#define L_Length 0.138 //设置a的长度 单位：m
#define D_Wheel 0.0591 //车轮的直径 单位:m
#define AllStep 200 //步进电机全步进一圈步数为200
#define MicroStep 8 //步进电机细分数（短接帽设置）
#define TotalStep 1600 //8细分下每圈步数:1600步
#define ClockWise HIGH //顺时针转动
#define AntiClockWise LOW //逆时针转动
#define NotSendPluse false //不发送脉冲
#define SendPluse true //发送脉冲
#define StartUse true //开始启用
#define StopUse false //停止启用步进电机
/*--------------------------------------------*
* 步进电机移动方向宏定义
*--------------------------------------------*/
#define MFord 1 //移动方向
#define MBack 2
#define MLeft 3
#define MRight 4
#define RLeft 5
#define RRight 6
/*--------------------------------------------*
* 步进电机控制对象定义
*--------------------------------------------*/
AccelStepper motora(1,A_Step,A_Dir); //定义步进电机控制对象a
AccelStepper motorx(1,X_Step,X_Dir);
AccelStepper motory(1,Y_Step,Y_Dir);
AccelStepper motorz(1,Z_Step,Z_Dir);
/*---------------------------------------------*
* 比率变量的设定
*---------------------------------------------*/
const double CarMaxSpeed=4000; //最大步进速度
const double CWheel=M_PI*D_Wheel; //车轮的周长 单位：m
const double StepOfMeter=TotalStep/CWheel; //步数/米
const double KMaxFre = 4000; //每秒最大步数 单位：步/秒
const double angle_L=0.323/90;// 将设置的小车线速度转化为整体角速度
//float Linear_v = 0.3; //车速度 m/s
/*-------------------------------------------*
* 步进电机的初始设定方向
*-------------------------------------------*/
void defaultDir(){
motorx.setPinsInverted(ClockWise,NotSendPluse,StartUse);//步进电机初始正转向为顺时针，是否发送步进脉冲步进，是否启用步进电机
motory.setPinsInverted(ClockWise,NotSendPluse,StartUse);
motorz.setPinsInverted(ClockWise,NotSendPluse,StartUse);
motora.setPinsInverted(ClockWise,NotSendPluse,StartUse);
}
/*-------------------------------------------*
* 步进电机的最大速度
*-------------------------------------------*/
void defaultMaxSpeed(){
motorx.setMaxSpeed(CarMaxSpeed);
motory.setMaxSpeed(CarMaxSpeed);
motorz.setMaxSpeed(CarMaxSpeed);
motora.setMaxSpeed(CarMaxSpeed);
}
/*-------------------------------------------*
* 步进电机的初始化
*-------------------------------------------*/
void intiMotors(){
motorx.setEnablePin(En_Pin); //设置步进电机使能端口
defaultDir(); //设置步进电机初始方向
defaultMaxSpeed(); //设置步进电机最大速度
motorx.enableOutputs(); //步进端口使能
}
/*-----------------------------------------*
* 步进电机运动到设定的速度
*-----------------------------------------*/
inline void runToSpeed(){
motorx.runSpeed();
motory.runSpeed();
motorz.runSpeed();
motora.runSpeed();
}
/*------------------------------------------*
* 步进电机速度设置
*------------------------------------------*/
inline void xySetVal(double vxy,double vw,int dir){
double v0=vxy*StepOfMeter; //直行速度转化为步数的速率
double v1=vw*StepOfMeter; //转动的速率转化为步数的速率
switch(dir){
case MFord:
motorx.setSpeed(-v0);
motory.setSpeed(-v0);
motorz.setSpeed(v0);
motora.setSpeed(v0);
break;
case MBack:
motorx.setSpeed(v0);
motory.setSpeed(v0);
motorz.setSpeed(-v0);
motora.setSpeed(-v0);
break;
case MLeft:
motorx.setSpeed(-v0);
motory.setSpeed(v0);
motorz.setSpeed(-v0);
motora.setSpeed(v0);
break;
case MRight:
motorx.setSpeed(v0);
motory.setSpeed(-v0);
motorz.setSpeed(v0);
motora.setSpeed(-v0);
break;
case RLeft:
motorx.setSpeed(v1);
motory.setSpeed(v1);
motorz.setSpeed(v1);
motora.setSpeed(v1);
break;
case RRight:
motorx.setSpeed(-v1);
motory.setSpeed(-v1);
motorz.setSpeed(-v1);
motora.setSpeed(-v1);
break;
default:break;
}
}
/*-----------------------------------------*
* 步进电机移动固定距离
*----------------------------------------*/
void moveTo(double dxy,double dw,int dir){
if((dxy == 0) && (dw == 0)) return;
double t1 = dxy/ linear_v;
dw=angle_L*dw; //将角度运算转化为长度运行
double t2 =dw/linear_v;
double t_last=max(t1,t2);
if(t_last==0)return; //防止发生不可预料的错误
//xySetVal(dxy/t,dw/t,dir);
xySetVal(linear_v,linear_v,dir);
//XYStar();
unsigned long delta_t=millis()+t_last*1000*1.03;
while(millis()<delta_t){
runToSpeed();
}
stopHalSec();
}
/*---------------------------------------------*
* 步进电机使能开关
*---------------------------------------------*/
inline void xyStop(){xySetVal(0, 0, 0); runToSpeed(); motorx.disableOutputs();}//步进电机使能关闭
inline void xyStar(){motorx.enableOutputs();}//步进电机使能开启
/*---------------------------------------------*
* 暂停(速度为零)
*---------------------------------------------*/
void stopHalSec(){
xySetVal(0, 0, 0);
runToSpeed();
}二维码和颜色识别库头文件：
/*
*
*/
#ifndef SCANNER_H
#define SCANNER_H
#include <Arduino.h>
#define MAX_SCANNERS 2
typedef struct{
uint8_t type;
String value;
}scanner_t;
class Scanner{
private:
uint8_t scannerIndex;
uint8_t _interface;
uint8_t _qr_scanner_time;
String _qr;
String _color;
typedef enum{
QRCODE = 0,
COLOR = 1
}ScannerInterfaceType;
String serialRead();
bool receiveCmd();
String readQRCode(unsigned long * _t);
String readColor();
public:
Scanner();
~Scanner();
void setType(uint8_t interface);
void scan();
String getData();
};
#endif 二维码识别和颜色读取库函数：
/*
* Serial -> information print
* Serial2 -> qrCode
* color_sensor_pin_define:
* s0 -> 30
* s1 -> 31
* s2 -> 34
* s3 -> 35
* out -> 33
* led -> 32
* vcc -> 5V
* gnd -> gnd
*/
#include "scanner.h"
//二维码模块触发检测
unsigned char hexdata[9] = {0x7E, 0x00, 0x08, 0x01, 0x00, 0x02, 0x01, 0xAB, 0xCD};
//颜色扫描端口设置
uint8_t s0 = 30;
uint8_t s1 = 31;
uint8_t s2 = 34;
uint8_t s3 = 35;
uint8_t out = 33;
uint8_t led = 32;
static scanner_t scanners[MAX_SCANNERS];
uint8_t scannerCount = 0; //扫描对象计数
//开始检测二维码返回数据
static void readHeader(){
while(Serial1.read() != 0x31) delay(10);
}
//颜色扫描端口初始化
static void initTcs(){
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(out, INPUT);
pinMode(led, OUTPUT);
/*S0和S1的设置可以缩放输出频率
S0:L,S1:L---Powerdown
S0:L,S1:H---2%
S0:H,S1:L---20%
S0:H,S1:H---100%
*/
//拓展频率设置为20%
digitalWrite(s0, HIGH);
digitalWrite(s1, LOW);
}
//二维码扫描开启
String Scanner::readQRCode(unsigned long * _t){
String incomingStr = "";
while(true){
incomingStr = Serial1.readStringUntil('\r');
if((incomingStr.length() >= 3) || (millis() - *_t > _qr_scanner_time))
break;
}
return incomingStr;
}
//读取颜色
String Scanner::readColor(){
int rcount=0,gcount=0,bcount=0;//三色计数器
float R_COE = 0.29, G_COE = 0.27, B_COE = 0.31;//白平衡系数
float rr = 0, gg = 0, bb = 0;
//识别10次取最大的一个颜色
digitalWrite(led, HIGH);
for(int i=0;i<100;i++){
int r = 0, g = 0, b = 0; //脉冲计数器
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
//count OUT, RED
r = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测红色脉冲时长
digitalWrite(s3, HIGH);
//count OUT, BLUE
b = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测蓝色脉冲时长
digitalWrite(s2, HIGH);
//count OUT, GREEN
g = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测绿色脉冲时长
//颜色白平衡
rr = r * R_COE;
gg = g * G_COE;
bb = b * B_COE;
if(rr < bb && rr < gg){
rcount++; //红色最小就是红色
}
else if(bb < rr && bb < gg){
bcount++; //蓝色最小就是蓝色
}
else if(gg < rr && gg < bb){
gcount++; //绿色最小就是绿色
}else{
//
}
delay(10);
}
digitalWrite(led, LOW); //关灯
if((rcount>bcount)&&(rcount>gcount)){
return "1";
}else if((gcount>rcount)&&(gcount>bcount)){
return "2";
}else if((bcount>rcount)&&(bcount>gcount)){
return "3";
}
}
Scanner::Scanner(){
_qr_scanner_time = 5000; // 5s，二维码扫描5秒钟
if(scannerCount < MAX_SCANNERS){
this->scannerIndex = scannerCount++;
}
else
return;
}
//析购函数
Scanner::~Scanner(){
}
void Scanner::setType(uint8_t interface){
_interface = interface;
scanners[this->scannerIndex].type = _interface;
if(_interface == COLOR)
initTcs(); //端口初始化
}
String Scanner::serialRead(){
if(_interface == QRCODE){
unsigned long wait_t = millis();
return readQRCode(&wait_t);
}else if(_interface == COLOR){
return readColor();
}
}
bool Scanner::receiveCmd(){
String data = serialRead();
if(data.length() > 0){
if(_interface == QRCODE){
_qr = data;
scanners[this->scannerIndex].value = _qr;
}else if(_interface == COLOR){
if(_color.indexOf(data) < 0){ //若字符串_color中没有该字符data则系统返回-1
_color += data;
if(_color.length() >= 2){ //c++中，length()只是用来获取字符串的长度。 例如：string str = “asdfghjkl”,则，str.length() = 9。
if(_color.equals("23") || _color.equals("32"))
_color += "1";
else if(_color.equals("13") || _color.equals("31"))
_color += "2";
else if(_color.equals("12") || _color.equals("21"))
_color += "3";
scanners[this->scannerIndex].value = _color;
}
}
else{
return false;
}
}
return true;
}else{
return false;
}
}
void Scanner::scan(){
switch(_interface){
case QRCODE: Serial1.write(hexdata, 9); readHeader();break;
case COLOR: /****/ ; break;
}
if(receiveCmd()){
return;
}
else if(_interface == QRCODE){
scan();
}
}
String Scanner::getData(){
return scanners[this->scannerIndex].value;
} 白平衡系数计算：
（为了降低环境对颜色传感器的影响，在颜色识别的库函数中有一组白平衡系数要提前检测输入进去）
//颜色扫描端口设置
uint8_t s0 = 30;
uint8_t s1 = 31;
uint8_t s2 = 34;
uint8_t s3 = 35;
uint8_t out = 33;
uint8_t led = 32;
void setup(){
Serial.begin(9600);
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(out, INPUT);
pinMode(led, OUTPUT);
/*S0和S1的设置可以缩放输出频率
S0:L,S1:L---Powerdown
S0:L,S1:H---2%
S0:H,S1:L---20%
S0:H,S1:H---100%
*/
digitalWrite(s0, HIGH);
digitalWrite(s1, HIGH);
//白平衡计算，10组算平均值
hanshu();
}
void hanshu(){
int r = 0, g = 0, b = 0; //脉冲计数器
float R_COE[10],G_COE[10], B_COE[10];//白平衡系数
digitalWrite(led, HIGH); //开灯
delay(150);
for(int i=0;i<10;i++){
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
//count OUT, RED
r = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测红色脉冲时长
digitalWrite(s3, HIGH);
//count OUT, BLUE
b = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测蓝色脉冲时长
digitalWrite(s2, HIGH);
//count OUT, GREEN
g = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测绿色脉冲时长
R_COE[i]=r/255; //红色白平衡系数
Serial.print("R_COE:");
Serial.println(R_COE[i]);
G_COE[i]=g/255; //绿色白平衡系数
Serial.print("G_COE:");
Serial.println(G_COE[i]);
B_COE[i]=b/255; //蓝色白平衡系数
Serial.print("B_COE:");
Serial.println(B_COE[i]);
}
float rr,gg,bb;
for(int i=0;i<10;i++){
rr+=R_COE[i];
gg+=G_COE[i];
bb+=B_COE[i];
}
//输出白平衡平均参数
Serial.print("R_balance:");
Serial.println(rr);
Serial.print("G_balance:");
Serial.println(gg);
Serial.print("B_balance:");
Serial.println(bb);
}
void loop(){
}==============================================================
更新（2019.10.12）：
直接看代码是一件痛苦的事情，可能用语言或者图来描述一下算法才是合理的，或者说才是思想的体现，因为代码里面充满了细节，而这些细节只有写代码的才知道为了什么，而有用的是整个思路。
主控制板程序：
负责控制和处理 颜色传感器，步进电机，二维码模块；主控制板的程序就是上图的四个，因为我写的时候使用VS编辑器写的，所以里面有很多多余的东西，我重新整理了下。
BigBigDiao_2：这个是主程序，但是里面也有一些其他的函数。
Stepper：这部分是步进电机的功能函数，步进电机不太懂的可以看看太极创客的视频和资料，讲的非常好 （电路电机参考知识 – 太极创客）。
剩下的两个是关于颜色识别和二维识别的功能函数。
这个文件夹的程序是烧录在主控制板上的。
副控制板程序：
负责控制舵机，也就是机械臂部分的，应为用一个控制板会出现冲突，所以才搞了两块，主控制板与副控制板通信实现舵机控制，从而实现机械臂的抓取放置动作。
这部分程序只有一个文件，但是这个要提前安装一个库，就是下图的库。
-
这部分程序烧录在副控制板。
程序思路（已经有一段时间了，有些东西也忘了，主要看思路）：
通过黑标计数黑线来实现位置的大致判断。首先从出发区出发，在到达第一物块和第二个物块的位置时，用颜色传感器识别这两个位置物料的颜色，第三个位置自动判断。到达二维码区开启二维码模块扫描功能，获得抓放次序，然后返回物料区的中心位置（这是理解自动规划路径的关键点，其实就是两个for循环比较实现的），还有就是放置区的颜色位置是死的，提前输入到宏定义里面就好了。每次抓取都从中心位置出发，然后回到出发位置。放置也是从中心位置出发，然后回到中心位置。（有更高级的思路，这里只是解释自己程序的思路）。最后回到出发区。
黑标和颜色识别在某些环境下会出现较大偏差，这是可以改进的地方。
还有一个白平衡系数，之前写了我就不赘述了。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
