机器人擂台赛无差别组主程序.doc

#include Apps/SystemTask.hint AD0 = 0;/左前红外测距int AD1 = 0;/右前红外测距int AD2 = 0;/后红外测距int AD4 = 0;/左灰度int AD5 = 0;/左灰度int AD6 = 0;/右灰度int AD7 = 0;/后灰度/左右灰度没有使用unsigned char IO0 = 0;/前左边沿红外接近unsigned char IO1 = 0;/前右边沿红外接近unsigned char IO2 = 0;/左侧边沿红外接近unsigned char IO3 = 0;/右侧边沿红外接近unsigned char IO4 = 0;/左侧红外接近unsigned char IO5 = 0;/右侧红外接近unsigned char IO6 = 0;/前方红外接近int nLastLeft = 0;/左侧速度备份int nLastRight = 0;/右侧速度备份/float fKS = 0.8;uint8 SERVO_MAPPING3 = 1,2,3;/*运动控制*/左侧1203ID为1/右侧1203ID为2/Move(500,500)表示以500的速度前进/Move(-500,500)表示以500的速度原地左转/右转和后退 依次类推void Move(int inLeft,int inRight)MFSetServoRotaSpd(1,inLeft*0.7);MFSetServoRotaSpd(2,-inRight*0.7);MFServoAction();nLastLeft = inLeft;nLastRight = inRight;void OperateEdgeLed(unsigned char flag)if(flag=0)MFDigiOutput(11,1);elseMFDigiOutput(11,0);void OperateEnemyLed(unsigned char flag)if(flag=0)MFDigiOutput(10,1);elseMFDigiOutput(10,0);/检测灰度#define FGRAYEDGE 360 /左前方红外测距检测边沿的阈值#define LGRAYEDGE 800 /右前方红外测距检测边沿的阈值#define RGRAYEDGE 360 /左前方红外测距检测边沿的阈值#define BGRAYEDGE 230 /右前方红外测距检测边沿的阈值void CheckGray()AD4 = MFGetAD(4); /前灰度AD5 = MFGetAD(5); /左灰度AD6 = MFGetAD(6); /右灰度AD7 = MFGetAD(7); /后灰度if (AD4 FGRAYEDGE) & (AD4 (AD7-BGRAYEDGE+FGRAYEDGE-20)/机器人前方是否到达擂台边沿if (AD5 RGRAYEDGE)Move(300, -300); /右转DelayMS(300);else if (AD5 LGRAYEDGE) & (AD6 RGRAYEDGE)Move(-300, 300); /左转DelayMS(300);Move(-300, -300); /倒退DelayMS(100);else if (AD7 FGRAYEDGE) & (AD7 (AD4-FGRAYEDGE+BGRAYEDGE-20)/机器人后方是否到达擂台边沿Move(300, 300); /前进DelayMS(300);elseMove(250, 250); /前进DelayMS(100);void CheckEdge()IO0 = MFGetDigiInput(0);IO1 = MFGetDigiInput(1);if (IO0 = 0) & (IO1 = 0)/机器人前方是否到达擂台边沿Move(500, 500); /前进DelayMS(1);else if (IO0 = 1) & (IO1 = 0)/机器人左侧到达擂台边沿Move(500, 800); /左转DelayMS(1);Move(0, 0); /前进DelayMS(1);else if (IO0 = 0) & (IO1 = 1)/机器人右侧到达擂台边沿Move(800, 500); /右转DelayMS(1);Move(0, 0); /前进DelayMS(1);else if (IO0 = 1) & (IO1 = 1)/机器正前方到达擂台边沿Move(0, 0); /前进DelayMS(1);Move(-600, -600); /后退DelayMS(300);Move(300, -300); /右转DelayMS(300);/*检测边缘*/返回0：没有检测到边沿/返回 1：前方左侧检测到边沿/返回 2：前方右侧检测到边沿/返回 3：前方检测到边沿/返回 4：左后检测到边沿/返回 5：右前检测到边沿/返回 6：后方检测到边沿/返回 7: 左前检测到边沿/返回 8: 右后检测到边沿/返回 9: 左侧检测到边沿/返回 10: 右侧检测到边沿#define FEDGE 360 /前方灰度检测边沿的阈值#define LEDGE 800/左侧灰度检测边沿的阈值#define REDGE 360/右侧灰度检测边沿的阈值#define BEDGE 230/后方灰度检测边沿的阈值unsigned char Edge()IO0 = MFGetDigiInput(0);IO1 = MFGetDigiInput(1);IO2 = MFGetDigiInput(2);IO3 = MFGetDigiInput(3);IO6 = MFGetDigiInput(6);AD4 = MFGetAD(4); /前置灰度AD5 = MFGetAD(5); /左侧灰度AD6 = MFGetAD(6); /又侧灰度AD7 = MFGetAD(7); /后置灰度if(IO6=1) if(IO0=1)&(IO1=0)/左前检测到边沿if(AD5 LEDGE)return 7;/左前检测到边沿elsereturn 1; /前方左侧检测到边沿else if(IO0=0)&(IO1=1)/右前检测到边沿 if(AD6 REDGE)return 5;/右前检测到边沿elsereturn 2; /前方右侧检测到边沿else if(IO0=1)&(IO1=1)/前方检测边沿（左前右前都检测到）if(AD5 REDGE)return 7;/左前检测到边沿else if(AD5 LEDGE) & (AD6 REDGE)return 5;/右前检测到边沿elsereturn 3; /前方检测到边沿else if(IO0=0)&(IO1=0) if(AD5 LEDGE) & (AD7 BEDGE) & (IO2=1)return 9;/左侧检测到边沿else if(AD6 REDGE) & ( AD7 BEDGE) & (IO3=1)return 10;/右侧检测到边沿else if(AD7 BEDGE) & ( AD7 (AD4-FEDGE+BEDGE-20) & (IO2=0) & (IO3=0)return 6;/后方检测到边沿elsereturn 0;else if(IO6=0)return 0;elsereturn 0; /没有检测到边沿/*检测敌人*/返回0:无敌人/返回1:棋子/返回2:左前方有敌人/返回3:右前方有敌人/返回4:正前方有敌人/返回5:正左方有敌人/返回6:正右方有敌人/返回7:正后方有敌人#define LENEMY 280 /左前方红外测距检测敌人的阈值#define RENEMY 280 /右前方红外测距检测敌人的阈值#define BENEMY 280 /后方红外测距检测敌人的阈值unsigned char Enemy()IO4 = MFGetDigiInput(4);IO5 = MFGetDigiInput(5);IO6 = MFGetDigiInput(6);AD0 = MFGetAD(0);/左前红外AD1 = MFGetAD(1);/右前红外AD2 = MFGetAD(2);/后方红外if(IO6 = 0)if(AD0 LENEMY) & (AD1 LENEMY) & (AD1 RENEMY)return 2;/左前方有敌人else if(AD0 RENEMY)return 3;/右前方有敌人else if(AD0 LENEMY) & (AD1 RENEMY)return 4;/正前方有敌人else if(IO4 = 0)return 5;/正左方有敌人else if(IO5=0)return 6;/正右方有敌人else if(IO6 = 1)&(AD0 LENEMY)&(AD1 BENEMY)return 7;/正后方有敌人else if(AD2 BENEMY)return 0;/无敌人elsereturn 0;/无敌人int main(void)MFInit();MFSetPortDirect(0x00000C00);DelayMS(3000);MFInitServoMapping(&SERVO_MAPPING0,3); MFSetServoMode(1,1); MFSetServoMode(2,1); MFSetServoMode(3,0); /等待启动，左红外接近或者右红外接近触发时跳出 while(1) IO4 = MFGetDigiInput(4); IO5 = MFGetDigiInput(5); if(IO4 = 0 | IO5 = 0) break; DelayMS(10); /上擂台 MFSetServoPos(3,400,512); MFServoAction(); DelayMS(1000); Move(600,600); DelayMS(2000); Move(0,0); DelayMS(200); MFSetServoPos(3,200,512); MFServoAction(); DelayMS(2000); /11号灯表示边缘检测，检测到边沿时灯亮 /10号灯表示敌方检测，检测到敌方时灯亮 OperateEdgeLed(1); OperateEnemyLed(1); DelayMS(500); OperateEdgeLed(0); OperateEnemyLed(0); DelayMS(500); int nEdge = 0; int nEnemy = 0;while (1)nEdge = Edge();/检测边沿OperateEdgeLed(nEdge);OperateEnemyLed(0);switch(nEdge)case 0:/没有检测到边沿nEnemy = Enemy();/检测敌人OperateEnemyLed(nEnemy);switch(nEnemy)case 0:/无敌人Move(600,600);break;case 1:/棋子Move(700,700);/向前方前进CheckEdge();break;case 2:/左前方有敌人Move(900,700);/向右前方前进CheckGray();break;case 3:/右前方有敌人Move(700,900);CheckGray();break;case 4:/正前方有敌人Move(1000,1000);DelayMS(100);CheckGray();MFSetServoRotaSpd(1,1000);MFSetServoRotaSpd(2,-1000);MFSetServoPos(3,400,512);DelayMS(100);CheckGray();Move(1000,1000);MFSetServoPos(3,200,512);DelayMS(100);CheckGray();break;case 5:/正左方有敌人CheckGray();Move(-700,-300);DelayMS(700);CheckGray();/(-500,500);/DelayMS(300);/CheckGray();break;case 6:/正右方有敌人CheckGray();Move(-300,-700);DelayMS(700);CheckGray();/Move(500,-500);/DelayMS(300);/CheckGray();break;case 7:/正后方有敌人CheckGray();Move(-1000,1000);DelayMS(300);break;break;case 1:/前方左侧检测到边沿Move(-800,-800);DelayMS(200);Move(600,-600);DelayMS(700);break;case 2:/前方右侧检测到边沿Move(-800,-800);DelayMS(200);Move(-400,400);DelayMS(800);break;case 3: /前方检测到边沿Move(-1000,-1000);DelayMS(100);Move