武术机器人源代码.docx
- 文档编号:7581952
- 上传时间:2023-01-25
- 格式:DOCX
- 页数:16
- 大小:15.58KB
武术机器人源代码.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();
}
}
}
}
}
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 武术 机器人 源代码