程序和方案还有很多要改进的地方，自己以后也没有机会再做这个比赛了，当时自己苦思冥想的东西，如果能帮助到需要的人，那就再次发挥了价值！也很希望大家比完赛后，能分享自己的思考，想法和方案。
================================================================
比赛视频，大神勿喷：https://www.bilibili.com/video/av49807687
================================================================
一开始我并不知道要参加这个比赛（老师偷偷给我报名了-_-!!），后来期末考试考完了，在楼下洗澡碰到实验室的同学，他说：“你是不是有一个比赛要参加”。我当时一脸懵逼，第二天老师就通知去实验室集合，说是有新项目。。。。。。
我们是第一次参加这个比赛项目，没啥经验，心里也没底，当时听完比赛规则，感觉还是挺简单的。（这种比赛项目难的不是做出来，而是做好，因为你不可能考虑到所有情况，比赛场地，光线啥的影响很大！！就算你在家里调试好了，到了比赛的地方，可能又会出现新问题，所以调试是最难的。）
我们是三月初开始做的，每天都花很多的时间在这上面，因为完全从零开始做，毫无头绪。比赛的内容听起来很简单，就是读取任务，然后按照任务顺序，抓取物块，放置到规定的地方。涉及到，电机控制，二维码读取，和一个路径的自主判断等，代码部分的主要难点在于各个模块之间的配合。特别是寻迹模块（黑标）与底盘的配合真的搞得头都大了！整个代码逻辑写出来做出来不难，就是细节很多，调试很麻烦，真的很麻烦！
而且这个比赛还有焊接电路版和拆装的环节，整个比赛我们一直处在疲于奔命的状态，我们负责装配的同学差点被逼疯了，田大头装机械臂的时候手都在抖，一边装一边骂（+_+!）。电路板在设计的时候可以把主控芯片和晶振分离出来，采用插拔式，这样焊接电路板会很省时间，只要焊接一个主控就好了，不然真的焊不完，就算焊完了几乎也用不了。。。
我们的方案是步进电机加麦克纳姆轮（超级稳（慢）的那种 ），但是寻迹做的不太好（黑标受光线影响太大！！！），底盘的运动速度不是很快，害怕速度给快了抖动（会飘）。
物块抓取是两块控制板通信。我们用了两块Arduino mega2560控制板，一块运行舵机控制程序，控制机械臂抓取物块，一块运行主程序，颜色识别是TCS230模块，二维码用的是二维码识别模块（微雪）。
整个项目思路得细节方面还是看代码吧，能注释的地方都注释了，变量命名也是有规律的（一个一个单词查的），一般是可以看懂的，这样可以直接理解思路！
下面是全部的代码（代码打包了，需要可以自取，和下面一样的）。
================================================================
程序重新打包了一下：
https://wwa.lanzous.com/i1Je0mxt86d 密码:6k6y
用到的其他库：
https://www.lanzous.com/i6rju8f 密码: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：这部分是步进电机的功能函数，步进电机不太懂的可以看看太极创客的视频和资料，讲的非常好 （http://www.taichi-maker.com/homepage/reference-index/circuit-reference-index/）。
剩下的两个是关于颜色识别和二维识别的功能函数。
这个文件夹的程序是烧录在主控制板上的。
副控制板程序：
负责控制舵机，也就是机械臂部分的，应为用一个控制板会出现冲突，所以才搞了两块，主控制板与副控制板通信实现舵机控制，从而实现机械臂的抓取放置动作。
这部分程序只有一个文件，但是这个要提前安装一个库，就是下图的库。
-
这部分程序烧录在副控制板。
程序思路（已经有一段时间了，有些东西也忘了，主要看思路）：
通过黑标计数黑线来实现位置的大致判断。首先从出发区出发，在到达第一物块和第二个物块的位置时，用颜色传感器识别这两个位置物料的颜色，第三个位置自动判断。到达二维码区开启二维码模块扫描功能，获得抓放次序，然后返回物料区的中心位置（这是理解自动规划路径的关键点，其实就是两个for循环比较实现的），还有就是放置区的颜色位置是死的，提前输入到宏定义里面就好了。每次抓取都从中心位置出发，然后回到出发位置。放置也是从中心位置出发，然后回到中心位置。（有更高级的思路，这里只是解释自己程序的思路）。最后回到出发区。
黑标和颜色识别在某些环境下会出现较大偏差，这是可以改进的地方。
还有一个白平衡系数，之前写了我就不赘述了。
转载：https://blog.csdn.net/C1664510416/article/details/89402154
查看评论