如何实现机器人足球自动放球功能？

策略选择-这部分放在lua层-myball.lua c++层的内容 lua层 --desc: Kicker_x = function() return COurRole_x("Kicker") end Kicker_y = function() return COurRole_y("Kicker") end R_x = function() return COurRole_x("Receiver") end R_y = function() return COurRole_y("Receiver") end --读取txt文件 local function readFile(fileName) local f = assert(io.open(fileName,'r')) local content = f:read('*all') local margin = string.find(content,' ') local x = tonumber( string.sub(content,0,margin)) local y =tonumber( string.sub(content,margin,string.len(content))) f:close() return x,y end r2kdir =function () return COurRole2RoleDir("Receiver","Kicker") end Receiverdir = function () return CRole2BallDir("Receiver") end gPlayTable.CreatePlay{ firstState = "choose", --先根据距离选择不同的策略 ["choose"]={ switch=function() local posx,posy posx,posy=readFile("E:\\work\\SOM\\Team\\user_skills\\target_pos.txt") if (CBall2PointDist(posx,posy)<150) then return "GetBall1" else return "GetBall2" end end, Kicker = task.GotoPos("Kicker",Kicker_x,Kicker_y,0), Receiver=task.GotoPos("Receiver",R_x,R_y,Receiverdir), Goalie=task.GotoPos("Goalie",1000,1000,0), }, --策略1，直接去拿球，其他不动或离开。 ["GetBall1"] = { switch = function() end, Kicker = task.GotoPos("Kicker",Kicker_x,Kicker_y,1.5), Receiver=task.ReceiverTask("get_place_ball"), Goalie=task.GotoPos("Goalie",1000,1000,0), }, --策略2，一个去踢球，一个准备接球。 ["GetBall2"] = { switch = function() --判断是否踢球了，本且是否是真踢球，没有获取球速的函数，所以用距离判断是否真踢球 if CIsBallKick("Kicker") and CBall2RoleDist("Kicker")>30 then return "PlaceBall" end end, Kicker = task.KickerTask("getball_shoot"), Receiver=task.ReceiverTask("Receiver_wait"), Goalie=task.GotoPos("Goalie",1000,1000,0), }, --策略2，接到球之后，再放球。 ["PlaceBall"] = { switch = function() end, Kicker = task.GotoPos("Kicker",Kicker_x,Kicker_y,1.5), Receiver = task.ReceiverTask("get_place_ball"), Goalie=task.GotoPos("Goalie",1000,1000,0), }, name = "myBall" } c++层 #include "PlayerTask.h" #include "worldmodel.h" //相关的全局参量包 #include "maths.h" // 相关的计算函数包 #include "util.h" // 相关的有用的工具头文件 #include "constants.h" // 相关的有用常量 #include <fstream> #include <iostream> #include <windows.h> using namespace std; extern "C"_declspec(dllexport)PlayerTask player_plan(const WorldModel *model, int robot_id); namespace { //实地测试得出 float head_len = 7; double miss = 11; //越大接球条件越宽松 float getangle = PI / 20; #define fast_pass 9 } bool is_getball(const point2f &ball, const point2f &runner) { //miss参数需要在比赛时实地调试 bool get_ball = (ball - runner).length() < miss; if (get_ball) { return true; } else { return false; } } //判断朝向角度是否符合 bool is_right_angle(const point2f &passer, float passer_dir, const point2f &target) { float target_to_passer = (target - passer).angle(); //两个矢量角度之差小于某个值，判断是否可以传球 bool pass; pass = fabs(target_to_passer - passer_dir) < 0.4; return pass; } PlayerTask player_plan(const WorldModel *model, int robot_id) { PlayerTask task; const point2f &ball = model->get_ball_pos(); const point2f &ball_val = model->get_ball_vel(); const point2f &runner = model->get_our_player_pos(robot_id); const point2f &runner_val = model->get_our_player_v(robot_id); //ball_totalV球的总速度 double ball_totalV = sqrt(ball_val.x * ball_val.x + ball_val.y * ball_val.y); const point2f& placePos = model->get_place_pos(); float dir = model->get_our_player_dir(robot_id); ifstream inputx("E:\\work\\SOM\\Team\\user_skills\\target_pos.txt", ios::in); double x, y; inputx >> x >> y; //point2f pos = placePos; point2f pos(x, y);//目标放球点 //车到目标点的方向 float runner_to_pos_dir = (pos - runner).angle(); float ball_to_pos_dir = (pos - ball).angle(); float rece_to_ball = (ball - pos).angle(); //----------------------------球是否到目标点(距离小于7),并且速度小于40，没到就执行一大堆拿球和放球----------- if ((ball - pos).length() < 7 && ball_totalV < 40 ) { task.needCb = false;//关闭吸球 Sleep(50);//延迟一下，为了彻底关闭吸球，防止电机停止的比较慢 task.orientate = dir;//角度不变 //是否已经退出球的范围区，如果距离小于30，就一点一点退出，否则停止 if (runner.dist(pos) < 30){ task.target_pos = runner + Maths::vector2polar(-10, runner_to_pos_dir); } else{ task.target_pos = runner; } } else { //是否拿到球，并且朝向目标位置方向(防止运球时会掉球) if (is_getball(ball, runner) && is_right_angle(runner, dir, pos)) { //-----------------------开始运球到点-------------- //一点一点移动，move_buff来修改，先走的快后走的慢 double move_buff = 5; if ((ball - pos).length() < 15){ move_buff = 1; } else if ((ball - pos).length() < 80){ move_buff = 10; } else{ move_buff = 15; } task.orientate = ball_to_pos_dir; task.needCb = true; task.target_pos = runner + Maths::vector2polar(move_buff, ball_to_pos_dir); } //没有拿到球，或者朝向目标不对 else{ //-----------------------------控制角度部分--------------------------------- //拿到球就开始转到目标朝向 if (is_getball(ball, runner)) { //--------------根据球的位置决定转球方向---------------------- //buff控制转动快慢,buff可以改变 float buff = 0.4; if (abs(dir - runner_to_pos_dir) < PI / 6) { buff = 0.1; } if (abs(dir - runner_to_pos_dir) < PI / 7) { buff = 0.05; } //buff取负号来判断不同情况 if (dir < runner_to_pos_dir) { buff = -buff; } //向左转或右转 if (abs(dir - runner_to_pos_dir) < PI) { task.orientate = dir - buff; } else { task.orientate = dir + buff; } //--------------------------转球结束-------------------- } //没拿到球，就朝向球的方向 else { task.orientate = (ball - runner).angle(); } //task.target_pos = runner; task.needCb = true; //----------------以下开始为控制跑点位置-------------------- //拿到球并且朝向有大问题，就开始原地旋转，防止小球掉落 if (is_getball(ball, runner) && is_right_angle(runner, dir, pos) == false) { task.target_pos = runner; } else if (is_getball(ball,runner)){ //拿到球并且角度没问题，这里不处理，因为上面已经处理过了 } else { //计算出球运动方向和小车最短距离的交点。 double k = ball_val.y / ball_val.x; double target_x = (runner.x / k + runner.y + k*ball.x - ball.y) / (k + 1 / k); double target_y = ball_val.y / ball_val.x*(target_x - ball.x) + ball.y; //------------------------------根据球速来控制跑点位置---------------------------- //球速小于100时，直接去拿球 if (ball_totalV < 60) { if ((ball - runner).length()<50){ task.target_pos = runner + Maths::vector2polar((ball - runner).length()*0.6, (ball - runner).angle()); } else{ task.target_pos = ball; } } //球速大于100时，前往球的位置，加上球速*0.5在球运动方向上的偏移 else if (ball_totalV > 400) { task.target_pos.x = target_x; task.target_pos.y = target_y; } else { point2f new_target = ball + Maths::vector2polar(ball_totalV * 0.1, ball_val.angle()); task.target_pos.x = target_x; task.target_pos.y = target_y; } } } } return task; } #include "PlayerTask.h" #include "worldmodel.h" //相关的全局参量包 #include "maths.h" // 相关的计算函数包 #include "util.h" // 相关的有用的工具头文件 #include "constants.h" // 相关的有用常量 #include <fstream> #include <iostream> #include <windows.h> using namespace std; extern "C"_declspec(dllexport)PlayerTask player_plan(const WorldModel *model, int robot_id); namespace { //实地测试得出 float head_len = 7; double miss = 11; //越大接球条件越宽松 float getangle = PI / 20; #define fast_pass 9 } bool is_getball(const point2f &ball, const point2f &runner) { //miss参数需要在比赛时实地调试 bool get_ball = (ball - runner).length() < miss; if (get_ball) { return true; } else { return false; } } //判断朝向角度是否符合 bool is_right_angle(const point2f &passer, float passer_dir, const point2f &target,const double receiver_val) { float target_to_passer = (target - passer).angle(); //两个矢量角度之差小于某个值，判断是否可以传球，并且小车基本不移动，就说明准备好了。 bool pass; pass = fabs(target_to_passer - passer_dir) < 0.07 && receiver_val<20; return pass; } PlayerTask player_plan(const WorldModel *model, int robot_id) { PlayerTask task; const point2f &ball = model->get_ball_pos(); const point2f &ball_val = model->get_ball_vel(); //const point2f ball(0,200); const point2f &runner = model->get_our_player_pos(robot_id); //const point2f &receiver = model->get_our_player_pos(1); const point2f &runner_val = model->get_our_player_v(robot_id); //const point2f &receiver_val = model->get_our_player_v(1); //ball_totalV球的总速度 double ball_totalV = sqrt(ball_val.x * ball_val.x + ball_val.y * ball_val.y); const int get_buff = 100; float dir = model->get_our_player_dir(robot_id); ifstream inputx("E:\\work\\SOM\\Team\\user_skills\\target_pos.txt", ios::in); double x, y; inputx >> x >> y; //point2f pos1(x, y);(-100,0) point2f post(x, y);//目标放球点 //point2f post = model->get_place_pos(); double min = 10000; //拿到接球人的id给receiver,遍历场上所有的车，寻找 const bool* exist_id = model->get_our_exist_id(); int receiverid; for (int i = 0; i <= MAX_ROBOTS; i++){ if (exist_id[i]){ if (i != robot_id){ const point2f &temp = model->get_our_player_pos(i); if (temp.dist(post) < min){ min = temp.dist(post); receiverid = i; } } } } const point2f &receiver = model->get_our_player_pos(receiverid); const point2f &receiver_val = model->get_our_player_v(receiverid); const double receiver_tval = sqrt(pow(receiver_val.x, 2) + pow(receiver_val.y, 2)); point2f pos = receiver;//目标踢球点 //车到目标点的方向 float runner_to_pos_dir = (pos - runner).angle(); float ball_to_pos_dir = (pos - ball).angle(); //以下参数由测试得出 float rece_to_ball = (ball - pos).angle(); //是否拿到球，并且朝向目标位置，并且另一辆小车基本不动了。 if (is_getball(ball, runner) && is_right_angle(runner, dir, pos, receiver_tval)) { task.orientate = dir; //-----------------------开始踢球-------------- task.needKick = true; task.orientate = dir; task.flag = 0; //射球力度根据测试修改 double k = 0.03; if (receiver.dist(runner) > 450){ k = 0.01; } task.kickPower = k*receiver.dist(runner); } else{ //-----------------------------控制角度部分--------------------------------- if (is_getball(ball, runner)) { //--------------根据球的位置决定转球方向---------------------- //buff控制转动快慢,buff可以改变 float buff = 0.4; if (abs(dir - runner_to_pos_dir) < PI / 6) { buff = 0.1; } if (abs(dir - runner_to_pos_dir) < PI / 7) { buff = 0.05; } //buff取负号来判断不同情况 if (dir < runner_to_pos_dir) { buff = -buff; } //向左转或右转 if (abs(dir - runner_to_pos_dir) < PI) { task.orientate = dir - buff; } else { task.orientate = dir + buff; } //--------------------------转球结束-------------------- } else { task.orientate = (ball - runner).angle(); } task.needCb = true; //----------------以下开始为控制跑点位置-------------------- //拿到球并且朝向有大问题，就开始原地旋转，防止小球掉落 if (is_getball(ball, runner) && is_right_angle(runner, dir, pos, receiver_tval) == false) { task.target_pos = runner; } else if (is_getball(ball, runner)){ //拿到球并且角度没问题，这里不处理，因为上面已经处理过了 } else { //------------------------------根据球速来控制跑点位置---------------------------- //球速小于100时，直接去拿球 if (ball_totalV < 100) { if ((ball - runner).length()<50){ task.target_pos = runner + Maths::vector2polar((ball - runner).length()*0.6, (ball - runner).angle()); } else{ task.target_pos = ball; } } //球速大于100时，前往球的位置，加上球速*0.5在球运动方向上的偏移 else if (ball_totalV > 400) { task.target_pos = ball + Maths::vector2polar(ball_totalV * 2, ball_val.angle()); } else { task.target_pos = ball + Maths::vector2polar(ball_totalV * 0.5, ball_val.angle()); } } } return task; } Receiver_wait.cpp #include "PlayerTask.h" #include "worldmodel.h" //相关的全局参量包 #include "maths.h" // 相关的计算函数包 #include "util.h" // 相关的有用的工具头文件 #include "constants.h" // 相关的有用常量 #include <fstream> #include <iostream> #include <windows.h> using namespace std; extern "C"_declspec(dllexport)PlayerTask player_plan(const WorldModel *model, int robot_id); PlayerTask player_plan(const WorldModel *model, int robot_id) { PlayerTask task; const point2f &ball = model->get_ball_pos(); const point2f &ball_val = model->get_ball_vel(); const point2f &runner = model->get_our_player_pos(robot_id); float dir = model->get_our_player_dir(robot_id); ifstream inputx("E:\\work\\SOM\\Team\\user_skills\\target_pos.txt", ios::in); double x, y; inputx >> x >> y; point2f pos(x, y);//目标放球点 //point2f pos = model->get_place_pos(); //point2f pos1(300, -20); float ball_to_pos_dir = (pos - ball).angle(); //车在目标点的位置上，再后退一定距离。朝向小球的方向 task.target_pos = pos + Maths::vector2polar(-35, ball_to_pos_dir); task.orientate = (ball - runner).angle(); return task; }