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

// 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;
}
//global variables
const double PI = 3.1415926;
int WHO = 1; //1 blue 0 yellow
const double MAXL = 112; //球场对角线长
FILE *DEBUGFILE; //调试文件
Environment *ENV;
int NEEDROTATE[5] = {1, 1, 1, 1, 1}; //1 need, else not need
int PD[5] = {0}; //巡逻方向,由 点 1 到 2
double TRACE[6][2][2] = {-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 DISPLACEMENT[6] = {0}; //纪录每个机器人在 1/6 秒内的位移
int EV[6] = {0}; //estimate v 估计的机器人的速度 and the ball
double COUNT1 = 0, COUNT2 = 0; //保存调用次数
double PBP[2] = {0, 0}; //预测的球的坐标 predict ball position
int WIB = 5; //whree is ball
double ROBOTLENGTH = 3.2; //length of a robot
//basic methods
void 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 ball
void predictBall(double s); //预测球的出现
void run(Robot *robot, int rID, int vl, int vr);
//advanced methods
void passBall(Robot *robotS, Robot *robotG, int rIDS, int rIDG); //传球 robot send ,robot get
void 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); //把球打到指定地点
//stratgies
bool canKShoot(Robot *robot, int rID);//是否可以用头撞球进 can kick shoot
bool canRShoot(Robot *robot, int rID); //是否可以旋转撞球进 rotate
int whereIsBall(); //判断球在哪个区域,返回区域编号
bool hasEnemyBetween(Robot *robot, double x, double y); //is there enemy between robot and x,y
void defend(int wib); //
void attack(int wib); //
//role controller
void waiter(Robot *robot, int rID, double x, double y); //在 x,y 点等待球，一旦 r.x>x,距离小于某值时kick ball
void activeDefender(Robot *robot, int rID); //绕到球的右边 kick ball
void negativeDefender(Robot *robot, int rID, double x, double y); //首在某点并对着球，一旦距离小于某一值 kick ball，b.x > x
void 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 states
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);
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; //ball's coordinate
int lt = 0;
bx = env->currentBall.pos.x;
by = env->currentBall.pos.y;
rx = env->home[0].pos.x;
whereIsBall();
//to(&env->home[3], 3,env->currentBall.pos.x, env->currentBall.pos.y);
//negativeDefender(&env->home[2],2,0,58);
//activeDefender(&env->home[1],1);
//activeDefender(&env->home[2],2);
//activeDefender(&env->home[3],3);
//attacker(&env->home[4],4);
keeper(&env->home[0],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 < 52.92 && rx-bx>-0.5)
    go(&env->home[0], 0, bx-0.3, by);
for(lt=0;lt<5;lt++){
    if(canKShoot(&env->home[lt], lt) && env->home[lt].pos.x < 27)
     go(&env->home[lt],lt,bx,by);
}
}
void strategyForFB(Environment *env){ //对 freeball
//
}
void strategyForPlK(Environment *env){
//
}
void strategyForPeK(Environment *env){
//
double bx,by; //,rx,ry;
//rx = env->home[3].pos.x;
//ry = env->home[3].pos.y;
bx = env->currentBall.pos.x;
by = env->currentBall.pos.y;
if(env->whosBall ==1){
    //
    if(env->opponent[0].pos.y > 43.00){
     rotateTo(&env->home[3],3,6.8, 33.0);
     if(NEEDROTATE[3] != 1)
      go(&env->home[3],3,bx,by);
    }
}
else if(env->whosBall ==2)
    keeper(&env->home[0], 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(NEEDROTATE[rID] != 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(NEEDROTATE[rID] != 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 vBest[6] = {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(NEEDROTATE[rID] != 1){
    run(robot, rID, vBest[lt-1], vBest[lt-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 < 0.2)
     run(robot, rID, 0, 0);
}
}
void backTo(Robot *robot, int rID, double x, double y){
//实验后误差为    2
int ve = 30; //速度误差
double le = 3; //距离控制速度
double dx, dy, length;
double ox, oy; //x,y 关于机器人对称的点
int vk = 0;
int v[5] = {-30, -50, -70, -90, -120};
dx = robot->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), NEEDROTATE[rID] == 1)
    rotateTo(robot, rID, ox, oy);
else{
    if(length > 10)
     run(robot, rID, v[vk], v[vk]);
    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 rotation
double dy; //目的点与机器人的 y 坐标差的绝对值
double beta; //机器人轴线与线的最小夹角
int v; //速度
double vk, k2 = 87; //速度修正,k2 是二次模型的系数
int error[6] = {30, 20, 15, 13, 11 ,9}; //允许误差分为 6 个等级,选取的策略很多。。
int curError; //当前允许的误差
int ll; //记数器

vk = 1; //速度修正为 1
length = sqrt((robot->pos.x - desX) * (robot->pos.x - desX) + (robot->pos.y - desY) * (robot->pos.y - desY));
dy = desY - robot->pos.y;
if(dy < 0)
    dy = dy*-1;
sinValue = dy/length;
if(sinValue >1)
    sinValue = 1;
if(sinValue < -1)
    sinValue = -1; //防止由于计算误差导致的溢出
alpha = asinf(sinValue)/PI * 180; //asin 的返回值是弧度！！
if((desY < robot->pos.y) && (desX < robot->pos.x)) //左下
    alpha = -180 + alpha;
else if((desY < robot->pos.y) && (desX > robot->pos.x)) //右下
    alpha = -alpha;
else if((desY > robot->pos.y) && (desX < robot->pos.x)) //左上
    alpha = 180 - alpha;
else if((desY > robot->pos.y) && (desX > robot->pos.x)) //右上
    alpha = alpha;
else if((desY == robot->pos.y) && desX < robot->pos.x) //左
    alpha = 180;
else if((desY == robot->pos.y) && desX > robot->pos.x) //右
    alpha = 0; //把角转化为系统角度

if(robot->rotation < 0 )
    aRRobot = 360 + robot->rotation;
else
    aRRobot = robot->rotation;
if(alpha < 0)
    aR = 360 + alpha;
else
    aR = alpha;
//把角转化为0-360坐标系中角度
beta = fabs(aRRobot - aR); //计算夹角
if(aRRobot - aR >0 && beta < 180)
    direction = 1;
if(aRRobot - aR >0 && beta > 180)
    direction = -1;
if(aRRobot - aR <0 && beta < 180)
    direction = -1;
if(aRRobot - aR <0 && beta > 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 < 0) //速度控制
    v= 0;
if(v> 60)
    v = 60;
if(v > 20)
    v = 60;
if(direction == 1){
    run(robot, rID, int(0.3*EV[rID])+v, int(0.3*EV[rID])-v);
}
else if(direction == -1){
    run(robot, rID, int(0.3*EV[rID])-v, int(0.3*EV[rID])+v);
}
//选取误差限
for(ll = 0; ll < 6; ll++){
    if(length >= MAXL/6*(ll) && length < MAXL/6*(ll +1))
     curError = error[ll];
}
//应该根据距离的远近选取允许误差的大小,越远允许误差越小
//是先计算需要转还是先发轮速指令
if(beta > curError) //误差小于 curError 度
    NEEDROTATE[rID] =1;
else
    NEEDROTATE[rID] = 0;
//在误差限度内可以不再转
return beta;
//这个控制有待根据实验数据改进
}
void stop(Robot *robot, int rID){
//
int vReduce[6] = {-3, -10, -20, -30, -40, -50};
int lt = 0;
lt = int(EV[rID]/20);
robot->velocityLeft = vReduce[lt];
robot->velocityRight = vReduce[lt];
if(EV[rID] < 4){
    robot->velocityLeft = 0;
    robot->velocityRight = 0; // 要根据试验改
}
}
void estimateV(){
//
double dx, dy;
double k; //位移对于速度的系数 v= k*s
int ym = 0;
k = 6.8; //k 由试验测得 10/1.47符合的很好对 50 100 也
if(TRACE[0][0][0] == -1){
    for(ym = 0; ym < 5; ym++){
     TRACE[ym][0][0] = ENV->home[ym].pos.x;
     TRACE[ym][0][1] = ENV->home[ym].pos.y;
     TRACE[ym][1][0] = ENV->home[ym].pos.x;
     TRACE[ym][1][1] = ENV->home[ym].pos.y;
    }
    TRACE[5][0][0] = ENV->currentBall.pos.x;
    TRACE[5][0][1] = ENV->currentBall.pos.y;
    TRACE[5][1][0] = ENV->currentBall.pos.x;
    TRACE[5][1][1] = ENV->currentBall.pos.y;
}
if(COUNT2 - COUNT1 >= 10){ //每调用 10 次纪录坐标
    for(ym = 0; ym < 6; ym++){
     dx = TRACE[ym][1][0] - TRACE[ym][0][0];
     dy = TRACE[ym][1][1] - TRACE[ym][0][1];
     DISPLACEMENT[ym] = sqrt(dx*dx + dy*dy);
     TRACE[ym][0][0] = TRACE[ym][1][0];
     TRACE[ym][0][1] = TRACE[ym][1][1];
     EV[ym] = int(DISPLACEMENT[ym]*k); //用什么模型？
    }
    COUNT1 = COUNT2;
}
else{
    for(ym = 0; ym < 5; ym++){
     TRACE[ym][1][0] = ENV->home[ym].pos.x;
     TRACE[ym][1][1] = ENV->home[ym].pos.y;
    }
    TRACE[5][1][0] = ENV->currentBall.pos.x;
    TRACE[5][1][1] = ENV->currentBall.pos.y;
}
}
int whereIsBall(){
//
double x1, x2, y1, y2; //界限
double x, y;
int k; //区域代号基数
int re; //result
x1 = 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 <= x2 && x>= x1)
    k = 3;
else if(x < x1)
    k= 6;
if(y < y1)
    re =    3 + k;
else if(y >= y1 && y <= y2)
    re = 2 + k;
else if(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.y
double yc; //yc = f(enemyRobot.x)
bool re; //result
int 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 < 5; lt++){
    if(ENV->opponent.pos.x < x && ENV->opponent.pos.x > rx)
     re = false;
}
if(dx < 0.2){
}
else if(dy < 0.2){
}
else{
    k = dy/dx;
    yc
}
*/
return false; //should be re
}
double f(double k, double x0, double y0, double x){
//
return k*x - k*x0 + y0;