武术机器人源代码.docx

1、武术机器人源代码#include Apps/SystemTask.h #include funcwalk.huint8 SERVO_MAPPING5 = 1,2,3,4,5;int main() int x = 0; int nchess = 0; int y = 0; int enemy = 0; int bian = 0; int ad4 = 0; int test = 0; int io10 = 0; int nbian = 0; int chess = 0; MFInit(); MFInitServoMapping(&SERVO_MAPPING0,5); MFSetPortDirect

2、(0x00000C00); MFSetServoMode(1,1); MFSetServoMode(2,1); MFSetServoMode(3,1); MFSetServoMode(4,1); MFSetServoMode(5,0); DelayMS(1000); while (1) io8 = MFGetDigiInput(8); io9 = MFGetDigiInput(9); if (io8=0)|(io9=0) break ; DelayMS(100); MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServ

3、oRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,80); MFServoAction(); DelayMS(7000); for (x=0;x=1) break; while (1) io0 = MFGetDigiInput(0); io1 = MFGetDigiInput(1); io2 = MFGetDigiInput(2); io3 = MFGetDigiInput(3); io4 = MFGetDigiInput(4); io6 = MFGetDigiInput(6); io7 = MFGetDigiIn

4、put(7); io8 = MFGetDigiInput(8); io9 = MFGetDigiInput(9); ad0 = MFGetAD(0); ad1 = MFGetAD(1); ad2 = MFGetAD(2); ad3 = MFGetAD(3); nbian=io0+io1+io2+io3; nchess=io6*io7*io8*io9; /nbian=1,检测到边沿 /nchess=0,检测到物体 if (nbian0) /testbian int testbian() /0 未到边沿 /1 前方到达边沿 /2 左方到达边沿 /3 右方到达边沿 /4 后方到达边沿 io0 = M

5、FGetDigiInput(0); io1= MFGetDigiInput(1); io2= MFGetDigiInput(2); io3= MFGetDigiInput(3); if(io0=1|io1=1) return 1; else if(io2=1) return 2; else if(io3=1) return 3; / else if(io4=1) / / return 4; / else return 0; return 0; bian=testbian(); switch (bian) case 1: for (y=0;y200|ad1200|nbian=1) break;

6、for (x=0;x=1|nchess=0|ad0200|ad1200) break ; MFSetServoRotaSpd(1,1023); MFSetServoRotaSpd(2,1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(7); break ; case 4: MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,

7、1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(1000); break ; default : break ; switch (bian) case 2: MFSetServoRotaSpd(1,1023); MFSetServoRotaSpd(2,1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(1

8、000); for (x=0;x=1) break ; MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(10); break ; case 3: MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,-1023); MFSetS

9、ervoRotaSpd(4,-1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(1000); for (x=0;x=1) break ; MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(10); break ; case 0: MFSetServoRotaSp

10、d(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(100); break ; else if (ad0200)|(ad1200)|(ad270)|(ad370) /testenemy int testenemy() /0 未检测到敌人 /1 前方检测到敌人 /2 左方检测到敌人 /3 右方检测到敌人 /4 后方检测到敌人 ad0 = MFGetAD(0);

11、ad1 = MFGetAD(1); ad2 = MFGetAD(2); ad3 = MFGetAD(3); if(ad0200|ad1200) return 1; else if(ad270) return 2; else if(ad370) return 3; / else if(ad40) / / return 4; / else return 0; return 0; enemy=testenemy(); switch (enemy) case 1: while (1) MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFS

12、etServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(100); io2= MFGetDigiInput(2); io3= MFGetDigiInput(3); nbian=io2+io3; ad0 = MFGetAD(0); ad1 = MFGetAD(1); if(nbian=1|ad0200|ad1200) break ; break ; case 2: for (x=0;x200)|(ad1200) break ; MFSetServoR

13、otaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(100); break ; case 3: for (x=0;x200)|(ad1200) break ; MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRot

14、aSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(100); break ; else if (nchess=0) /testchess int testchess() /0 未检测到棋子 /1 前方检测到棋子 /2 左方检测到棋子 /3 右方检测到棋子 /4 后方检测到棋子 io6= MFGetDigiInput(6); io7= MFGetDigiInput(7); io8= MFGetDigiInput(8); io9= MFGetDigiInput(9); if(io6=0|io7=0) return 1;

15、 else if(io8=0) return 2; else if(io9=0) return 3; / else if(io10=0) / / return 4; / else return 0; return 0; chess=testchess(); switch (chess) case 1: MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoActio

16、n(); DelayMS(100); break ; case 4: MFSetServoRotaSpd(1,0); MFSetServoRotaSpd(2,0); MFSetServoRotaSpd(3,0); MFSetServoRotaSpd(4,0); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(1000); break ; default : break ; switch (chess) case 2: for (x=0;x150;x+) MFSetServoRotaSpd(1,-1023); MFSetServoRotaSp

17、d(2,-1023); MFSetServoRotaSpd(3,-1023); MFSetServoRotaSpd(4,-1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(10); io6 = MFGetDigiInput(6); io7 = MFGetDigiInput(7); if (!(io6=1)&(io7=1) break ; MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRota

18、Spd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(100); break ; case 3: for (x=0;x150;x+) MFSetServoRotaSpd(1,1023); MFSetServoRotaSpd(2,1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(10); io6 = MFGetDigiInput(6); io7 = MFGetDigiInput(7); if (!(io6=1)&(io7=1) break ; MFSetServoRotaSpd(1,-1023); MFSetServoRotaSpd(2,-1023); MFSetServoRotaSpd(3,1023); MFSetServoRotaSpd(4,1023); MFSetServoPos(5,180,512); MFServoAction(); DelayMS(100); break ; else walk();