Arduino智能物流小车各部分功能代码详解(省工程训练能力综合竞赛)
2024-05-18 12:22:47
文章目录
- 前言
- 测试部分
- 循迹部分
- 超声波测距部分
- 避障部分
- 机械臂控制部分
- 扫码部分
- 初赛部分
前言
考虑到之前那篇博客写的太简略，也只给了一个最终代码，比较容易让人看不懂。本篇主要写一些不同功能函数的代码讲解，要看整体部分的请前往我的另一篇博客—基于Arduino(MEGA2560)的智能物流小车项目
测试部分
▶ 此代码用于测试车轮是否正确连线，以及不同高低电平的小车行进状态。
▶ 更改运动控制函数的高低电平(HIGH,LOW)，保证小车烧录代码后能依次执行向前、向后、向左、向右、停止四个运动状态。
▶ 若不是正常顺序，更改代码即可，并且记住自己小车的四个状态的高低电平。
#include <Servo.h>
//定义五中运动状态
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4
//定义需要用到的引脚
int leftMotor1 = 14;
int leftMotor2 = 15;
int rightMotor1 = 16;
int rightMotor2 = 17;
int leftPWM = 2;
int rightPWM = 3;
void setup() {// put your setup code here, to run once://设置控制电机的引脚为输出状态pinMode(leftMotor1, OUTPUT);pinMode(leftMotor2, OUTPUT);pinMode(rightMotor1, OUTPUT);pinMode(rightMotor2, OUTPUT);pinMode(leftPWM, OUTPUT);pinMode(rightPWM, OUTPUT);
}void loop() {int cmd;for(cmd=0;cmd<4;cmd++)//依次执行向前、向后、向左、向右、停止四个运动状态{motorRun(cmd,255); delay(1000);//每个命令执行2s }
}
//运动控制函数(注意！需要更改的地方为此处)
void motorRun(int cmd,int value)
{analogWrite(leftPWM, value); //设置PWM输出，即设置速度analogWrite(rightPWM, value);switch(cmd){case 0:Serial.println("FORWARD"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;case 1:Serial.println("BACKWARD"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2,HIGH );digitalWrite(rightMotor1,HIGH );digitalWrite(rightMotor2,LOW);break;case 2:Serial.println("TURN LEFT"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, LOW);break;case 3:Serial.println("TURN RIGHT"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;default:Serial.println("STOP"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, LOW);}
}
循迹部分
#include <Servo.h>#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4int leftMotor1 = 14; //直流电机引脚
int leftMotor2 = 15;
int rightMotor1 = 16;
int rightMotor2 = 17;int trac1 = 11; //车头四个灰度传感器引脚
int trac2 = 10; //从车头方向的最右边开始排序
int trac3 = 9;
int trac4 = 8; int leftPWM = 3; //驱动PWM引脚
int rightPWM = 2;int data[4]; //记录灰度读取值(0/1)void setup() {//串口初始化Serial.begin(9600); //测速引脚初始化pinMode(leftMotor1, OUTPUT);pinMode(leftMotor2, OUTPUT);pinMode(rightMotor1, OUTPUT);pinMode(rightMotor2, OUTPUT);pinMode(leftPWM, OUTPUT);pinMode(rightPWM, OUTPUT);//寻迹模块引脚初始化pinMode(trac1, INPUT);pinMode(trac2, INPUT);pinMode(trac3, INPUT);pinMode(trac4, INPUT);
}//运动控制函数//确保与测试部分测试好的函数相同
void motorRun(int cmd,int value)
{analogWrite(leftPWM, value); //设置PWM输出，即设置速度analogWrite(rightPWM, value);switch(cmd){case FORWARD:Serial.println("FORWARD"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, HIGH);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;case BACKWARD:Serial.println("BACKWARD"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, HIGH);digitalWrite(rightMotor2, LOW);break;case TURNLEFT:Serial.println("TURN LEFT"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2,HIGH);digitalWrite(rightMotor1,HIGH);digitalWrite(rightMotor2,LOW);break;case TURNRIGHT:Serial.println("TURN RIGHT"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;default:Serial.println("STOP"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, LOW);}
}//循迹函数
void tracing()
{data[0] = digitalRead(11);data[1] = digitalRead(10);data[2] = digitalRead(9);data[3] = digitalRead(8);if((!data[0] && data[1] && data[2] & 4& !data[3])) {motorRun(FORWARD,220); }else if(data[0] && data[1] && data[2] && data[3]){motorRun(FORWARD,220);}/*else if(!data[0] && data[1] && data[2] && data[3]){motorRun(TURNLEFT, 100,100);} 上坡微调*/ else if(data[2] || data[3]) {motorRun(TURNLEFT,255);}else if(data[0] || data[1]) {motorRun(TURNRIGHT,255);}/* else if(!data[0] && !data[1] && data[2] &&!data[3]) {motorRun(TURNLEFT,220);}else if(!data[0] && !data[1] && data[2] && data[3]) {motorRun(TURNLEFT,220);}else if(!data[0] && !data[1] && !data[2] && data[3]) {motorRun(TURNLEFT,220);}else if(!data[0] && data[1] && !data[2] && !data[3]) {motorRun(TURNRIGHT,220);}else if(data[0] && data[1] && !data[2] && !data[3]) {motorRun(TURNRIGHT,220);}*/Serial.print(data[0]);Serial.print("---");Serial.print(data[1]);Serial.print("---");Serial.print(data[2]);Serial.print("---");Serial.print(data[3]);Serial.print("---");
}void loop()
{tracing();
}
超声波测距部分
const int TrigPin = 5;
const int EchoPin = 4; // 发射，接收引脚
float cm;
void setup()
{
Serial.begin(9600);
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);
}
void loop()
{
digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW); cm = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm
cm = (int(cm * 100.0)) / 100.0; //保留两位小数
Serial.print(cm);
Serial.print("cm");
Serial.println();
delay(1000);
}
避障部分
▶ 此代码用于检测小车运动过程中，超声波是否在正常工作。有时候通过串口监视超声波能正确读出距离，小车运动时超声波就不会正常工作。
▶ 此代码为小车一直前进，检测到障碍物时，执行左转-前行-右转-大前行-右转-前行-左转从而达到避开障碍物的目的且车头回正。
#include <Servo.h>#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4int leftMotor1 = 14;
int leftMotor2 = 15;
int rightMotor1 = 16;
int rightMotor2 = 17;int leftPWM = 2;
int rightPWM = 3;Servo myServo; //舵机int inputPin=4; // 定义超声波信号接收接口
int outputPin=5; // 定义超声波信号发出接口int dis;
float cm; void setup() {// put your setup code here, to run once://串口初始化Serial.begin(9600); //舵机引脚初始化myServo.attach(4);//测速引脚初始化pinMode(leftMotor1, OUTPUT);pinMode(leftMotor2, OUTPUT);pinMode(rightMotor1, OUTPUT);pinMode(rightMotor2, OUTPUT);pinMode(leftPWM, OUTPUT);pinMode(rightPWM, OUTPUT);//超声波控制引脚初始化pinMode(inputPin, INPUT);pinMode(outputPin, OUTPUT);
}
void motorRun(int cmd,int value)
{analogWrite(leftPWM, value); //设置PWM输出，即设置速度analogWrite(rightPWM, value);switch(cmd){case FORWARD:Serial.println("FORWARD"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, HIGH);digitalWrite(rightMotor2, LOW);break;case BACKWARD:Serial.println("BACKWARD"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, HIGH);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;case TURNLEFT:Serial.println("TURN LEFT"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2,HIGH );digitalWrite(rightMotor1,HIGH );digitalWrite(rightMotor2,LOW);break;case TURNRIGHT:Serial.println("TURN RIGHT"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;default:Serial.println("STOP"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, LOW);}
}
void avoidance()
{motorRun(FORWARD,200);dis=getDistance(); //正前方距离Serial.print(dis); Serial.println(); if(dis<40){motorRun(STOP,0);delay(1000);motorRun(TURNLEFT,255);delay(1100);motorRun(FORWARD,255);delay(1500);motorRun(TURNRIGHT,255);delay(1100);motorRun(FORWARD,255);delay(1800);motorRun(TURNRIGHT,255);delay(1100);motorRun(FORWARD,255);delay(1300);motorRun(TURNLEFT,255);delay(1100);}
}int getDistance()
{digitalWrite(outputPin, LOW); // 使发出发出超声波信号接口低电平2μsdelayMicroseconds(2);digitalWrite(outputPin, HIGH); // 使发出发出超声波信号接口高电平10μs，这里是至少10μsdelayMicroseconds(10);digitalWrite(outputPin, LOW); // 保持发出超声波信号接口低电平int distance = pulseIn(inputPin, HIGH); // 读出脉冲时间distance= distance/58; // 将脉冲时间转化为距离（单位：厘米）Serial.println(distance); //输出距离值if (distance>180||distance<1){//如果距离小于1大于180厘米返回数据return 180;}elsereturn distance;
}
void loop()
{avoidance();
}
机械臂控制部分
▶ 此部分用于检测舵机板是否正常控制机械臂运动。
#include <Wire.h>
#include <SoftwareSerial.h>
//发送部分的指令
#define FRAME_HEADER 0xFF //帧头
#define CMD_SERVO_SPEED 0x01 //设置舵机速度
#define CMD_SERVO_PLACE 0x02 //设置舵机位置
#define CMD_ACTION_GROUP_RUN 0x09 //运行动作组
#define CMD_STOP_REFRESH 0x0b //急停、恢复指令
#define GET_LOW_BYTE(A) (uint8_t)((A)) //宏函数 获得A的低八位
#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8) //宏函数 获得A的高八位
SoftwareSerial ServoSerial(19,18); // RX, TX 软串口
int old[6] = {30,138,157,162,82,45};//记录上一次舵机状态
void setup() {// put your setup code here, to run once:ServoSerial.begin(9600);Serial.begin(9600);servo_(164,0,20,135,180,65);//手爪下移到物块中间（手爪开）}
/************************************************函数名：moveServo功能：舵机运动控制 参数：servoID 舵机编号 Position：目标位置 Speed：移动速度************************************************/
void moveServo(uint8_t servoID, uint16_t Position, uint16_t Speed)
{uint8_t buf[15];
// if (servoID > 15 || !(Speed > 0)) {
// return;
// }Position = Position*100/9+500; buf[0] = FRAME_HEADER; buf[1] = CMD_SERVO_SPEED;buf[2] = servoID; buf[3] = GET_LOW_BYTE(Speed); buf[4] = GET_HIGH_BYTE(Speed); buf[5] = FRAME_HEADER; buf[6] = CMD_SERVO_PLACE; buf[7] = servoID; buf[8] = GET_LOW_BYTE(Position); buf[9] = GET_HIGH_BYTE(Position); ServoSerial.write(buf, 10);
}
/************************************************函数名：servo_功能：机械臂运动控制 参数：D1 D2 D3 D4 D5 D6 6个参数依次为6个舵机转动到的角度************************************************/
void servo_(int D1, int D2, int D3, int D4, int D5, int D6)
{int d[6];int t[6];int d_max = 0;int t_max = 0;int speed;int place[6] = {D1, D2, D3, D4, D5, D6 };for (int i = 0; i < 6; i++) {d[i] = abs(place[i] - old[i]);if (d[i] > d_max) {d_max = d[i];}old[i] = place[i];}for (int i = 0; i < 6; i++) {if (d_max >= 60) {speed = d[i] * 16 / d_max;}else {speed = d[i] * 10 / d_max;}if ((d[i] > 140) && (i == 1)){speed = 20;} if (speed > 20){speed = 20;} if (speed <= 5) {speed = 5;}moveServo(i, place[i], speed/2);t[i] = d[i] * 112 / speed;if (t[i] > t_max) {t_max = t[i];}}delay(t_max);
}
/*************************************************函数名：grab_init()功能：机械臂初始状态*************************************************/void grab_init(){}
/*************************************************函数名：grab()功能：拽去一个物料*************************************************/
void grab()
{}
/*************************************************函数名：put_down()功能：放下一个物料*************************************************/
void put_down()
{
}
void loop()
{servo_(90,32,29,158,18,14);servo_(86,72,34,180,11,14);servo_(92,97,52,180,11,14);servo_(92,97,52,180,11,50);
}
扫码部分
▶ 此代码用于检测红外扫码模块是否正常工作。
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>SoftwareSerial mySerial(12,13); // RX TX, PCINTString SciString = ""; // 主控板接收扫码模块发送回来的字符串unsigned char SetSeiDecMod[] = {0x07,0xC6,0x04,0x00,0xFF,0x8A,0x08,0xFD,0x9E}; //扫码模块初始化协议
unsigned char StaDec[] = {0x04,0xE4,0x04,0x00,0xFF,0x14}; //扫码模块扫码协议
boolean flag_SetSeiDecMod = true; //扫码模块初始化标志位
boolean flag_StaDec = false; //扫码模块扫码标志位
int i,j;
int Position = 0; //字符在字符串里的位置
char scanflag = ' '; //字符串最后一位字符 e.g ZJGXDS01 返回 1
void setup()
{Serial.begin(9600);mySerial.begin(9600);
}
char scan()
{char get_char = ' ';if(flag_SetSeiDecMod == true) //模块初始化功能{for(i = 0;i < sizeof(SetSeiDecMod);i++){mySerial.write(SetSeiDecMod[i]);delay(1);}flag_SetSeiDecMod = 0;flag_StaDec = 1;}if(flag_StaDec == 1) //模块扫码功能{for(i = 0;i < sizeof(StaDec);i++){mySerial.write(StaDec[i]);delay(1);}}delay(10); while(mySerial.available()) //所扫的字符串返回{char inChar = (char)mySerial.read();delay(5);SciString += inChar;} Position = SciString.indexOf('Z'); //找到字符Z的位置 if(Position != -1) //字符'Z'找到{SciString = SciString.substring(Position,SciString.length()); //截取有效字符串 e.g sdaaf,ZJGXDS01 ---------> ZJGXDS01// 返回的字符串 截取到有效的字符串 get_char = SciString.charAt(7); // 返回第7位字符 e.g ZJGXDS01 ----> 1SciString = ""; //接受字符串清空 return get_char; //返回有效字符串的最后一位字符}else //字符'Z'未能找到 {flag_SetSeiDecMod = 1; //初始化标志位 方便第二次扫码flag_StaDec = 0;SciString = ""; //接受字符串清空 get_char = ' '; //返回空字符return get_char;} delay(5000);
}
void loop()
{ if(scanflag != ' ') //有效字符串末尾字符不是空字符 则打印该字符{Serial.println(scanflag);scanflag = ' ';//拽去对应的物体}else //每隔0.3秒 扫码一次 直到找到有效字符串{//调整机械臂scanflag = scan();delay(500);//Serial.println(scanflag);}
}
初赛部分
初赛与复赛的区别是
▶ 初赛：上料5抓3 中途为避障
▶ 复赛：上料10抓6 中途为跨越减速带或栅格
另一篇博客中是复赛代码
#include <Servo.h>
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4
#define FRAME_HEADER 0xFF //帧头
#define CMD_SERVO_SPEED 0x01 //设置舵机速度
#define CMD_SERVO_PLACE 0x02 //设置舵机位置
#define CMD_ACTION_GROUP_RUN 0x09 //运行动作组
#define CMD_STOP_REFRESH 0x0b //急停、恢复指令
#define GET_LOW_BYTE(A) (uint8_t)((A)) //宏函数 获得A的低八位
#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8) //宏函数 获得A的高八位
int leftMotor1 = 14;
int leftMotor2 = 15;
int rightMotor1 = 16;
int rightMotor2 = 17;//定义电机
int trac1 =11;
int trac2 = 10;
int trac3 = 9;
int trac4 = 8; //定义前侧红外/从车头方向的最右边开始排序
int trac5 = 7;
int trac6 = 6;//定义右侧红外
int inputPin =4; // 定义超声波信号接收接口
int outputPin =5; // 定义超声波信号发出接口
int leftPWM = 3;
int rightPWM = 2;//定义调速
int Flag_1 = 0; //取物料标志
int Flag_2 = 0; //放物料标志
int dis;
int q,w,e,r= 0;
int data[6];//定义红外读取数组
int i,j;
int Position=0;
char scanflag = ' ';
String SciString = " ";
boolean flag_SetSeiDecMod = true;
boolean flag_StaDec = false;
unsigned char SetSeiDecMod[] = {0x07,0xC6,0x04,0x00,0xFF,0x8A,0x08,0xFD,0x9E};
unsigned char StaDec[] = {0x04,0xE4,0x04,0x00,0xFF,0x14};
SoftwareSerial mySerial(12,13); // TX,RX
SoftwareSerial ServoSerial(19,18); // RX, TX 软串口void setup()
{Serial.begin(9600); //串口初始化ServoSerial.begin(9600);mySerial.begin(9600);pinMode(leftMotor1, OUTPUT);pinMode(leftMotor2, OUTPUT);pinMode(rightMotor1, OUTPUT);pinMode(rightMotor2, OUTPUT);pinMode(leftPWM, OUTPUT);pinMode(rightPWM, OUTPUT); //测速引脚初始化pinMode(trac1, INPUT);pinMode(trac2, INPUT);pinMode(trac3, INPUT);pinMode(trac4, INPUT); pinMode(trac5, INPUT);pinMode(trac6, INPUT);pinMode(inputPin, INPUT);pinMode(outputPin, OUTPUT); //超声波控制引脚初始化
}void motorRun(int cmd,int value)
{analogWrite(leftPWM, value); //设置PWM输出，即设置速度analogWrite(rightPWM, value);switch(cmd){case FORWARD:Serial.println("FORWARD"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;case BACKWARD:Serial.println("BACKWARD"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2,HIGH );digitalWrite(rightMotor1,HIGH );digitalWrite(rightMotor2,LOW);break;case TURNLEFT:Serial.println("TURN LEFT"); //输出状态digitalWrite(leftMotor1, HIGH);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, HIGH);digitalWrite(rightMotor2, LOW);break;case TURNRIGHT:Serial.println("TURN RIGHT"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, HIGH);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, HIGH);break;default:Serial.println("STOP"); //输出状态digitalWrite(leftMotor1, LOW);digitalWrite(leftMotor2, LOW);digitalWrite(rightMotor1, LOW);digitalWrite(rightMotor2, LOW);}
}int getDistance()
{digitalWrite(outputPin, LOW); // 使发出发出超声波信号接口低电平2μsdelayMicroseconds(2);digitalWrite(outputPin, HIGH); // 使发出发出超声波信号接口高电平10μs，这里是至少10μsdelayMicroseconds(10);digitalWrite(outputPin, LOW); // 保持发出超声波信号接口低电平int distance = pulseIn(inputPin, HIGH); // 读出脉冲时间distance= distance/58; // 将脉冲时间转化为距离（单位：厘米）Serial.println(distance); //输出距离值if (distance > 180||distance < 15){return 180;}elsereturn distance;
}void tracing()
{data[0] = digitalRead(11);data[1] = digitalRead(10);data[2] = digitalRead(9);data[3] = digitalRead(8);data[4] = digitalRead(7);data[5] = digitalRead(6);if((!data[0] && data[1] && data[2] && !data[3])) {motorRun(FORWARD,255);}else if(data[0] && data[1] && data[2] && data[3]){motorRun(FORWARD,255);}else if(!data[0] && data[1] && data[2] && data[3]){motorRun(FORWARD,255);}else if(!data[0] && !data[1] && data[2] && data[3]){motorRun(TURNLEFT, 225);}else if(data[2] || data[3]){motorRun(TURNLEFT, 225);}else if(data[0] || data[1]) {motorRun(TURNRIGHT, 225);}
}void tracing2()
{data[0] = digitalRead(11);data[1] = digitalRead(10);data[2] = digitalRead(9);data[3] = digitalRead(8);data[4] = digitalRead(7);data[5] = digitalRead(6);if((!data[0] && data[1] && data[2] && !data[3])) {motorRun(FORWARD,220);}else if(data[0] && data[1] && data[2] && data[3]){motorRun(FORWARD,220);}else if(!data[0] && data[1] && data[2] && data[3]){motorRun(FORWARD,220);}else if(data[0] && data[1] && data[2] && !data[3]){motorRun(FORWARD,220);}else if(data[0] && data[1] && !data[2] && !data[3]){motorRun(FORWARD,220);}else if(!data[0] && !data[1] && data[2] && data[3]){motorRun(TURNLEFT, 200);}else if( data[2] || data[3]){motorRun(TURNLEFT, 200);}else if(data[0] || data[1]) {motorRun(TURNRIGHT, 200);}
}void tracing3()
{data[0] = digitalRead(11);data[1] = digitalRead(10);data[2] = digitalRead(9);data[3] = digitalRead(8);data[4] = digitalRead(7);data[5] = digitalRead(6);if((data[1] && data[2])) {motorRun(FORWARD,220);}else if( data[0] && data[1]){motorRun(FORWARD,220);}else if( data[1] && !data[2]){motorRun(TURNRIGHT, 190);}else if(!data[1] && data[2]) {motorRun(TURNLEFT, 190);}
}void moveServo(uint8_t servoID, uint16_t Position1, uint16_t Speed)
{uint8_t buf[15];Position1 = Position1*100/9+500; buf[0] = FRAME_HEADER; buf[1] = CMD_SERVO_SPEED;buf[2] = servoID; buf[3] = GET_LOW_BYTE(Speed); buf[4] = GET_HIGH_BYTE(Speed); buf[5] = FRAME_HEADER; buf[6] = CMD_SERVO_PLACE; buf[7] = servoID; buf[8] = GET_LOW_BYTE(Position1); buf[9] = GET_HIGH_BYTE(Position1); ServoSerial.write(buf, 10);
}void servo_(int D1, int D2, int D3, int D4, int D5, int D6)
{int old[6] = {90,90,90,90,90,90};//初始位置int d[6];int t[6];int d_max = 0;int t_max = 0;int speed;int place[6] = {D1, D2, D3, D4, D5, D6 };for (int i = 0; i < 6; i++) {d[i] = abs(place[i] - old[i]);if (d[i] > d_max) {d_max = d[i];}old[i] = place[i];}for (int i = 0; i < 6; i++) {if (d_max >= 60) {speed = d[i] * 16 / d_max;}else {speed = d[i] * 10 / d_max;}if ((d[i] > 140) && (i == 1)){speed = 20;} if (speed > 20){speed = 20;} if (speed <= 5) {speed = 5;}moveServo(i, place[i], speed*3/2);t[i] = d[i] * 112 / speed;if (t[i] > t_max) {t_max = t[i];}}//delay(t_max);
}char scan()
{char get_char = ' ';Position=0;if(flag_SetSeiDecMod == true) //模块初始化功能{for(i = 0;i < sizeof(SetSeiDecMod);i++){mySerial.write(SetSeiDecMod[i]);delay(1);}flag_SetSeiDecMod = 0;flag_StaDec = 1;}if(flag_StaDec == 1) //模块扫码功能{for(i = 0;i < sizeof(StaDec);i++){mySerial.write(StaDec[i]);delay(1);}}delay(10); while(mySerial.available()) //所扫的字符串返回{char inChar = (char)mySerial.read();delay(5);SciString += inChar;}Position = SciString.indexOf('Z'); //找到字符Z的位置 if(Position != -1) //字符'Z'找到{SciString = SciString.substring(Position,SciString.length()); //截取有效字符串 e.g sdaaf,ZJGXDS01 ---------> ZJGXDS01// 返回的字符串 截取到有效的字符串 get_char = SciString.charAt(7); // 返回第7位字符 e.g ZJGXDS01 ----> 1SciString = ""; //接受字符串清空 return get_char; //返回有效字符串的最后一位字符}else //字符'Z'未能找到 {flag_SetSeiDecMod = 1; //初始化标志位 方便第二次扫码flag_StaDec = 0;SciString = ""; //接受字符串清空 get_char = ' '; //返回空字符return get_char;}
}void judgment1()
{ switch(scanflag){case '3': //***************************抽签调整**********************************moveServo(6, 0, 15);delay(500);grab1();break;case '6': //***************************抽签调整**********************************moveServo(6, 36, 15);delay(500);grab1();break;case '9' : //***************************抽签调整**********************************moveServo(6, 72, 15);delay(500);grab1();break;}initial_a();delay(300);Serial.println(scanflag);while(mySerial.read() >= 0){}/*while(mySerial.available()>0)mySerial.read();*/Flag_1=Flag_1+1;motorRun(FORWARD,200);delay(300);
}void judgment3()
{ switch(scanflag){case '3': //***************************抽签调整**********************************moveServo(6, 0, 15);delay(500);laydown();break;case '6': //***************************抽签调整**********************************moveServo(6, 36, 15);delay(500);laydown();break;case '9': //***************************抽签调整**********************************moveServo(6, 72, 15);delay(500);laydown();break;}Serial.println(scanflag);while(mySerial.read() >= 0){}/*while(mySerial.available()>0)mySerial.read();*/
}void judgment4()
{ switch(scanflag){case '3': //***************************抽签调整**********************************moveServo(6, 0, 15);delay(500);layup();break;case '6': //***************************抽签调整**********************************moveServo(6, 36, 15);delay(500);layup();break;case '9': //***************************抽签调整**********************************moveServo(6, 72, 15);delay(500);layup();break;}initial_b();delay(500);Serial.println(scanflag);while(mySerial.read() >= 0){}/*while(mySerial.available()>0)mySerial.read();*/Flag_2=Flag_2+1;motorRun(FORWARD,200);delay(300);
}void avoidance()
{dis=getDistance(); //正前方距离dis=0;dis=getDistance(); Serial.print("***");Serial.print(dis);Serial.print("***");if(dis < 30){motorRun(TURNLEFT,255);delay(920);motorRun(FORWARD,255);delay(1010);motorRun(TURNRIGHT,255);delay(925);motorRun(FORWARD,255);delay(1220);motorRun(TURNRIGHT,255);delay(950);w=w+1;}
}void initial_a()
{
servo_(129,70,108,161,91,65);
}void initial_b()
{
servo_(128,110,50,124,91,65);
}void grab1()
{e=e+1;servo_(129,61,99,146,93,75);delay(300);servo_(129,61,99,146,93,45);delay(300);servo_(129,77,99,146,93,45);delay(300);servo_(11,97,74,138,85,45);delay(1200);servo_(11,100,65,149,85,45);delay(300);servo_(11,86,65,149,85,45);delay(300);servo_(11,86,65,149,85,75);delay(300);servo_(11,95,70,140,93,75);delay(300);servo_(129,80,95,134,93,70); delay(1200);
}void layup()
{e=e+1;servo_(13,90,90,162,85,70);delay(1200);servo_(13,68,92,162,85,70);delay(300);servo_(13,68,92,162,85,45);delay(300);servo_(13,92,92,162,85,45);delay(300);servo_(128,98,79,124,95,45);delay(1200);servo_(128,77,83,124,95,45);delay(300);servo_(128,77,83,130,95,67);delay(300);servo_(128,105,79,130,95,67);delay(300);
}void laydown()
{e=e+1;servo_(128,106,90,124,91,65);delay(300);servo_(13,90,90,162,85,70);delay(1200);servo_(13,68,92,162,85,70);delay(300);servo_(13,68,92,162,85,45);delay(300);servo_(13,92,92,162,85,45);delay(300);servo_(128,90,79,162,90,45);delay(1200);servo_(128,120,79,162,90,45);delay(300);servo_(128,120,49,121,90,45);delay(300);servo_(128,91,50,110,90,45);delay(500);servo_(128,91,50,110,90,67);delay(800);servo_(128,117,38,110,90,67);delay(800);
}void loop()
{//Flag_1=8;//Flag_1=99;//Flag_2=1;initial_a();motorRun(FORWARD,255);delay(200);//initial_b();while(Flag_1 == 0){ tracing();//循迹if(data[0]&&data[1]&&data[2]&&data[3]){motorRun(FORWARD,255);delay(200);Flag_1= 1;}}while(Flag_1 == 1) //上坡十字调整{tracing3();if(data[0]&&data[1]&&!data[3]){Flag_1 = 2;}}while(Flag_1 == 2){data[4] = digitalRead(7);motorRun(FORWARD,220);//tracing3();if(data[4]){ Flag_1 = 3;}}while(Flag_1 == 3) //上料第一次停止{motorRun(BACKWARD,150);delay(405); //上料第一次急停调整motorRun(STOP,0);servo_(129,65,108,162,91,70); //上料区第一次扫码1delay(300);q=162;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(200);if(scanflag != ' '){r=r+1;}q=q-1;servo_(129,65,108,q,91,70);}judgment1();}while( Flag_1 == 4) //上料第二次停止{ data[4] = digitalRead(7);r=0;motorRun(FORWARD,200);if(data[4]){ motorRun(BACKWARD,150);delay(105); //上料第二次急停调整 motorRun(STOP,0); servo_(129,65,108,162,91,70); //上料区第二次扫码1delay(300);q=162;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(200);if(scanflag != ' '){r=r+1;}q=q-1;servo_(129,65,108,q,91,70);}judgment1();}}while( Flag_1 == 5) //上料第三次停止{r=0;motorRun(FORWARD,200);data[4] = digitalRead(7);if(data[4] ){ motorRun(BACKWARD,150);delay(185); //上料第三次急停调整 motorRun(STOP,0);servo_(129,65,108,162,91,70); //上料区第三次扫码1delay(300);q=162;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(200);if(scanflag != ' '){r=r+1;}q=q-1;servo_(129,65,108,q,91,70);}judgment1();}}while(Flag_1 == 6) //上料第四次停止
{if(e>=3){Flag_1=8;}r=0;motorRun(FORWARD,200);data[4] = digitalRead(7);if(data[4]){ motorRun(BACKWARD,150);delay(230); //上料第四次急停调整 motorRun(STOP,0);servo_(129,65,108,162,91,70); //上料区第四次扫码1delay(300);q=162;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(200);if(scanflag != ' '){r=r+1;}q=q-1;servo_(129,65,108,q,91,70);}judgment1();}}while(Flag_1 == 7) //上料第五次停止
{if(e>=3){Flag_1=8;}r=0;motorRun(FORWARD,200);data[5] = digitalRead(6);if(data[5]){ motorRun(BACKWARD,150); delay(160); //上料第五次急停调整 motorRun(STOP,0);servo_(129,65,108,162,91,70); //上料区第五次扫码1delay(300);q=162;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(200);if(scanflag != ' '){r=r+1;}q=q-1;servo_(129,65,108,q,91,70);}judgment1();}}while(Flag_1 == 8) //避障{ e=0;tracing2();//执行：前面4个巡线if(data[0] && data[1] && data[2] && data[3]){Flag_1=9;}}while(Flag_1 == 9){tracing2();avoidance();//避障if(w==1){Flag_1 = 10; }}while(Flag_1 == 10){data[2] = digitalRead(9);data[3] = digitalRead(8);motorRun(FORWARD,220);if(data[2] || data[3]){motorRun(TURNLEFT, 255);delay(615);initial_b();Flag_2 = 1;Flag_1 = 11;}}while(Flag_2 == 1){ tracing2();if(data[0]&&data[1]&&!data[3]){ Flag_2 = 2;}}while(Flag_2 == 2) //下料第一次停止{r=0;motorRun(BACKWARD,150);delay(308); //下料第一次急停调整motorRun(STOP,0);servo_(128,106,50,124,91,65); //下料区第一次扫码1 downq=124;while(mySerial.read() >= 0){}while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,106,50,q,91,65); }judgment3();servo_(128,97,82,132,91,65); //下料区第一次扫码2 updelay(1000);q=132;while(r==1){scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,97,82,q,91,65);}judgment4();}while( Flag_2 == 3) //下料第二次停止{ r=0;motorRun(FORWARD,200);//直行data[4] = digitalRead(7);if(data[4] ){ motorRun(BACKWARD,150);delay(160); //下料第二次急停调整 motorRun(STOP,0);servo_(128,106,50,124,91,65); //下料区第二次扫码1 downq=124;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,106,50,q,91,65); }judgment3();servo_(128,97,82,132,91,65); //下料区第二次扫码2 updelay(1000);q=132;while(r==1){scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,97,82,q,91,65); }judgment4();}}while( Flag_2 == 4) //下料第三次停止{if(e>=3){Flag_2=7;}r=0;motorRun(FORWARD,200);//直行data[4] = digitalRead(7);if(data[4] ){ motorRun(BACKWARD,150);delay(230); //下料第三次急停调整 motorRun(STOP,0);servo_(128,106,50,124,91,65); //下料区第三次扫码1 downq=124;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,106,50,q,91,65); }judgment3();servo_(128,97,82,132,91,65); //下料区第三次扫码2 updelay(1000);q=132;while(r==1){scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,97,82,q,91,65); }judgment4();}}while(Flag_2 == 5) //下料第四次停止
{if(e>=3){Flag_2=7;}r=0;motorRun(FORWARD,150);//直行data[4] = digitalRead(7);if(data[4] ){ motorRun(BACKWARD,150);delay(242); //下料第四次急停调整 motorRun(STOP,0);servo_(128,106,50,124,91,65); //下料区第四次扫码1 downq=124;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,106,50,q,91,65); }judgment3();servo_(128,97,82,132,91,65); //下料区第四次扫码2 updelay(1000);q=132;while(r==1){scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,97,82,q,91,65);}judgment4();}}while(Flag_2 == 6) //下料第五次停止
{if(e>=3){Flag_2=7;}r=0;motorRun(FORWARD,200);//直行data[5] = digitalRead(6);if(data[5] ){ motorRun(BACKWARD,150);delay(150); //下料第五次急停调整 motorRun(STOP,0); servo_(128,106,50,124,91,65); //下料区第五次扫码1 downq=124;while(r==0){ scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,106,50,q,91,65);}judgment3();servo_(128,97,82,132,91,65); //下料区第五次扫码2 updelay(1000);q=132;while(r==1){scanflag == ' ';scanflag = scan();Serial.println("scan");delay(300);if(scanflag != ' '){r=r+1;}q=q-1;servo_(128,97,82,q,91,65); }judgment4();}}while(Flag_2 == 7){ tracing();if(!data[0] && !data[1] && !data[2] && !data[3]){motorRun(FORWARD,200);delay(300);motorRun(STOP,0);delay(5000);}}
}
Arduino智能物流小车各部分功能代码详解(省工程训练能力综合竞赛)相关推荐
- Arduino智能物流小车项目(MEGA2560)
因为这是我做的第一个项目,然后平时也不太喜欢拍照.再加上有强迫症,没事喜欢清理文件相册.导致这个项目的材料照片文件十分匮乏(其实几乎都没了).这是用的一些仅存残余文件,来写本人的第一篇博客,算是留作纪 ...
- php怎么自定义设置打印区域,JavaScript_jQuery实现区域打印功能代码详解，使用CSS控制打印样式，需要设 - phpStudy...
jQuery实现区域打印功能代码详解 使用CSS控制打印样式,需要设置样式media="print",并且将页面中不需要打印的元素的样式display属性设置为none.如DEMO ...
- android收藏功能demo,Android使用Realm数据库实现App中的收藏功能(代码详解)
前 言 App数据持久化功能是每个App必不可少的功能,而Android最常用的数据持久化方式主要有以下的五种方式: 使用SharedPreferences存储数据: 文件存储数据: SQLite数据 ...
- java搜索代码_Java实现搜索功能代码详解
首先,我们要清楚搜索框中根据关键字进行条件搜索发送的是get请求,并且是向当前页面发送get请求 //示例代码 请求路径为当前页面路径 "/product" 当我们要实现多条件搜索 ...
- java 搜索_Java实现搜索功能代码详解
首先,我们要清楚搜索框中根据关键字进行条件搜索发送的是Get请求,并且是向当前页面发送Get请求 //示例代码 请求路径为当前页面路径 "/product" 当我们要实现多条件搜索 ...
- 公交线路换乘代码PHP,微信开发之公交换乘功能代码详解
1)API申请 2)API调用 3)[b]效果展示[/b] 4)精准查询 --------------------------------------------------------------- ...
- 代码详解 | 用Pytorch训练快速神经网络的9个技巧
点击上方"视学算法",选择加"星标"或"置顶" 重磅干货,第一时间送达 作者丨William Falcon 来源丨稀牛Xiniu 编辑丨极市 ...
- C语言实现通讯录代码详解（保姆级讲解）
引言 我们常说去用C语言去完成一些项目,实现一些我们想要的功能和搭建一个简单的平台或者完成一个小游戏的代码编写.我想说的是无论你想要用C语言去实现什么,首先得确定你需要实现的哪些些功能,然后通过自己对 ...
- 【CV】Pytorch一小时入门教程-代码详解
目录 一.关键部分代码分解 1.定义网络 2.损失函数(代价函数) 3.更新权值 二.训练完整的分类器 1.数据处理 2. 训练模型(代码详解) CPU训练 GPU训练 CPU版本与GPU版本代码区别 ...
最新文章
- BFS Sicily 1215: 脱离地牢
- 【过程记录】springboot整合redis/分别用redisRepository和redistemplate操作redis
- 将一个简单远程调用的方式例子改为异步调用
- ps抠头发插件_彻底告别抠图的烦恼！PS自动抠图超级插件Topaz ReMask大放送
- 在线判题系统(oj)效果分析图_在线代码编写平台开发分享
- 费纸箱手工制作机器人_环保手工：怎么把纸箱废物利用制作立体小猫(步骤图解)...
- 不能连接到 mysql_怎么无法连接到数据库？
- Putty工具包简单使用
- 阿里巴巴矢量图标库 iconfont 的使用方法
- 京东X无人超市布局瞄准加油站，下一个场景会在哪儿
- 《系统分析与设计》个人第二次作业
- 使用LibreOffice将office文档转pdf（java实现）
- 趁年轻，别把自己关在笼子里
- 使用SLT系统抽数到hana系统
- 高清HDMI高清编码器（HDMI网络传输器）使用及前景
- encapsulation dot1q vlan-id命令
- 宿主机连接oracle容器_Linux虚拟机与windows宿主机oracle的连接配置
- 知识图谱库汇总！——教育领域能够直接应用的知识图谱
- CnPlaza.com 照片打印管理 【使用说明-打印机设置】 即影即有
- HANDSFREE小车搭载VX-11雷达配置方法
热门文章
- html中word wrap,HTML的断行word-wrap: break-word 和 word-break: break-all 到底有啥区别？
- 对抗生成网络学习（十四）——DRAGAN对模型倒塌问题的处理和生成图像质量评价(tensorflow实现)
- 全新交通大动脉！华为智慧铁路解决方案助力中老铁路正式开通；马瑞利任命樊坚强先生为中国区总裁 | 美通社头条...
- 边沿检测与提取，轮廓跟踪与Hough变换(转)
- TensorFlow Lite编译Android so库
- mysql 连接间歇性失联解决办法
- idea连接数据库失败的几种解决方案
- python人脸识别、语音合成、智能签到系统（2）
- 购物车实现原理（干货满满）
- 密歇根安娜堡大学的计算机科学教授,密歇根大学安娜堡分校计算机科学与工程研究生offer及申请要求...