2017年山东省双足机器人一等奖（四自由度）arduino源程序+比赛心得

比赛规则：

 

 

机器人照片：

 

2017年山东省机器人比赛一等奖（交叉足）

四自由度，arduino nano 最小系统板，用了5个稳压模块其中4个模块分别负责给4个舵机供电，降压模块调到5.2v（电压调高了容易烧舵机！！），另一个给arduino供电，电池采用航模锂电池。

舵机最好要选用优质舵机60块钱以上的吧，否则很容易坏。

 

比赛程序大体思路，循迹分为左侧循迹和右侧循迹，一开始右侧循迹当左下光电管遇到黑桶时继续右侧循迹前进5--7步，然后左转60度左右，直行。当上方任意光电管遇到黑线时，原地右转直到上方所有光电管检测不到黑线时跳出循环，开始左侧循迹。左侧循迹时记录向前迈出的步数，当大于50步时再让右侧的光电管开始检测黑桶（原因：右侧光电管第一次路过的黑桶时不应该拐弯），当大于50步时使右下的光电管发挥作用，检测黑桶。剩下的步骤和第一次遇见黑桶处理方法一样。

 

程序编写关键在于步态的编写。但是在下方源代码已被隐去（提示：步态控制用的常量进行控制）

代码多用#define 定义常量，便于之后的调试。

补充：

---

2018/10/6  过了一年在回看自己写的代码，表示已经看不懂了，当时对变量命令太过随意啦。。

今年看了一下比赛规则，和2017年的差不多，但是增加了一个1cmX1cm的障碍物，个人认为没什么好方法，步子迈大些跨过去？

最后补充一些小trap，光电管尽量用这种黄色的光电管 ：         

                                                     

在比赛中假如光线太强这种小型的就失灵啦

                                                      

脚底和地面的接触材料也就考究，我们那年用的鼠标垫来解决打滑问题。

基础硬件做好了，最主要的还是代码的调试，别心急慢慢来。

最后附上比赛时的视频（部分）：  https://vc.bilibili.com/video/968603

---

2019/2/20更

最近我是入坑一些深度学习知识，做了一个小demo是有关自动驾驶的项目，正好回想到了这个比赛，这个还有一种解决方案是采用强化学习的Q-Learning算法，具体的Q-Learning的算法思想就是让模型通过不断的尝试自动构建出Q-table决策分支。

个人认为可以尝试实现的解决方案（高端一些的。。）：

赛道检测方面采用计算机视觉通过CNN模型或者是普通的图形学转化得到赛道的黑色边界

行动决策上采用Q-Leraing算法，设置合适的奖励机制，把机器人仍在赛道上进行自主学习。。。嗯。。岂不美滋滋。。

附上两个有关的博客，一个是车道线检测，另一个是Q-Learning在游戏上训练自动保持车道的代码

https://blog.csdn.net/qq_39622065/article/details/84991964

https://blog.csdn.net/qq_39622065/article/details/84668436

---

附源代码：（Arduino）

代码较乱，大佬勿喷。

/*
  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 
#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

	//隐去了代码

}