SimuroSot 仿真代码
// 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;
}
void patrol(Robot *robot, int rID, double x1, double y1, double x2, double y2){
//现在还只能在垂直 x 轴的直线上巡逻
double ex = 1, ve = 8; //允许的距离误差
int v1 = 60, v2 = -60;
double alpha;
/*
double dx,dy;
double length1, length2; //机器人到 2 点的距离
double length; //2 点距离
*/
if(x2 != x1)
x2 = x1; //x 坐标要相等,y1 > y2
if(y1 < y2){
alpha = y1;
y1 = y2;
y2 = alpha;
}
/*
dx = x2 - x1;
dy = y2 - y1;
length = sqrt(dx*dx + dy*dy);
dx = robot->pos.x - x1;
dy = robot->pos.y - y1;
length1 = sqrt(dx*dx + dy*dy);
dx = robot->pos.x - x2;
dy = robot->pos.y - y2;
length2 = sqrt(dx*dx + dy*dy);
*/
if(fabs(robot->pos.x - x1) > ex )
to(robot, rID, x1, y1);
else if((rotateTo(robot, rID, x2, y2), NEEDROTATE[rID] == 1) && PD[rID] == 0){
alpha = rotateTo(robot, rID, x2, y2);
if(NEEDROTATE[rID] == 0)
PD[rID] = 1;
}
else {
if(robot->pos.y > y1){
PD[rID] = 1;
if(EV[rID] > ve)
stop(robot, rID);
else
run(robot, rID, v1, v1);
}
else if(robot->pos.y < y2){
PD[rID] = -1;
if(EV[rID] > ve)
stop(robot, rID);
else
run(robot, rID, v2, v2);
}
if(PD[rID] == 1){
run(robot, rID, v1, v1);
}
else if(PD[rID] == -1){
run(robot, rID, v2, v2);
}
}
}
void passBall(Robot *robotS, Robot *robotG, int rIDS,int rIDG){
//只当球在 2 机器人之间时
double dx, dy, length; //robotS 和 球的距离
double cl = 4; //could pass length
dx = robotS->pos.x - ENV->currentBall.pos.x;
dy = robotS->pos.y - ENV->currentBall.pos.y;
length = sqrt(dx*dx + dy*dy);
kickBall(robotS, rIDS, robotG->pos.x, robotG->pos.y);
}
void nearBall(Robot *robot, int rID, double x, double y){
//
double dx, dy, length; //机器人和球的距离
double dxs, dys; //球和目的点的位置差
double le = 3.5; //保持的距离
double tx, ty; //target x，y
double L = 4;
dx = robot->pos.x - ENV->currentBall.pos.x;
dy = robot->pos.y - ENV->currentBall.pos.y;
length = sqrt(dx*dx + dy*dy);
dxs = PBP[0] - x;
dys = PBP[1] - y;
if(length > le){
if(dxs < 0)
tx = PBP[0] - L; //要根据球的直径得出
if(dxs >= 0)
tx = PBP[0] + L;
if(dys < 0)
ty = PBP[1] - L;
if(dys >= 0)
ty = PBP[1] + L;
to(robot, rID, tx, ty);
}
else
run(robot, rID, EV[5], EV[5]);
}
void predictBall(double s){
//xs 秒后球的位置
double dx, dy;
double xs;
xs = s;
dx = TRACE[5][1][0] - TRACE[5][0][0];
dy = TRACE[5][1][1] - TRACE[5][0][1];
PBP[0] = ENV->currentBall.pos.x + dx*xs*6;
PBP[1] = ENV->currentBall.pos.y + dy*xs*6;
}
void kickBall(Robot *robot, int rID, double x, double y){
//
double dx, dy, k;
double tx, ty, length; //目标地址
double tdx, tdy;
double le= 8; //斜边长
predictBall(1/6); //预测 1/6 秒后球的位置
dx = PBP[0] - x;
dy = PBP[1] - y;
k = dy/dx;
tdx = sqrt(le*le/(k*k + 1));
tdy = fabs(k*tdx);
if(dx < 0)
tx = PBP[0] - tdx;
if(dx >= 0)
tx = PBP[0] + tdx;
if(dy < 0)
ty = PBP[1] - tdy;
if(dy >= 0)
ty = PBP[1] + tdy;
dx = robot->pos.x - tx;
dy = robot->pos.y - ty;
length = sqrt(dx*dx + dy*dy);
if(length > 2.5) //?
to(robot, rID, tx, ty);
else if (rotateTo(robot, rID, ENV->currentBall.pos.x, ENV->currentBall.pos.y), NEEDROTATE[rID] == 1)
rotateTo(robot, rID, x, y);
else
go(robot, rID, ENV->currentBall.pos.x, ENV->currentBall.pos.y);
}
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;
}
void defend(int wib){
//
if(wib == 1){
//
waiter(&ENV->home[4], 4,80,46);
negativeDefender(&ENV->home[2], 2, 0, 58.5);
activeDefender(&ENV->home[3], 3);
waiter(&ENV->home[1], 1, 48.73, 58.56);
}
else if(wib == 2){
//
waiter(&ENV->home[4], 4, 48.73, 58.56);
activeDefender(&ENV->home[3], 3);
waiter(&ENV->home[2], 2,76,43);
negativeDefender(&ENV->home[1], 1,83, 0);
}
else if(wib == 3){
//
waiter(&ENV->home[4], 4, 48.73, 58.56);
activeDefender(&ENV->home[3], 3);
negativeDefender(&ENV->home[1], 1, 0, 26);
waiter(&ENV->home[2], 2,79,40);
}
}
void attack(int wib){
//
if(wib == 4){
//
waiter(&ENV->home[4], 4, 60, 64);
attacker(&ENV->home[3], 3);
activeDefender(&ENV->home[2], 2);
negativeDefender(&ENV->home[1], 1, 61, 0);
}
else if(wib == 5){
//
negativeDefender(&ENV->home[1], 1, 83, 0);
negativeDefender(&ENV->home[2], 2, 70, 0);
attacker(&ENV->home[3], 3);
activeDefender(&ENV->home[4], 4);
}
else if(wib == 6){
//
waiter(&ENV->home[4], 4, 60, 21);
attacker(&ENV->home[3], 3);
activeDefender(&ENV->home[2], 2);
negativeDefender(&ENV->home[1], 1, 61, 0);
}
else if(wib == 7){
//
waiter(&ENV->home[4], 4, 27, 64);
attacker(&ENV->home[3], 3);
waiter(&ENV->home[2], 2,27,21);
negativeDefender(&ENV->home[1], 1, 61, 0);
}
else if(wib == 8){
//
waiter(&ENV->home[4], 4, 37, 52.9);
attacker(&ENV->home[3], 3);
waiter(&ENV->home[2], 2,37,33.7);
negativeDefender(&ENV->home[1], 1, 61, 0);
}
else if(wib == 9){
//
waiter(&ENV->home[4], 4, 27, 64);
attacker(&ENV->home[3], 3);
waiter(&ENV->home[2], 2,27,21);
negativeDefender(&ENV->home[1], 1, 61, 0);
}
}
void waiter(Robot *robot, int rID, double x, double y){
//在 x,y 点等待球，一旦 r.x>x， kick ball
double dx, dy, length;
double bx, by, lengthb; //ball's x,y,length between ball and robot
double l = 12; //距离小于 l 时可以 kick ball
dx = robot->pos.x - x;
dy = robot->pos.y - y;
length = sqrt(dx*dx + dy*dy);
bx = ENV->currentBall.pos.x;
by = ENV->currentBall.pos.y;
lengthb = sqrt(dx*dx + dy*dy);
if(length > l)
go(robot, rID, x, y);
else if(length < l)
rotateTo(robot, rID, ENV->currentBall.pos.x, ENV->currentBall.pos.y);
if(lengthb < 13 && robot->pos.x-bx > 0.5)
go(robot, rID, ENV->currentBall.pos.x-0.3, ENV->currentBall.pos.y);
}
void activeDefender(Robot *robot, int rID){
//绕到球的右边 kick ball
double bx, by; //ball's x ang y
double rx, ry;
double xl, yl; //跑后离球的距离
double dx;
double desx, desy;
xl = 10;
yl = 6;
bx = ENV->currentBall.pos.x;
by = ENV->currentBall.pos.y;
rx = robot->pos.x ;
ry = robot->pos.y ;
dx = rx - bx;
if(dx < 1.0){ //
if(ry < by){
desy = by - yl;
if(desy < 7)
desy = 7;
}
if(ry >= by){
desy = by + yl;
if(desy > 74)
desy = 74;
}
desx = bx + xl;
if(desx > 90)
desx = 90;
go(robot, rID, desx, desy);
}
else
go(robot, rID, bx - 0.3, by);
if(bx > 88.28 && by < 33.93 && ry > by)
go(robot, rID, bx, by - 0.5);
if(bx > 88.28 && by > 49.68 && ry < by)
go(robot, rID, bx, by+0.5);
}
void negativeDefender(Robot *robot, int rID, double x, double y){
//首在某点并对着球，一旦距离小于某一值 kick ball
double dx, dy, length;
double bx, by;
bx = ENV->currentBall.pos.x;
by = ENV->currentBall.pos.y;
dx = robot->pos.x - bx;
dy = robot->pos.y - by;
length = sqrt(dx*dx + dy*dy);
if(x ==0){
if(length < 8 && dx > 0.5)
go(robot, rID, bx, by);
else
to(robot, rID, bx, y);
}
else if(y == 0){
if(length <8 && dx > 0.5)
go(robot, rID, bx, by);
else
to(robot, rID, x, by);
}
}
void keepHelper(Robot *robot, int rID){
//协助守门员在球门的另一半守门，x 坐标小于守门员
}
void keeper(Robot *robot, int rID){ //守门员，在固定 x 坐标跟着球的 y 坐标跑
//
double X, Y; //守门员在固定的 x,y 坐标
double bx, by, rx, ry ;//ball's and robot's coordiantes
double le; //误差
int bv[6] = {3,10, 20, 30,40,50}; //速度选择空间
//int k;
predictBall(1);
le = 2.0;
X = 91.1684 + 0.5;
Y = 41.3061;
rx = robot->pos.x;
ry = robot->pos.y;
bx = ENV->currentBall.pos.x;
by = ENV->currentBall.pos.y;
if(PBP[1] > 52.9)
PBP[1] = 52.9;
if(PBP[1] < 33.9)
PBP[1] = 33.9;
if(by > 52.9)
by = 52.9;
if(by < 33.9)
by = 33.9;
if(bx < 80)
to(robot, rID, X,PBP[1]);
else
go(robot, rID, X,by);
/*
k = int((fabs(by - ry))/15*6);
if(k > 5)
k= 5;
if(fabs(rx - X) > 0.5){
if(sqrt((bx-X)*(bx-X) + (by-Y)*(by-Y))> le)
to(robot, rID, X, Y);
else
rotateTo(robot, rID,X, 6.3730);
}
else{
if(by < ry)
run(robot, rID,bv[k],bv[k]);
else
run(robot, rID,-1*bv[k],-1*bv[k]);
if(fabs(ry - by) < 0.8)
run(robot, rID, 0,0);
}
*/
}
void attacker(Robot *robot, int rID){
//绕到球的右边 kick ball
double bx, by; //球的坐标
//double desx, desy; //目的点
double rx,ry; //球的 x 坐标
/*
double gx, gy1, gy2, desy1, desy2; //球门 gx,gy1 gx,gy2 gy2>gy1,
double k1, k2;
gx = 6.81;
gy1 = 33.93;
gy2 = 49.68; //球门 2 点的坐标
*/
predictBall(1/30);
bx = ENV->currentBall.pos.x;
by = ENV->currentBall.pos.y;
rx = robot->pos.x;
ry = robot->pos.y;
if(canKShoot(robot, rID)){
go(robot, rID, PBP[0], by);
}
else{
/*
k1 = (gy1 - by)/(gx - bx);
k2 = (gy2 - by)/(gx - bx);
desy1 = f(k1 , gx, gy1, rx);
desy2 = f(k2 , gx, gy2, rx);
desx = rx;
desy = (desy1 + desy2)/2;
if(desy > 75)
desy = 75;
if(desy < 5)
desy = 5;
if(desx > 91)
desx = 91;
if(desx < 5)
desx = 5;
to(robot, rID, desx, desy);
*/
if(by < 33.9){
if(ry - by <= 0.5 && rx- bx > 1)
go(robot, rID,PBP[0],by);
else
go(robot, rID,bx+8,(by-6<7)?7:by-6);
}
else if(by > 52.9){
if(ry - by >= -0.5 && rx- bx > 1)
go(robot, rID,PBP[0],by);
else
go(robot, rID,bx +8,(by+6 > 74)?74:by+6);
}
else if(by>= 33.9 && by <= 52.9){
if(rx- bx > 1)
go(robot, rID,PBP[0],by);
else
go(robot,rID,bx +8, (ry>by)?by+ 6:by-6);
}
if(by < 10.0 || by >75&& rx -bx > 0.5 && fabs(ry - by) <3)
nearBall(robot,rID,20, 43.0);
//DEBUGFILE = fopen("debug.txt", "a");
//fprintf(DEBUGFILE, "%s\n", "attacker");
//fprintf(DEBUGFILE, "%d %f %f %f\n", WIB, by, ry - by,rx - bx);
//fclose(DEBUGFILE);
}
}
bool canKShoot(Robot *robot, int rID){
//是否可以用头撞球进 can kick shoot
double rx, ry, bx, by; //ball x,y and robot x,y
double k, y; //y 是直线与 x=6.8 的交点
double length;
rx = robot->pos.x;
ry = robot->pos.y;
bx = ENV->currentBall.pos.x;
by = ENV->currentBall.pos.y;
length = sqrt((rx - bx)*(rx - bx) + (ry - by)*(ry - by));
if(length > 4.5 && rx - bx > 0){
k = (by - ry)/(bx - rx);
y = f(k, bx, by, 6.8118);
if(y > 33.9 && y < 49.6)
return true;
else
return false;
}
else
return false;
}
/*
DEBUGFILE = fopen("debug.txt", "a");
fprintf(DEBUGFILE, "%f %f\n", , );
fclose(DEBUGFILE);
*/
网上找到的一个蓝队的机器人足球程序
