/*
Write by Qingtai Jiang Leftleg.write (95); Leftfoot.write (100); Rightleg.write (80); Rightfoot.write (64); Delay (s); State 1 Initial angle #define AA 88//Rudder initial angle left upper rudder #define BB 89//Left Lower rudder #define CC 88//Right rudder #define DD 88//lower Right
Steering GEAR ANGLE #define AA 88//Rudder initial angle left upper rudder #define BB 93//Left Lower rudder #define CC 88//Right Upper rudder #define DD 88//Right lower steering machine Straight//1 Oblique barrel forward modification amount//slightly left-1 straight line #define QL//zhingxing () #define QR #define QJ-2//* #inc Lude <Servo.h> #define BUSHU 50//Walk semicircle #define TIMES 2//oblique barrel Turn #define STAR 5//start and end times #define TT 1//Foot increase total #define Guotong 7///barrel after the walk of the number of steps #define Zhengliang 0//second barrel increase #define TONG2 0//second barrel turn of the increment #defin E AA 88//Rudder initial angle left upper rudder #define BB 93//Left Lower rudder #define CC 88//Right rudder #define DD 88//Right Lower rudder #define SS 50//Delay Time //1 Oblique Barrel forward modification//slightly left-1 straight line #define QL//zhingxing () #define QR #define QJ 0////2 oblique barrel forward modification amount Slightly to the right #define QQL//Zhingxingy () #define QQR #define QQJ-2///forward slightly biased to the left #define QSZL//Zsz Hixing ();
L y #define QSZR #define QSZJ 0///forward slightly biased to right #define QSYL//yszhixing () #define QSYR 26
#define QSYJ-3////=======================================================/Small left #define XZL #define XZR 22 #define XZJ 2//In situ left-turn modification #define ZL 38//left step value #define ZR 0//Right step value #define ZJ 0//J is larger. Turn left #define QZL #define QZR 5 #define QZJ 0//======================================================= =//Small Right turn #define XYL #define XYR #define XYJ-2//In-situ right to change the amount of #define YL 0 #define YR 0 #define YJ/forward Right #define QYL 5 #define QYR #define QYJ 0//=============================================================//define Ports #d
Efine Lleg 6//Rudder port #define LFOOT 5 #define RLEG #define RFOOT 11//Photoelectric Tube port//left port #define ZG1 2//Photoelectric pipe port #define ZG2 3//Left Photoelectric tube #define ZG3 12//left transverse photoelectric tube #define YG1 A2//Right Photoelectric tube #define YG2 A3 #define YG3 A1//right transverse photoelectric #define Zhong1
4//Middle Photoelectric tube servo leftleg;//define rudder 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; Log step int count1;//over bucket delay int count2; Cross-pass judgment int panduan1 (); The right tracing function int panduan2 (); Left tracing void zhixingy (); Straight-line void zhixing () of the barrel; Straight-line void zszhixing (); function void Yszhixing () with a slightly straight left line; Slightly straight to the right void Xzuozhuan (); Slightly larger than the top of void Xyouzhuan (); Small Right turn void Zuozhuan (); Turn left void Qzuozhuan () in situ; Forward left 5 void Youzhuan (); In situ right turn void Qyouzhuan ();
Forward right turn 5 void ok (); void Gzhixing (); Modified gait straight-line 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); Normal gait void gongneng2 (int a, int b, int c, int d, int s, float l, float r, Int j); Modify the gait 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),//connection 6th Port Leftleg.write (AA);//initialization angle Leftfoot.write (BB);
Rightleg.write (CC);
Rightfoot.write (DD);
Count = 0;
count1 = 0;
Delay (1500);
*////1 is black line 0 is white lines int panduan1 ()//---------------------------------------right Track {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 ();
The middle force of the Right Turn} 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 ();
The middle force of the Right Turn} 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 ();
The middle force of the Right Turn} 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 is black line 0 is white lines int panduan2 ()//====================================== left track {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)//--------------------bucket Bucket {while (1) {y1 = digit
Alread (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;
}//------------------------main function---------------------------------------------------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++)//cross black line {zhixing (); } OK (); control function}//================================================================================= void ok () {int qwer = 1,
I
count1 = 0;
Count2 = 0;
while (1)//Right track past the beginning of the bucket {qwer = Panduan1 ();
if (Qwer = = 0) {break; } delay (50);
Turn over barrel Xzuozhuan (); for (i = 0; i <Times
i++) {Zuozhuan ();
//===================================== while (1)//oblique bucket {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))//protection mechanism {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))////When all is not detected the black line jumps out of the loop
{break;
}} break; }//===================================== qwer = 1;
Start Left trace count1 = 0;
Count2 = 0;
Count = 0;
while (1)//left patrol bypasses the second bucket {qwer = panduan2 ();
if (Qwer = = 0) {break;
} delay (50);
Xyouzhuan (); For(i = 0; I < times + tong2; i++)
{Youzhuan ();
} count2 = 0;
================================================ while (1)//oblique over bucket {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++)//over finish line {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 ()//No need to change the right straight line for the program naming error {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) {}
Start building with 50+ products and up to 12 months usage for Elastic Compute Service
1 on 1 presale consultation
24/7 Technical Support 6 Free Tickets per Quarter Faster Response
Alibaba Cloud offers highly flexible support services tailored to meet your exact needs.
A comprehensive suite of global cloud computing services to power your business
Payment Methods We Support
