gfc.cpp

机器人足球的一个自学的源代码

// Strategy.cpp : Defines the entry point for the DLL application.

#include "stdafx.h"
#include "Strategy.h"

#include <math.h>

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;
}
const int WHO = 0; //1 蓝方， 其他 黄方
FILE *DEBUGFILE; // 调试文件
Environment *ENV; //环境的全局变量
int BLUES = 0,YELLOWS = 0; //blue yellow sccor
bool INYELLOW = false, INBLUE = false; //球是否在门里
const double PI = 3.1415926;
double ROBOTWIDTH = 3.14; //机器人的宽度
double BALLD = 1.7; //球的直径 1.916
double HOMETRACE[5][2][2] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}; //纪录 5 个机器人的轨迹
double BALLTRACE[1][2][2] = {-1,-1,-1,-1}; //球的轨迹
double ENEMYTRACE[5][2][2] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}; //纪录 敌方5 个机器人的轨迹
double HOMEDISPLACEMENT[5] = {0}; 
double BALLDISPLACEMENT[1] = {0}; 
double ENEMYDISPLACEMENT[5] = {0}; //纪录机器人和球在 1/30 秒内的位移
int HOMEEV[5] = {0}; //estimated v 估计的机器人的速度
int BALLEV[1] = {0}; //ball's speed
int ENEMYEV[5] = {0}; //敌方机器人速度
double COUNT1 = 0, COUNT2 = 0; //保存调用次数
double HOMEVDIRECTION[5] = {0};
double BALLVDIRECTION[1] = {0};
double ENEMYVDIRECTION[5] = {0}; //球和机器人的速度方向
double PBP[2] = {0, 0}; //预测的球的坐标 predict ball position
double HOMEROTATION[5][2] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
double ENEMYROTATION[5][2] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}; //机器人的转角
int ORDER[5] = {4,3,2,1,0}; //离机器人的距离
int RORDER[5] = {0,1,2,3,4}; //各机器人对应的距离顺序
void strategyForD(Environment *env); //default
void strategyForFB(Environment *env);
void strategyForPlK(Environment *env);
void strategyForPeK(Environment *env);
void strategyForFK(Environment *env);
void strategyForGK(Environment *env);
void ballInGate();
void run(Robot *robot, int rID, int vl, int vr); //给左右轮子赋值，该函数保证数据在合法的范围
void estimateV(); //估计所有机器人的速度,and the ball
double lineAngel(double x1, double y1, double x2, double y2); //直线在 0-360 坐标系中的角度
double robotAngel(Robot *robot, int rID); // 转换机器人角度
void vDirection(); //计算球和机器人的方向
double f(double k, double x0, double y0, double x); //计算直线在 x 的值
double fn(double k,double x0,double y0,double y); //计算直线在 y 的值
double l(double x1, double y1, double x2, double y2); //计算 2 点的距离
void predictBall(double xs); //计算 xs 秒后球的位置,存入PBP[]
void rotation(); //计算所有机器人的转角
bool isStill(Robot *robot,int rID); //判断是否静止
int direction(double ra,double la); //返回机器人旋转方向,正向时
//advanced methods
bool canKShoot(Robot *robot, int rID); //是否可以把球撞进
bool canTShoot(Robot *robot,int rID); //是否可以旋转进球 可以就旋转
void turn(Robot *robot,int rID,int dire); //顺(1)/逆(-1)时针旋转
void goBackTo(Robot *robot, int rID, double x,double y); //
void goTo(Robot *robot, int rID, double x, double y); //到某点,不会停止
void sGo(Robot *robot, int rID, double x, double y); //super go 根据当前方向到某点
void to(Robot *robot, int rID, double x, double y); //
void backTo(Robot *robot, int rID, double x, double y); //到某点后静止
void sTo(Robot *robot,int rID,double x,double y); //super to 根据当前方向到某点
void rotateTo(Robot *robot, int rID, double x, double y); //指向某点，静止？？
void rotateBackTo(Robot *robot,int rID, double x,double y);
void sRotate(Robot *robot, int rID,double x, double y); //super
void sRotate(Robot *robot, int rID, double angel);
void order(); //返回机器人距离球距离的顺序，存入 ORDER[],由近到远
bool hasEnemyNear(double x, double y); //是否有敌机在 x,y 附近
bool hasEnemyBetween(double x1,double y1,double x2,double y2); //按步长执行 hasEnemyNear
bool hasEnemyIn(double x1,double y1,double x2,double y2); // 是否有敌方在矩形区域
void turnKick(Robot *robot,int rID,int where); //旋转以把球踢到 4 个象限中的一个
//role controller
void keeper(Robot *robot, int rID,double x,double y,double angel); //守卫者
void pushHelper(Robot *robot,int rID); //助推员
void goalie(Robot *robot,int rID); //守门员
void toBRight(Robot *robot,int rID); //到球右边
void toBLeft(Robot *robot,int rID); //到球左边
void toBUp(Robot *robot,int rID); //到球上边
void toBDown(Robot *robot,int rID); //到球下边
void leftSoccer(Robot *robot,int rID); //
void middleSoccer(Robot *robot,int rID); //
void rightSoccer(Robot *robot,int rID); //
void backSoccer(Robot *robot,int rID); //左中右后
void Create ( Environment *env );
void Destroy ( Environment *env );
void Strategy ( 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;
        ballInGate();
        estimateV();
        rotation();
        order();

        switch(env->gameState){
                case 0 : //s(env);break;
                case FREE_BALL : strategyForD(env); break;
                case PLACE_KICK : strategyForD(env); break;
                case PENALTY_KICK : strategyForD(env); break;
                case FREE_KICK : strategyForD(env); break;
                case GOAL_KICK : strategyForD(env); break;
        }
}

void strategyForD(Environment *env){
        //
        goalie(&env->home[0],0);
        leftSoccer(&env->home[1],1);
        rightSoccer(&env->home[4],4);
        middleSoccer(&env->home[3],3);
        backSoccer(&env->home[2],2);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%d\n",);
        //fclose(DEBUGFILE);
}

void strategyForFB(Environment *env){
        //
}

void strategyForPlK(Environment *env){
        //
}

void strategyForPeK(Environment *env){
        //
}

void strategyForFK(Environment *env){
        //
}

void strategyForGK(Environment *env){
        //
}
void ballInGate(){ //进球是犯规的怎么办 ？？
        //
        double bx,by;

        bx = ENV->currentBall.pos.x;
        by = ENV->currentBall.pos.y;
        if(by < GTOPY && by > GBOTY && bx < FLEFTX){
                if(!INYELLOW)
                        BLUES ++;
                INYELLOW = true;
        }
        else
                INYELLOW = false;
        if(by < GTOPY && by > GBOTY && bx > FRIGHTX){
                if(!INBLUE)
                        YELLOWS ++;
                INBLUE = true;
        }
        else
                INBLUE = false;
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "b:y %d:%d\n",BLUES,YELLOWS);
        //fclose(DEBUGFILE);
}

void run(Robot *robot, int rID, int vl, int vr){
        //
        if(vl > 125)
                vl = 125;
        if(vr > 125)
                vr = 125;
        if(vl < -125)
                vl = -125;
        if(vr < -125)
                vr = -125;
        robot->velocityLeft = vl;
        robot->velocityRight = vr;
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%d\n",HOMEEV[rID]);
        //fclose(DEBUGFILE);
}

void estimateV(){
        //
        double dx, dy;
        double k; //位移对于速度的系数 v= k*s
        int ym = 0;

        k = 33.45; //k 由试验测得 10/1.47符合的很好对 50 100 也
        if(HOMETRACE[0][0][0] == -1){
                for(ym = 0; ym < 5; ym++){
                        HOMETRACE[ym][0][0] = ENV->home[ym].pos.x;
                        HOMETRACE[ym][0][1] = ENV->home[ym].pos.y;
                        HOMETRACE[ym][1][0] = ENV->home[ym].pos.x;
                        HOMETRACE[ym][1][1] = ENV->home[ym].pos.y;
                }
                for(ym = 0; ym < 5; ym++){
                        ENEMYTRACE[ym][0][0] = ENV->opponent[ym].pos.x;
                        ENEMYTRACE[ym][0][1] = ENV->opponent[ym].pos.y;
                        ENEMYTRACE[ym][1][0] = ENV->opponent[ym].pos.x;
                        ENEMYTRACE[ym][1][1] = ENV->opponent[ym].pos.y;
                }
                BALLTRACE[0][0][0] = ENV->currentBall.pos.x;
                BALLTRACE[0][0][1] = ENV->currentBall.pos.y;
                BALLTRACE[0][1][0] = ENV->currentBall.pos.x;
                BALLTRACE[0][1][1] = ENV->currentBall.pos.y;
        }
        if(COUNT2 - COUNT1 >= 2){ //每调用 2 次纪录坐标
                for(ym = 0; ym < 5; ym++){
                        dx = HOMETRACE[ym][1][0] - HOMETRACE[ym][0][0];
                        dy = HOMETRACE[ym][1][1] - HOMETRACE[ym][0][1];
                        HOMEDISPLACEMENT[ym] = sqrt(dx*dx + dy*dy);
                        HOMETRACE[ym][0][0] = HOMETRACE[ym][1][0];
                        HOMETRACE[ym][0][1] = HOMETRACE[ym][1][1];
                        HOMEEV[ym] = int(HOMEDISPLACEMENT[ym]*k); //用什么模型？
                }
                for(ym = 0; ym < 5; ym++){
                        dx = ENEMYTRACE[ym][1][0] - ENEMYTRACE[ym][0][0];
                        dy = ENEMYTRACE[ym][1][1] - ENEMYTRACE[ym][0][1];
                        ENEMYDISPLACEMENT[ym] = sqrt(dx*dx + dy*dy);
                        ENEMYTRACE[ym][0][0] = ENEMYTRACE[ym][1][0];
                        ENEMYTRACE[ym][0][1] = ENEMYTRACE[ym][1][1];
                        ENEMYEV[ym] = int(ENEMYDISPLACEMENT[ym]*k); //用什么模型？
                }
                dx = BALLTRACE[0][1][0] - BALLTRACE[0][0][0];
                dy = BALLTRACE[0][1][1] - BALLTRACE[0][0][1];
                BALLDISPLACEMENT[0] = sqrt(dx*dx + dy*dy);
                BALLTRACE[0][0][0] = BALLTRACE[0][1][0];
                BALLTRACE[0][0][1] = BALLTRACE[0][1][1];
                BALLEV[0] = int(BALLDISPLACEMENT[0]*k); //用什么模型？
                
                COUNT1 = COUNT2;
        }
        else{
                for(ym = 0; ym < 5; ym++){
                        HOMETRACE[ym][1][0] = ENV->home[ym].pos.x;
                        HOMETRACE[ym][1][1] = ENV->home[ym].pos.y;
                }
                for(ym = 0; ym < 5; ym++){
                        ENEMYTRACE[ym][1][0] = ENV->opponent[ym].pos.x;
                        ENEMYTRACE[ym][1][1] = ENV->opponent[ym].pos.y;
                }
                BALLTRACE[0][1][0] = ENV->currentBall.pos.x;
                BALLTRACE[0][1][1] = ENV->currentBall.pos.y;

                vDirection();
        }
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%f %f %f\n",HOMETRACE[3][0][0],HOMETRACE[3][1][0],COUNT2);
        //fclose(DEBUGFILE);
}
double lineAngel(double x1, double y1, double x2, double y2){
        //以 x1,y1 为起点
        double length, dx, dy;
        double sinValue, alpha;

        dx = x2 - x1;
        dy = y2 - y1;
        if(fabs(dx) < 0.1 && dy > 0)
                return 90.0;
        else if(fabs(dx) < 0.1 && dy < 0)
                return 270.0;
        if(fabs(dy) < 0.1 && dx > 0)
                return 0.0;
        else if(fabs(dy) < 0.1 && dx < 0)
                return 180.0;
        if(dx < 1 || dy < 1){
                dx = dx*1000;
                dy = dy*1000;
        }
        if(dx < 0.1 || dy < 0.1){
                dx = dx*10000;
                dy = dy*10000;
        }
        else if(dx < 10 || dy < 10){
                dx = dx*100;
                dy = dy*100;
        } //放大以减小误差
        length = sqrt(dx*dx + dy*dy);
        sinValue = dy/length;
        if(sinValue >1)
                sinValue = 1;
        if(sinValue < -1)
                sinValue = -1; //防止由于计算误差导致的溢出
        alpha = asinf(sinValue)/PI * 180.0;
        if(dx > 0){
                if(alpha > 0)
                        alpha = alpha;
                else if(alpha < 0)
                        alpha = 360.0 + alpha;
        }
        else if(dx < 0){
                if(alpha > 0)
                        alpha = 180.0 - alpha;
                else if(alpha < 0)
                        alpha = 180.0 + fabs(alpha);
        }
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%f %f %f\n", dy, length, alpha);
        //fclose(DEBUGFILE);
        return alpha;
}

double robotAngel(Robot *robot, int rID){
        //
        double alpha;

        alpha = robot->rotation;
        if(alpha > 0)
                alpha = alpha;
        else if(alpha < 0)
                alpha = 360.0 + alpha;
        return alpha;
}