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

/*

  write by qingtai jiangleftleg.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)
{}//=============================================================================================================================================
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)
{}

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

双足竞走机器人的意义_基于STM32双足竞步机器人的研究与设计
龙源期刊网 http://www.qikan.com.cn 基于 STM32 双足竞步机器人的研究与设计作者:卢士林李彩霞张奎庆段忠臣来源:<智能计算机与应用> 2016 年第 ...
[足式机器人]Part1 双足和四足的运动Ch04——【Legged Robots that Balance 读书笔记】
本文仅供学习使用本文结合个人理解体会作者的思路,也从个人科研角度出发,部分翻译参考由:QQ群(2群legged robot that balance)提供,感谢他们为本文英文版翻译所做的贡献(但有些 ...
双足机器人课设报告_双足竞步机器人-智能步行者设计-技术报告
第五章步态规划步态是在步行运动过程中,机器人的各个关节在时序和空间上的一种协调关系,通由各关节运动的一组时间轨迹来描述.步态规划的目标是产生期望步态,即产生在某个步行周期中的实现某种步念的各关节运 ...
diy直立双足机器人_动手制作机器人，双足移动机器人DIY
直立行走的双足机器人首先要准备的材料主要有: 电机x1.橡皮筋x3.电源开关x1.电池x1.软吸管竹签.纸皮板.冰淇淋棍若干需要用到的工具有: 裁纸刀.剪刀.圆规.502胶水和热熔胶枪 1 用裁 ...
diy直立双足机器人_速看！近期国内外机器人资讯大汇总
导语三季度即将过半,二季度机器人事件回顾,智览行业发展.二季度机器人行业呈现出了怎样的发展动态呢?从行业大角度出发,带领大家回顾过去二季度的行业精彩. 国内钛米机器人钛元助力首款人工智能5G农业机 ...
双足机器人的稳定性判据_双足机器人稳定性判据研究
2017年 5月下双足机器人稳定性判据研究刘丹丹 ,张舰行(沈阳城市学院辽宁沈阳110000) 论述 225 [摘要]双足机器人是 20世纪人类最伟大的发明之一,其具有的独特的双足运动方式 ...
双足竞走机器人的意义_双足竞走机器人设计1
双足竞走机器人设计国家级大学生创新训练项目项目编号: 201210449116 孙亚军 1 ,王志 1 ,尤在勇 2 ( 1. 滨州学院自动化系,山东滨州, 256600 : 2. 四川大学机械 ...
每公里配速9分18秒，双足机器人完成5公里慢跑
内容描述:俄勒冈州立大学的 Cassie 在 53 分钟里完成了一段五公里慢跑,刷新了双足机器人的运动记录. 近日,来自美国俄勒冈州立大学的知名机器人研究团队 Agility Robotics 打造的 ...
装上螺旋桨，加州理工让只能行走的双足机器人「上了天」，还玩起了障碍滑板、走绳索...
视学算法报道编辑:杜伟.陈萍能走能飞,这才是最酷的双足机器人吧?! 有的机器人可以行走,有的可以飞行.你见过既能行走又能飞行的双足机器人吗? 加州理工学院的研究人员就打造了这样一台机器人 LEON ...

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

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

最新文章

热门文章

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

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

最新文章

热门文章

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

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