2017年山东省机器人比赛 双足竟步 arduino源代码（删去了关键步态程序 gongneng1 和 gongneng2）

/*

  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(); //前进的左转 5void youzhuan();  //原地右转void qyouzhuan(); //前进的右转 5void 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){  }//=============================================================================================================================================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){ }