?? football.text
字號:
我的第一個機器人足球程序(960行) (一)
//lili
//backstreetlili@163.com
// 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; //調試文件
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; //where 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 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;
} ?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -