2017年山东省双足机器人一等奖（四*度）arduino源程序+比赛心得
程序员文章站
2022-07-14 16:50:36
...
比赛规则：
机器人照片：
2017年山东省机器人比赛一等奖（交叉足）。
四*度，arduino nano 最小系统板，用了5个稳压模块其中4个模块分别负责给4个舵机供电，降压模块调到5.2v（电压调高了容易烧舵机！！），另一个给arduino供电，电池采用航模锂电池。
舵机最好要选用优质舵机，否则很容易坏。
比赛程序大体思路，循迹分为左侧循迹和右侧循迹，一开始右侧循迹当左下光电管遇到黑桶时继续右侧循迹前进5--7步，然后左转60度左右，直行。当上方任意光电管遇到黑线时，原地右转直到上方所有光电管检测不到黑线时跳出循环，开始左侧循迹。左侧循迹时记录向前迈出的步数，当大于50步时再让右侧的光电管开始检测黑桶（原因：右侧光电管第一次路过的黑桶时不应该拐弯），当大于50步时使右下的光电管发挥作用，检测黑桶。剩下的步骤和第一次遇见黑桶处理方法一样。
程序编写关键在于步态的编写。但是在下方源代码已被隐去（提示：步态控制用的常量进行控制）
代码多用#define 定义常量，便于之后的调试。
补充：
2018/10/6 过了一年在回看自己写的代码，表示已经看不懂了，当时对变量命令太过随意啦。。
今年看了一下比赛规则，和2017年的差不多，但是增加了一个1cmX1cm的障碍物，个人认为没什么好方法，步子迈大些跨过去？
最后补充一些小trap，光电管尽量用这种黄色的光电管 ：
在比赛中假如光线太强这种小型的就失灵啦
脚底和地面的接触材料也就考究，我们那年用的鼠标垫来解决打滑问题。
基础硬件做好了，最主要的还是代码的调试，别心急慢慢来。
最后附上比赛时的视频（部分）： https://vc.bilibili.com/video/968603
附源代码：
代码较乱，大佬勿喷。
/*
write by qingtai jiang
leftleg.write(95); leftfoot.write(100); rightleg.write(80); rightfoot.write(64); delay(s); //状态1
初始角度
#define aa 88 //舵机初始角度 左上 舵机
#define bb 89 // 左下 舵机
#define cc 88 //右上 舵机
#define dd 88 // 右下 舵机
调角度
#define aa 88 //舵机初始角度 左上 舵机
#define bb 93 // 左下 舵机
#define cc 88 //右上 舵机
#define dd 88 // 右下 舵机
直
//1斜桶前进修改量 // 稍微偏左 -1直线
#define ql 26 // zhingxing()
#define qr 26
#define qj -2 //
*/
#include <Servo.h>
#define bushu 50 //走半圆的步数
#define times 2 //斜着过桶转的 次数
#define star 5 //开始 和 终点 走的次数
#define tt 1 //抬脚的 增加量 总
#define guotong 7 //过桶后走的步数
#define zhengliang 0 //第二次过桶的增加量
#define tong2 0 //第二次过桶转弯的增量
#define aa 88 //舵机初始角度 左上 舵机
#define bb 93 // 左下 舵机
#define cc 88 //右上 舵机
#define dd 88 // 右下 舵机
#define ss 50 //延迟时间
//1斜桶前进修改量 // 稍微偏左 -1直线
#define ql 26 // zhingxing()
#define qr 26
#define qj 0 //
//2斜桶前进修改量 // 稍微偏右
#define qql 26 // zhingxingy()
#define qqr 26
#define qqj -2 //
//前进 稍微稍微的 偏向左 改了对调
#define qszl 26 // zszhixing(); l 30 y 28
#define qszr 26
#define qszj 0 //
//前进 稍微稍微的 偏向右
#define qsyl 26 // yszhixing()
#define qsyr 26
#define qsyj -3 //
//=======================================================
//小左转
#define xzl 38
#define xzr 22
#define xzj 2
//原地左转修改量
#define zl 38 // 左边 迈步的数值
#define zr 0 // 右边 迈步的数值
#define zj 0 // j的值越大往左转的厉害
// 前进时的左转
#define qzl 42
#define qzr 5
#define qzj 0
//========================================================
//小右转
#define xyl 22
#define xyr 36
#define xyj -2
//原地右转修改量 同上
#define yl 0
#define yr 38
#define yj 0
//前进时的右转
#define qyl 5
#define qyr 42
#define qyj 0
//=============================================================
//定义各端口
#define lleg 6 //舵机 端口
#define lfoot 5
#define rleg 10
#define rfoot 11
//光电管 端口 // 左侧口
#define zg1 2 //光电管 端口
#define zg2 3 //左侧 光电管
#define zg3 12 //左侧横向 光电管
#define yg1 A2 //右侧 光电管
#define yg2 A3
#define yg3 A1 //右侧横向光电
#define zhong1 4 //中间光电管
Servo leftleg;//定义舵机
Servo leftfoot;
Servo rightleg;
Servo rightfoot;
int y1, y2, y3, z1, z2, z3, zhong;
int a, b, c, d, j;
int s;
float l, r;
float count; //记录步数
int count1;//过桶延迟
int count2; //过通后的 判断
int panduan1(); //右侧寻迹函数
int panduan2(); //左侧寻迹
void zhixingy(); //过桶的直行
void zhixing(); //直行
void zszhixing(); // 向左稍微直行的 函数
void yszhixing(); // 向右稍微直行
void xzuozhuan(); // 比上边的稍微大点
void xyouzhuan(); //小右转
void zuozhuan(); //原地左转
void qzuozhuan(); //前进的左转 5
void youzhuan(); //原地右转
void qyouzhuan(); //前进的右转 5
void ok();
void gzhixing(); //修改步态 直行
void gzzhixing(); //
void gxzuozhuan();
void gxyouzhuan();
void gzuozhuan();
void gyouzhuan();
void gongneng(int a, int b, int c, int d, int s, float l, float r, int j); //正常步态
void gongneng2(int a, int b, int c, int d, int s, float l, float r, int j); // 修改步态
void setup()
{
pinMode(zg1, INPUT);
pinMode(zg2, INPUT);
pinMode(zg3, INPUT);
pinMode(yg1, INPUT);
pinMode(yg2, INPUT);
pinMode(yg3, INPUT);
pinMode(zhong1, INPUT);
leftleg.attach (lleg);
leftfoot.attach (lfoot);
rightleg.attach (rleg);
rightfoot.attach (rfoot);//连接6号端口
leftleg.write (aa);//初始化角度
leftfoot.write (bb);
rightleg.write (cc);
rightfoot.write (dd);
count = 0;
count1 = 0;
delay(1500);
}
/*以上内容为初始化*/
// 1是黑线 0是白线
int panduan1() //---------------------------------------右侧循迹
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if ((y1 == 0) && (y2 == 0))
{
xyouzhuan(); //中间力度 的右转
}
if ((y2 == 0) && (y1 == 1))
{
zszhixing();
}
if (((y2 == 1) && (y1 == 1)) || (y2 == 1) || ((y2 == 1) && (y1 == 0)) || (zhong == 1))
{
qzuozhuan();
}
if (z3 == 0)
{
while (1)
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
if ((y1 == 0) && (y2 == 0))
{
xyouzhuan(); //中间力度 的右转
}
if ((y2 == 0) && (y1 == 1))
{
zszhixing();
}
if (((y2 == 1) && (y1 == 1)) || (y2 == 1) || ((y2 == 1) && (y1 == 0)))
{
qzuozhuan();
}
if (z3 == 1)
{
while (1)
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
if ((y1 == 0) && (y2 == 0))
{
xyouzhuan(); //中间力度 的右转
}
if ((y2 == 0) && (y1 == 1))
{
zszhixing();
}
if (((y2 == 1) && (y1 == 1)) || (y2 == 1) || ((y2 == 1) && (y1 == 0)))
{
qzuozhuan();
}
count1++;
if (count1 > guotong)
{
break;
}
}
break;
}
}
return 0;
}
return 1;
}
// 1是黑线 0是白线
int panduan2() //======================================左侧循迹
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if ((z1 == 0) && (z2 == 0))
{
gxzuozhuan();
}
if ((z1 == 1) && (z2 == 0))
{
gzzhixing();
}
if (((z1 == 0) && (z2 == 1)) || (z2 == 1) || ((z1 == 1) && (z2 == 1)) || (zhong == 1))
{
gqyouzhuan();
}
count++;
if ((y3 == 0) && (count > bushu)) // -------------------- 检测到桶 过桶
{
while (1)
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
if ((z1 == 0) && (z2 == 0))
{
gxzuozhuan();
}
if ((z1 == 1) && (z2 == 0))
{
gzzhixing();
}
if (((z1 == 0) && (z2 == 1)) || (z2 == 1) || ((z1 == 1) && (z2 == 1)))
{
gqyouzhuan();
}
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
if (y3 == 1)
{
while (1)
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
if ((z1 == 0) && (z2 == 0))
{
gxzuozhuan();
}
if ((z1 == 1) && (z2 == 0))
{
gzzhixing();
}
if (((z1 == 0) && (z2 == 1)) || (z2 == 1) || ((z1 == 1) && (z2 == 1)))
{
gqyouzhuan();
}
count1++;
if (count1 > (guotong + zhengliang))
{
break;
}
}
break;
}
}
return 0;
}
return 1;
}
//------------------------主函数---------------------------------------------------
void loop()
{
//gxzuozhuan();
// zhixing();
// zhixingy();
//leftleg.write(aa); leftfoot.write(bb); rightleg.write(cc); rightfoot.write(dd);
//Serial.print(z3);
int i;
for (i = 0; i < star; i++) //过黑线
{
zhixing();
}
ok(); //控制函数
}
//=================================================================================
void ok()
{
int qwer = 1, i;
count1 = 0;
count2 = 0;
while (1) //右侧循迹 过一开始的桶
{
qwer = panduan1();
if (qwer == 0)
{
break;
}
}
delay(50); //拐弯 过桶
xzuozhuan();
for (i = 0; i < times; i++)
{
zuozhuan();
}
//=====================================
while (1) //斜着过桶
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
zhixing();
count2++;
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if (((z1 == 1) || (z2 == 1)) && (count2 < 6)) //保护机制
{
while (1)
{
youzhuan();
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if ((z1 == 0) && (z2 == 0))
{
count2 = 7;
break;
}
}
}
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if (( (y2 == 1) || (z1 == 1) || (z2 == 1) || (zhong == 1) || (y1 == 1)) && (count2 > 6))
{
while (1)
{
youzhuan();
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if ((y1 == 0) && (y2 == 0) && (z1 == 0) && (z2 == 0) && (zhong == 0)) //当都检测不到黑线时 跳出循环
{
break;
}
}
break;
}
}
//=====================================
qwer = 1; //开始左侧寻迹
count1 = 0;
count2 = 0;
count = 0;
while (1) //左侧 巡线 绕过过第二个桶
{
qwer = panduan2();
if (qwer == 0)
{
break;
}
}
delay(50);
xyouzhuan();
for (i = 0; i < times + tong2; i++)
{
youzhuan();
}
count2 = 0;
//================================================
while (1) //斜着过桶
{
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
zhixingy();
count2++;
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if ((count2 < 6) && ((y1 == 1) || (y2 == 1)))
{
while (1)
{
zuozhuan();
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
if ((y1 == 0) && (y2 == 0))
{
count2 = 7;
break;
}
}
}
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if (((y1 == 1) || (y2 == 1) || (z2 == 1) || (z1 == 1) || (zhong == 1)) && (count2 > 5))
{
while (1)
{
zuozhuan();
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
zhong = digitalRead(zhong1);
if ((y1 == 0) && (y2 == 0) && (z1 == 0) && (z2 == 0) && (zhong == 0))
{
break;
}
}
break;
}
}
//======================================================
qwer = 1;
while (1)
{
qwer = panduan1();
if (qwer == 0)
{
break;
}
}
//=========================================================
qwer = 1;
count1 = 0;
count2 = 0;
count = 0;
qwer = panduan1();
qwer = panduan1();
qwer = 1;
while (1)
{
qwer = panduan1();
y1 = digitalRead(yg1);
y2 = digitalRead(yg2);
y3 = digitalRead(yg3);
z1 = digitalRead(zg1);
z2 = digitalRead(zg2);
z3 = digitalRead(zg3);
if ((z1 == 1) || (z2 == 1))
{
break;
}
}
for (i = 0; i < star + 5; i++) //过终点线
{
zhixing();
}
}
void zhixingy()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qql;
r = qqr;
s = ss;
j = qqj;
gongneng(a, b, c, d, s, l, r, j);
}
void zszhixing()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qszl;
r = qszr;
s = ss;
j = qszj;
gongneng(a, b, c, d, s, l, r, j);
}
void yszhixing()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qsyl;
r = qsyr;
s = ss;
j = qsyj;
gongneng(a, b, c, d, s, l, r, j);
}
void zhixing()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = ql;
r = qr;
s = ss;
j = qj;
gongneng(a, b, c, d, s, l, r, j);
}
void xzuozhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = xzl;
r = xzr;
s = ss;
j = xzj;
gongneng(a, b, c, d, s, l, r, j);
}
void zuozhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = zl;
r = zr;
s = ss;
j = zj;
gongneng(a, b, c, d, s, l, r, j);
}
void qzuozhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qzl;
r = qzr;
s = ss;
j = qzj;
gongneng(a, b, c, d, s, l, r, j);
}
void youzhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = yl;
r = yr;
s = ss;
j = yj;
gongneng(a, b, c, d, s, l, r, j);
}
void qyouzhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qyl;
r = qyr;
s = ss;
j = qyj;
gongneng(a, b, c, d, s, l, r, j);
}
void xyouzhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = xyl;
r = xyr;
s = ss;
j = xyj;
gongneng(a, b, c, d, s, l, r, j);
}
void gongneng(int a, int b, int c, int d, int s, float l, float r, int j)
{
leftleg.write(a + l / 2 + j); leftfoot.write(b); rightleg.write(c + r / 2); rightfoot.write(d); delay(s); //状态4
//隐去了代码
}
//=============================================================================================================================================
void gzzhixing()//不需要改了 仅为程序命名错误 改右直行
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qsyl;
r = qsyr;
s = ss;
j = qsyj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gzhixing()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = ql;
r = qr;
s = ss;
j = qj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gxzuozhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = xzl;
r = xzr;
s = ss;
j = xzj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gzuozhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = zl;
r = zr;
s = ss;
j = zj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gqzuozhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qzl;
r = qzr;
s = ss;
j = qzj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gyouzhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = yl;
r = yr;
s = ss;
j = yj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gqyouzhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = qyl;
r = qyr;
s = ss;
j = qyj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gxyouzhuan()
{
a = aa;
b = bb;
c = cc;
d = dd;
l = xyl;
r = xyr;
s = ss;
j = xyj;
gongneng2(a, b, c, d, s, l, r, j);
}
void gongneng2(int a, int b, int c, int d, int s, float l, float r, int j)
{
leftleg.write(a - l / 2 - j); leftfoot.write(b); rightleg.write(c - r / 2); rightfoot.write(d); delay(s); //状态1
//隐去了代码
}