我的第一个机器人足球程序.doc

/ Strategy.cpp : Defines the entry point for the DLL application#include stdafx.h#include Strategy.h#include BOOL APIENTRY DllMain( HANDLE hModule, DWORD ul_reason_for_call, LPVOID lpReserved ) switch (ul_reason_for_call) case DLL_PROCESS_ATTACH: case DLL_THREAD_ATTACH: case DLL_THREAD_DETACH: case DLL_PROCESS_DETACH: break; return TRUE;/global variablesconst double PI = 3.1415926;int WHO = 1; /1 blue 0 yellowconst double MAXL = 112; /球场对角线长FILE *DEBUGFILE; /调试文件Environment *ENV;int NEEDROTATE5 = 1, 1, 1, 1, 1; /1 need, else not needint PD5 = 0; /巡逻方向,由 点 1 到 2double TRACE622 = -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1; /纪录 5 个机器人的轨迹double DISPLACEMENT6 = 0; /纪录每个机器人在 1/6 秒内的位移int EV6 = 0; /estimate v 估计的机器人的速度 and the balldouble COUNT1 = 0, COUNT2 = 0; /保存调用次数double PBP2 = 0, 0; /预测的球的坐标 predict ball positionint WIB = 5; /whree is balldouble ROBOTLENGTH = 3.2; /length of a robot/basic methodsvoid go(Robot *robot, int rID, const double x, const double y); /朝某点运动void backGo(Robot *robot, int rID, const double x, const double y); /double rotateTo(Robot *robot, int rID, const double desX, const double desY); /转动void stop(Robot *robot, int rID); /停止void to(Robot *robot, int rID, double x, double y); /到某点静止void backTo(Robot *robot, int rID, double x, double y); /反向到某点静止void estimateV(); /估计所有机器人的速度,and the ballvoid predictBall(double s); /预测球的出现void run(Robot *robot, int rID, int vl, int vr);/advanced methodsvoid passBall(Robot *robotS, Robot *robotG, int rIDS, int rIDG); /传球 robot send ,robot getvoid patrol(Robot *robot, int rID, double x1, double y1, double x2, double y2); /在 2 点间晃动void nearBall(Robot *robot, int rID, double x, double y); /接近球,并且与 x,y 不在球的同一边void kickBall(Robot *robot, int rID, double x, double y); /把球打到指定地点/stratgiesbool canKShoot(Robot *robot, int rID);/是否可以用头撞球进 can kick shootbool canRShoot(Robot *robot, int rID); /是否可以旋转撞球进 rotateint whereIsBall(); /判断球在哪个区域,返回区域编号bool hasEnemyBetween(Robot *robot, double x, double y); /is there enemy between robot and x,yvoid defend(int wib); /void attack(int wib); /role controllervoid waiter(Robot *robot, int rID, double x, double y); /在 x,y 点等待球，一旦 r.xx,距离小于某值时kick ballvoid activeDefender(Robot *robot, int rID); /绕到球的右边 kick ballvoid negativeDefender(Robot *robot, int rID, double x, double y); /首在某点并对着球，一旦距离小于某一值 kick ball，b.x xvoid keepHelper(Robot *robot, int rID); /协助守门员在球门的另一半守门，x 坐标小于守门员void keeper(Robot *robot, int rID); /守门员，在固定 x 坐标跟着球的 y 坐标跑void attacker(Robot *robot, int rID); /绕到球的右边 kick ball/strategy for all statesvoid strategyForD(Environment *env); /defaultvoid strategyForFB(Environment *env);void strategyForPlK(Environment *env);void strategyForPeK(Environment *env);void strategyForFK(Environment *env);void strategyForGK(Environment *env);extern C STRATEGY_API void Create ( Environment *env )/ allocate user data and assign to env-userData/ eg. env-userData = ( void * ) new MyVariables ();extern C STRATEGY_API void Destroy ( Environment *env )/ free any user data created in Create ( Environment * )/ eg. if ( env-userData != NULL ) delete ( MyVariables * ) env-userData;extern C STRATEGY_API void Strategy ( Environment *env )/here are my Strategies ,as the game state differes calls the responding method COUNT2 +;ENV = env;estimateV();switch(env-gameState) case 0 : strategyForD(env);break; case FREE_BALL : strategyForD(env); break; case PLACE_KICK : strategyForD(env); break; case PENALTY_KICK : strategyForPeK(env); break; case FREE_KICK : strategyForD(env); break; case GOAL_KICK : strategyForD(env); break;void strategyForD(Environment *env) /对默认情况/double bx,by ,rx; /balls coordinateint lt = 0;bx = env-currentBall.pos.x;by = env-currentBall.pos.y;rx = env-home0.pos.x;whereIsBall();/to(&env-home3, 3,env-currentBall.pos.x, env-currentBall.pos.y);/negativeDefender(&env-home2,2,0,58);/activeDefender(&env-home1,1);/activeDefender(&env-home2,2);/activeDefender(&env-home3,3);/attacker(&env-home4,4);keeper(&env-home0,0);switch(WIB) case 1 : defend(1); break; case 2 : defend(2); break; case 3 : defend(3); break; case 4 : attack(4); break; case 5 : attack(5); break; case 6 : attack(6); break; case 7 : attack(7); break; case 8 : attack(8); break; case 9 : attack(9); break;/以下相当于紧急模块if(bx 88.28 & by 33.93 & by -0.5) go(&env-home0, 0, bx-0.3, by);for(lt=0;lthomelt, lt) & env-homelt.pos.x homelt,lt,bx,by);void strategyForFB(Environment *env) /对 freeball/void strategyForPlK(Environment *env)/void strategyForPeK(Environment *env)/double bx,by; /,rx,ry;/rx = env-home3.pos.x;/ry = env-home3.pos.y;bx = env-currentBall.pos.x;by = env-currentBall.pos.y;if(env-whosBall =1) / if(env-opponent0.pos.y 43.00) rotateTo(&env-home3,3,6.8, 33.0); if(NEEDROTATE3 != 1) go(&env-home3,3,bx,by); else if(env-whosBall =2) keeper(&env-home0, 0);void strategyForFK(Environment *env)/void strategyForGK(Environment *env)/void go(Robot *robot, int rID, const double x, const double y)/double toX, toY;int vl,vr; /轮速toX = x;toY = y;vl = 125;vr = 125;rotateTo(robot, rID, toX, toY);if(NEEDROTATErID != 1) robot-velocityLeft = vl; robot-velocityRight = vr;void backGo(Robot *robot, int rID, const double x, const double y)/double toX, toY;int vl, vr;vl = vr = -125;toX = robot-pos.x +(robot-pos.x - x);toY = robot-pos.x +(robot-pos.y - y);rotateTo(robot, rID, toX, toY);if(NEEDROTATErID != 1) robot-velocityLeft = vl; robot-velocityRight = vr;void to(Robot *robot, int rID, double x, double y)/ 调用前要把 NEEDROTATE 置 1,给每个机器人保存一个 NEEDROTATE ?/是否先要停止机器人 double length; /机器人离目的点的距离double dx, dy;int lt;int vBest6 = 30, 50, 60, 70, 100, 125; / 最适合的速度对不同的距离double l = 3; /允许的误差double beta;dx = robot-pos.x - x;dy = robot-pos.y - y;length = sqrt(dx*dx + dy*dy);lt = int(length/MAXL*6);beta = rotateTo(robot, rID, x, y);rotateTo(robot, rID, x, y);if(NEEDROTATErID != 1) run(robot, rID, vBestlt-1, vBestlt-1); if(length l+ 4) run(robot, rID, 22, 22); if(length l+2) run(robot, rID, 16, 16); if(length l) run(robot, rID, 11, 11); if(length 2) run(robot, rID, 7, 7); if(length 1) run(robot, rID, 4, 4); if(length 0.7) run(robot, rID, 2, 2); if(length 0.4) run(robot, rID, 1, 1); if(length pos.x- x;dy = robot-pos.y - y;length = sqrt(dx*dx + dy*dy);ox = robot-pos.x + dx;oy = robot-pos.y + dy;if(length = 100) length = 99;vk = int(length/20);if(rotateTo(robot, rID, ox, oy), NEEDROTATErID = 1) rotateTo(robot, rID, ox, oy);else if(length 10) run(robot, rID, vvk, vvk); else if(length le) run(robot, rID, -15, -15); else if(length 0.1) run(robot, rID, 0, 0);double rotateTo(Robot *robot, int rID,const double desX, const double desY) /给出某个目的点后转角/整个转动过程都要考虑机器人和目的点始终是运动的double alpha; /机器人与目的点的连线与标准角度系统成的角，即从正 X 轴转到该直线成的角double length; /两点间的距离double sinValue; /alpha 的 sin 值int direction; /旋转方向1 为顺时针，-1 为逆时针double aRRobot,aR; /机器人和直线在 0-360 的角度坐标系中的角度，abuslote rotationdouble dy; /目的点与机器人的 y 坐标差的绝对值double beta; /机器人轴线与线的最小夹角int v; /速度double vk, k2 = 87; /速度修正,k2 是二次模型的系数int error6 = 30, 20, 15, 13, 11 ,9; /允许误差分为 6 个等级,选取的策略很多。int curError; /当前允许的误差int ll; /记数器vk = 1; /速度修正为 1length = sqrt(robot-pos.x - desX) * (robot-pos.x - desX) + (robot-pos.y - desY) * (robot-pos.y - desY);dy = desY - robot-pos.y;if(dy 1) sinValue = 1;if(sinValue -1) sinValue = -1; /防止由于计算误差导致的溢出alpha = asinf(sinValue)/PI * 180; /asin 的返回值是弧度！if(desY pos.y) & (desX pos.x) /左下 alpha = -180 + alpha;else if(desY pos.y) & (desX robot-pos.x) /右下 alpha = -alpha;else if(desY robot-pos.y) & (desX pos.x) /左上 alpha = 180 - alpha;else if(desY robot-pos.y) & (desX robot-pos.x) /右上 alpha = alpha;else if(desY = robot-pos.y) & desX pos.x) /左 alpha = 180;else if(desY = robot-pos.y) & desX robot-pos.x) /右 alpha = 0; /把角转化为系统角度if(robot-rotation rotation;else aRRobot = robot-rotation;if(alpha 0 & beta 0 & beta 180) direction = -1;if(aRRobot - aR 0 & beta 180) direction = -1; if(aRRobot - aR 180) direction = 1; /计算旋转方向if(beta 180) beta = 360 - beta;else beta = beta;/如何在符合运动学的情况下精确控制机器人的转角 ？v = int(beta/180*60-vk); /线性模型，应该有其他更好的模型/v = sqrt(k2* beta) - vk; /二次模型,经实验没有线性的好if(v 60) v = 60;if(v 20) v = 60;if(direction = 1) run(robot, rID, int(0.3*EVrID)+v, int(0.3*EVrID)-v);else if(direction = -1) run(robot, rID, int(0.3*EVrID)-v, int(0.3*EVrID)+v);/选取误差限for(ll = 0; ll = MAXL/6*(ll) & length curError) /误差小于 curError 度 NEEDROTATErID =1;else NEEDROTATErID = 0;/在误差限度内可以不再转return beta;/这个控制有待根据实验数据改进void stop(Robot *robot, int rID)/int vReduce6 = -3, -10, -20, -30, -40, -50;int lt = 0;lt = int(EVrID/20);robot-velocityLeft = vReducelt;robot-velocityRight = vReducelt; if(EVrID velocityLeft = 0; robot-velocityRight = 0; / 要根据试验改void estimateV()/double dx, dy;double k; /位移对于速度的系数 v= k*sint ym = 0;k = 6.8; /k 由试验测得 10/1.47符合的很好对 50 100 也if(TRACE000 = -1) for(ym = 0; ym homeym.pos.x; TRACEym01 = ENV-homeym.pos.y; TRACEym10 = ENV-homeym.pos.x; TRACEym11 = ENV-homeym.pos.y; TRACE500 = ENV-currentBall.pos.x; TRACE501 = ENV-currentBall.pos.y; TRACE510 = ENV-currentBall.pos.x; TRACE511 = ENV-currentBall.pos.y;if(COUNT2 - COUNT1 = 10) /每调用 10 次纪录坐标 for(ym = 0; ym 6; ym+) dx = TRACEym10 - TRACEym00; dy = TRACEym11 - TRACEym01; DISPLACEMENTym = sqrt(dx*dx + dy*dy); TRACEym00 = TRACEym10; TRACEym01 = TRACEym11; EVym = int(DISPLACEMENTym*k); /用什么模型？ COUNT1 = COUNT2;else for(ym = 0; ym homeym.pos.x; TRACEym11 = ENV-homeym.pos.y; TRACE510 = ENV-currentBall.pos.x; TRACE511 = ENV-currentBall.pos.y;int whereIsBall()/double x1, x2, y1, y2; /界限double x, y;int k; /区域代号基数int re; /resultx1 = 37.2016;x2 = 70.2706;y1 = 33.7197;y2 = 58.5660;x = ENV-currentBall.pos.x;y = ENV-currentBall.pos.y;if(x x2) k = 0;else if(x = x1) k = 3;else if(x x1) k= 6;if(y = y1 & y y2) re = 1 + k;WIB = re;return re;bool hasEnemyBetween(Robot *robot, double x, double y)/*double dx, dy, k;double rx, ry, temp; /robot.x, robot.ydouble yc; /yc = f(enemyRobot.x)bool re; /resultint lt;dx = robot-pos.x - x;dy = robot-pos.y - y;rx = robot-pos.x;ry = robot-pos.y;if(x rx) temp = rx; rx = x; x = temp;if(y ry) temp = ry; ry = t; y = temp;for(lt = 0; lt opponent.pos.x opponent.pos.x rx) re = false;if(dx 0.2)else if(dy 0.2)else k = dy/dx; yc */return false; /should be redouble f(double k, double x0, double y0, double x)/return k*x - k*x0 + y0;