江苏省工程训练赛--物料搬运小车（附代码）
2024-06-30 03:30:24
程序和方案还有很多要改进的地方，自己以后也没有机会再做这个比赛了，当时自己苦思冥想的东西，如果能帮助到需要的人，那就再次发挥了价值！也很希望大家比完赛后，能分享自己的思考，想法和方案。
================================================================
比赛视频，大神勿喷：https://www.bilibili.com/video/av49807687
================================================================
一开始我并不知道要参加这个比赛（老师偷偷给我报名了-_-!!），后来期末考试考完了，在楼下洗澡碰到实验室的同学，他说：“你是不是有一个比赛要参加”。我当时一脸懵逼，第二天老师就通知去实验室集合，说是有新项目。。。。。。
我们是第一次参加这个比赛项目，没啥经验，心里也没底，当时听完比赛规则，感觉还是挺简单的。（这种比赛项目难的不是做出来，而是做好，因为你不可能考虑到所有情况，比赛场地，光线啥的影响很大！！就算你在家里调试好了，到了比赛的地方，可能又会出现新问题，所以调试是最难的。）
我们是三月初开始做的，每天都花很多的时间在这上面，因为完全从零开始做，毫无头绪。比赛的内容听起来很简单，就是读取任务，然后按照任务顺序，抓取物块，放置到规定的地方。涉及到，电机控制，二维码读取，和一个路径的自主判断等，代码部分的主要难点在于各个模块之间的配合。特别是寻迹模块（黑标）与底盘的配合真的搞得头都大了！整个代码逻辑写出来做出来不难，就是细节很多，调试很麻烦，真的很麻烦！
而且这个比赛还有焊接电路版和拆装的环节，整个比赛我们一直处在疲于奔命的状态，我们负责装配的同学差点被逼疯了，田大头装机械臂的时候手都在抖，一边装一边骂（+_+!）。电路板在设计的时候可以把主控芯片和晶振分离出来，采用插拔式，这样焊接电路板会很省时间，只要焊接一个主控就好了，不然真的焊不完，就算焊完了几乎也用不了。。。
我们的方案是步进电机加麦克纳姆轮（超级稳（慢）的那种 ），但是寻迹做的不太好（黑标受光线影响太大！！！），底盘的运动速度不是很快，害怕速度给快了抖动（会飘）。
物块抓取是两块控制板通信。我们用了两块Arduino mega2560控制板，一块运行舵机控制程序，控制机械臂抓取物块，一块运行主程序，颜色识别是TCS230模块，二维码用的是二维码识别模块（微雪）。
整个项目思路得细节方面还是看代码吧，能注释的地方都注释了，变量命名也是有规律的（一个一个单词查的），一般是可以看懂的，这样可以直接理解思路！
下面是全部的代码（代码打包了，需要可以自取，和下面一样的）。
================================================================
程序重新打包了一下：
https://wwa.lanzoui.com/i1Je0mxt86d 密码:6k6y
字符串读取希函数库：
https://wwa.lanzoui.com/i42y87a
用到的其他库：
https://wwa.lanzoui.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 // DEBUG1beginModel();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//物料放置时从左至右为蓝,绿,红,对应序号为321enum 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/swhile (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; //物料放置时从左至右为红,绿,蓝,对应序号为123for (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); //定义步进电机控制对象aAccelStepper 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 2typedef 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---PowerdownS0: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, REDr = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测红色脉冲时长digitalWrite(s3, HIGH);//count OUT, BLUEb = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测蓝色脉冲时长digitalWrite(s2, HIGH);//count OUT, GREENg = 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++;}elsereturn;
}//析购函数
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---PowerdownS0: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, REDr = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测红色脉冲时长digitalWrite(s3, HIGH);//count OUT, BLUEb = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //检测蓝色脉冲时长digitalWrite(s2, HIGH);//count OUT, GREENg = 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循环比较实现的），还有就是放置区的颜色位置是死的，提前输入到宏定义里面就好了。每次抓取都从中心位置出发，然后回到出发位置。放置也是从中心位置出发，然后回到中心位置。（有更高级的思路，这里只是解释自己程序的思路）。最后回到出发区。
黑标和颜色识别在某些环境下会出现较大偏差，这是可以改进的地方。
还有一个白平衡系数，之前写了我就不赘述了。
江苏省工程训练赛--物料搬运小车（附代码）相关推荐
- python 随机森林调参_Python机器学习实践：随机森林算法训练及调参-附代码
原标题:Python机器学习实践:随机森林算法训练及调参-附代码 文章发布于公号[数智物语] (ID:decision_engine),关注公号不错过每一篇干货. 来源 | 博客园 作者 | 战争热诚 ...
- 2022年 大学生工程训练比赛[物料搬运]
本人和团结参加了2022年大学生工程训练(简称工训赛)校赛选拔,准备了几个月的时间和花费了较多的资金,由于疫情等多种情况,很遗憾未能参加湖南省省赛,过了这么久还是写个博客记录参赛准备和调试过程. 目录 ...
- 工程训练大赛物流小车_我校在全国大学生工程训练综合能力竞赛总决赛中创佳绩...
6月1日至6月2日,第六届全国大学生工程训练综合能力竞赛总决赛在天津职业技术师范大学隆重举行.我校三名学子在无碳小车"S环形"赛道挑战赛中获一等奖,这是我校首次在该项赛事中摘得重量 ...
- 工程训练大赛物流小车_宁大机械学子在工程训练省赛中斩获佳绩，取得历史性突破...
2019第六届 浙江省大学生工程训练综合能力竞赛暨第五届浙江创客教育基地联盟创客大赛 圆满落幕 --宁大斩获省一等奖1项,二等奖3项,三等奖1项,获奖率100%,获奖人数15人创历史新高. 赛事介绍 ...
- slim php dd model,第二十四节，TensorFlow下slim库函数的使用以及使用VGG网络进行预训练、迁移学习(附代码)...
在介绍这一节之前,需要你对slim模型库有一些基本了解,具体可以参考第二十二节,TensorFlow中的图片分类模型库slim的使用.数据集处理,这一节我们会详细介绍slim模型库下面的一些函数的使用 ...
- 手把手带你入门和实践特征工程的万字笔记（附代码下载）
? 说起特征工程,都说是机器学习建模中最为重要而且费时的一项工作,而且它涉及的知识点会非常地多,经验老道的老司机自然是轻车熟路了,但对于刚刚入门的新手司机,学习到的知识点都是东一点西一点的,不够系统化 ...
- 快速上手笔记，PyTorch模型训练实用教程（附代码）
机器之心发布 作者:余霆嵩 前言 自 2017 年 1 月 PyTorch 推出以来,其热度持续上升,一度有赶超 TensorFlow 的趋势.PyTorch 能在短时间内被众多研究人员和工程师接受并 ...
- 随机森林算法训练及调参-附代码
随机森林算法的理论知识 随机森林是一种有监督学习算法,是以决策树为基学习器的集成学习算法.随机森林非常简单,易于实现,计算开销也很小,但是它在分类和回归上表现出非常惊人的性能,因此,随机森林被誉为&q ...
- Python机器学习实践：随机森林算法训练及调参-附代码
文章发布于公号[数智物语] (ID:decision_engine),关注公号不错过每一篇干货. 来源 | 博客园 作者 | 战争热诚 随机森林是一种有监督学习算法,是以决策树为基学习器的集成学习算法 ...
最新文章
- Html 教程 （1）简介
- Sleep() sleep() usleep()
- 序列化与反序列化存储、updatepanel
- 机器学习小字典（一）——SVM
- 提升工作效率的优秀工具收藏
- python sklearn 梯度下降法_Python- sklearn之梯度下降算法原理
- Spring+SpringMVC+MyBatis+easyUI整合基础篇（五）讲一下maven
- c#（asp.net/core）杂谈笔记
- php完美实现下载远程图片保存到本地
- ipython安装成功后用不了_ipython安装避坑指南
- 6-3近期工作总结、下一步工作安排及技术知识
- 服务器显示board板,IBM x3650M4面板Board亮黄灯 故障维修
- 计算机应用基础系统总线,2019年自学考试计算机应用基础试题(4)
- 域服务器性能要求,ad域服务器配置要求
- 【论文阅读】The Generals’ Scuttlebutt: Byzantine-Resilient Gossip Protocols
- 面纱星云的西面纱中的女巫帚星云
- 埃森哲 java_【埃森哲JAVA软件工程师面试】要求挺高，从面试到拿到offer历经时间长-看准网...
- jquery语法三ajax+echarts插件的使用
- Java Web实现用户登录功能
- 关于程序员今年的“金三银四”被网友们称为“铜三铁四”这回事.......