武术机器人源代码.docx
《武术机器人源代码.docx》由会员分享，可在线阅读，更多相关《武术机器人源代码.docx（16页珍藏版）》请在冰豆网上搜索。
武术机器人源代码
#include"Apps/SystemTask.h"
#include"func\walk.h"
uint8SERVO_MAPPING[5]={1,2,3,4,5};
intmain（）
{
intx=0;
intnchess=0;
inty=0;
intenemy=0;
intbian=0;
intad[4]={0};
inttest=0;
intio[10]={0};
intnbian=0;
intchess=0;
MFInit（）;
MFInitServoMapping（&SERVO_MAPPING[0],5）;
MFSetPortDirect（0x00000C00）;
MFSetServoMode（1,1）;
MFSetServoMode（2,1）;
MFSetServoMode（3,1）;
MFSetServoMode（4,1）;
MFSetServoMode（5,0）;
DelayMS（1000）;
while
（1）
{
io[8]=MFGetDigiInput（8）;
io[9]=MFGetDigiInput（9）;
if（（io[8]==0）||（io[9]==0））
{
break;
}
DelayMS（100）;
}
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,80）;
MFServoAction（）;
DelayMS（7000）;
for（x=0;x<2000;x++）
{
MFSetServoRotaSpd（1,-1000）;
MFSetServoRotaSpd（2,-1000）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
io[0]=MFGetDigiInput（0）;
io[1]=MFGetDigiInput
（1）;
nbian=io[0]+io[1];
if（nbian>=1）
{
break;
}
}
while
（1）
{
io[0]=MFGetDigiInput（0）;
io[1]=MFGetDigiInput
（1）;
io[2]=MFGetDigiInput
（2）;
io[3]=MFGetDigiInput（3）;
io[4]=MFGetDigiInput（4）;
io[6]=MFGetDigiInput（6）;
io[7]=MFGetDigiInput（7）;
io[8]=MFGetDigiInput（8）;
io[9]=MFGetDigiInput（9）;
ad[0]=MFGetAD（0）;
ad[1]=MFGetAD
（1）;
ad[2]=MFGetAD
（2）;
ad[3]=MFGetAD（3）;
nbian=io[0]+io[1]+io[2]+io[3];
nchess=io[6]*io[7]*io[8]*io[9];
//nbian>=1,检测到边沿
//nchess=0,检测到物体
if（nbian>0）
{
//testbian
inttestbian（）
{
//0未到边沿
//1前方到达边沿
//2左方到达边沿
//3右方到达边沿
//4后方到达边沿
io[0]=MFGetDigiInput（0）;
io[1]=MFGetDigiInput
（1）;
io[2]=MFGetDigiInput
（2）;
io[3]=MFGetDigiInput（3）;
if（io[0]==1||io[1]==1）
{
return1;
}
elseif（io[2]==1）
{
return2;
}
elseif（io[3]==1）
{
return3;
}
//elseif（io[4]==1）
//{
//return4;
//}
else
{
return0;
}
return0;
}
bian=testbian（）;
switch（bian）
{
case1:
for（y=0;y<100;y++）
{
MFSetServoRotaSpd（1,1023）;
MFSetServoRotaSpd（2,1023）;
MFSetServoRotaSpd（3,-1023）;
MFSetServoRotaSpd（4,-1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
ad[0]=MFGetAD（0）;
ad[1]=MFGetAD
（1）;
io[2]=MFGetDigiInput
（2）;
io[3]=MFGetDigiInput（3）;
nbian=io[2]+io[3];
if（ad[0]>200||ad[1]>200||nbian>=1）
{
break;
}
}
for（x=0;x<100;x++）
{
io[2]=MFGetDigiInput
（2）;
io[3]=MFGetDigiInput（3）;
io[8]=MFGetDigiInput（8）;
io[9]=MFGetDigiInput（9）;
ad[0]=MFGetAD（0）;
ad[1]=MFGetAD
（1）;
nchess=io[8]*io[9];
nbian=io[2]+io[3];
if（nbian>=1||nchess==0||ad[0]>200||ad[1]>200）
{
break;
}
MFSetServoRotaSpd（1,1023）;
MFSetServoRotaSpd（2,1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（7）;
}
break;
case4:
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（1000）;
break;
default:
break;
}
switch（bian）
{
case2:
MFSetServoRotaSpd（1,1023）;
MFSetServoRotaSpd（2,1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（1000）;
for（x=0;x<100;x++）
{
io[0]=MFGetDigiInput（0）;
io[1]=MFGetDigiInput
（1）;
nbian=io[0]+io[1];
if（nbian>=1）
{
break;
}
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
}
break;
case3:
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,-1023）;
MFSetServoRotaSpd（4,-1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（1000）;
for（x=0;x<100;x++）
{
io[0]=MFGetDigiInput（0）;
io[1]=MFGetDigiInput
（1）;
nbian=io[0]+io[1];
if（nbian>=1）
{
break;
}
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
}
break;
case0:
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（100）;
break;
}
}
else
{
if（（ad[0]>200）||（ad[1]>200）||（ad[2]>70）||（ad[3]>70））
{
//testenemy
inttestenemy（）
{
//0未检测到敌人
//1前方检测到敌人
//2左方检测到敌人
//3右方检测到敌人
//4后方检测到敌人
ad[0]=MFGetAD（0）;
ad[1]=MFGetAD
（1）;
ad[2]=MFGetAD
（2）;
ad[3]=MFGetAD（3）;
if（ad[0]>200||ad[1]>200）
{
return1;
}
elseif（ad[2]>70）
{
return2;
}
elseif（ad[3]>70）
{
return3;
}
//elseif（ad[4]>0）
//{
//return4;
//}
else
{
return0;
}
return0;
}
enemy=testenemy（）;
switch（enemy）
{
case1:
while
（1）
{
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（100）;
io[2]=MFGetDigiInput
（2）;
io[3]=MFGetDigiInput（3）;
nbian=io[2]+io[3];
ad[0]=MFGetAD（0）;
ad[1]=MFGetAD
（1）;
if（nbian>=1||ad[0]<200||ad[1]<200）
{
break;
}
}
break;
case2:
for（x=0;x<150;x++）
{
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,-1023）;
MFSetServoRotaSpd（4,-1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
ad[0]=MFGetAD（0）;
ad[1]=MFGetAD
（1）;
if（（ad[0]>200）||（ad[1]>200））
{
break;
}
}
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（100）;
break;
case3:
for（x=0;x<150;x++）
{
MFSetServoRotaSpd（1,1023）;
MFSetServoRotaSpd（2,1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
ad[0]=MFGetAD（0）;
ad[1]=MFGetAD
（1）;
if（（ad[0]>200）||（ad[1]>200））
{
break;
}
}
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（100）;
break;
}
}
else
{
if（nchess==0）
{
//testchess
inttestchess（）
{
//0未检测到棋子
//1前方检测到棋子
//2左方检测到棋子
//3右方检测到棋子
//4后方检测到棋子
io[6]=MFGetDigiInput（6）;
io[7]=MFGetDigiInput（7）;
io[8]=MFGetDigiInput（8）;
io[9]=MFGetDigiInput（9）;
if（io[6]==0||io[7]==0）
{
return1;
}
elseif（io[8]==0）
{
return2;
}
elseif（io[9]==0）
{
return3;
}
//elseif（io[10]==0）
//{
//return4;
//}
else
{
return0;
}
return0;
}
chess=testchess（）;
switch（chess）
{
case1:
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（100）;
break;
case4:
MFSetServoRotaSpd（1,0）;
MFSetServoRotaSpd（2,0）;
MFSetServoRotaSpd（3,0）;
MFSetServoRotaSpd（4,0）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（1000）;
break;
default:
break;
}
switch（chess）
{
case2:
for（x=0;x<150;x++）
{
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,-1023）;
MFSetServoRotaSpd（4,-1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
io[6]=MFGetDigiInput（6）;
io[7]=MFGetDigiInput（7）;
if（!
（（io[6]==1）&&（io[7]==1）））
{
break;
}
}
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（100）;
break;
case3:
for（x=0;x<150;x++）
{
MFSetServoRotaSpd（1,1023）;
MFSetServoRotaSpd（2,1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（10）;
io[6]=MFGetDigiInput（6）;
io[7]=MFGetDigiInput（7）;
if（!
（（io[6]==1）&&（io[7]==1）））
{
break;
}
}
MFSetServoRotaSpd（1,-1023）;
MFSetServoRotaSpd（2,-1023）;
MFSetServoRotaSpd（3,1023）;
MFSetServoRotaSpd（4,1023）;
MFSetServoPos（5,180,512）;
MFServoAction（）;
DelayMS（100）;
break;
}
}
else
{
walk（）;
}
}
}
}
}
升级会员 