diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp index df48df6..faf5077 100644 --- a/Core/src/MotionControl/CMmotion.cpp +++ b/Core/src/MotionControl/CMmotion.cpp @@ -394,7 +394,8 @@ void goto_point_omni( const PlayerPoseT& start, double ang_a, factor_a; double time_a, time_a_acc, time_a_dec, time_a_flat, time; double time_acc, time_dec, time_flat; - compute_motion_2d(x, v, target_vel, max_accel, max_decel, max_speed, accel_factor, a, time); + // compute_motion_2d(x, v, target_vel, max_accel, max_decel, max_speed, accel_factor, a, time); + __new_compute_motion_2d(x, v, target_vel, max_accel, max_speed, accel_factor, a, time); // a = Utils::Polar2Vector(std::min(1.0,a.mod()),a.dir()); factor_a = 1; diff --git a/Core/src/OtherLibs/cmu/obstacle.cpp b/Core/src/OtherLibs/cmu/obstacle.cpp index bf7a1aa..b03a165 100644 --- a/Core/src/OtherLibs/cmu/obstacle.cpp +++ b/Core/src/OtherLibs/cmu/obstacle.cpp @@ -35,7 +35,6 @@ namespace { const double BALL_AVOID_DIST = PARAM::Field::BALL_SIZE + 5.0f; const float DEC_MAX = 450; const float ACCURATE_AVOID = 250; - } //====================================================================// // Obstacle class implementation @@ -403,13 +402,17 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra for(int i = 0; i < PARAM::Field::MAX_PLAYER; ++i) { const PlayerVisionT& teammate = pVision->ourPlayer(i); if((i != player) && teammate.Valid()) { + auto dist = teammateAvoidDist; + PARAM::Vehicle::V2::PLAYER_SIZE; if(i == TaskMediator::Instance()->rightBack() || i == TaskMediator::Instance()->leftBack() || WorldModel::Instance()->CurrentRefereeMsg() == "GameStop" || - WorldModel::Instance()->CurrentRefereeMsg() == "OurTimeout") - add_circle(vector2f(teammate.Pos().x(), teammate.Pos().y()), vector2f(teammate.Vel().x(), teammate.Vel().y()), PARAM::Vehicle::V2::PLAYER_SIZE, 1, drawObs); - else - add_circle(vector2f(teammate.Pos().x(), teammate.Pos().y()), vector2f(teammate.Vel().x(), teammate.Vel().y()), teammateAvoidDist, 1, drawObs); + WorldModel::Instance()->CurrentRefereeMsg() == "OurTimeout"){ + dist = PARAM::Vehicle::V2::PLAYER_SIZE; + } + auto move_dist = std::min(teammate.Vel().mod() * 0.5, 1000.0); // move for 0.5s + auto move_end = teammate.Pos() + Utils::Polar2Vector(move_dist, teammate.Vel().dir()); + add_long_circle(vector2f(teammate.Pos().x(), teammate.Pos().y()), vector2f(move_end.x(),move_end.y()), vector2f(teammate.Vel().x(), teammate.Vel().y()), dist, 1, drawObs); } } } @@ -422,9 +425,9 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra if((target.dist(opp.Pos()) < PARAM::Field::MAX_PLAYER_SIZE / 2) ) { continue; } - else { - add_circle(vector2f(opp.Pos().x(), opp.Pos().y()), vector2f(opp.Vel().x(), opp.Vel().y()), oppAvoidDist, 1, drawObs); - } + auto move_dist = std::min(opp.Vel().mod() * 0.5, 1000.0); // move for 0.5s + auto move_end = opp.Pos() + Utils::Polar2Vector(move_dist, opp.Vel().dir()); + add_long_circle(vector2f(opp.Pos().x(), opp.Pos().y()), vector2f(move_end.x(),move_end.y()), vector2f(opp.Vel().x(), opp.Vel().y()), oppAvoidDist, 1, drawObs); } } } diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp index cac531b..5ef7e3b 100644 --- a/Core/tactics/skill/Touch.cpp +++ b/Core/tactics/skill/Touch.cpp @@ -42,21 +42,22 @@ void CTouch::plan(const CVisionModule* pVision){ const auto ballStopPose = BallSpeedModel::instance()->poseForTime(9999); // stupid version of getballPos - CGeoPoint bestPos = BallSpeedModel::instance()->poseForTime(1.0).Pos(); + CGeoPoint bestMousePos = BallSpeedModel::instance()->poseForTime(1.0).Pos(); for(double dist = 0; dist < 3000; dist += 20){ - auto pos = ballPos + Utils::Polar2Vector(dist, ballVelDir); - double t1 = predictedTime(me, pos); + auto _mousePos = ballPos + Utils::Polar2Vector(dist, ballVelDir); + auto _robot_pos = _mousePos+Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER, ballVelDir); + double t1 = predictedTime(me, _robot_pos); double t2 = BallSpeedModel::Instance()->timeForDist(dist); if (DEBUG_SWITCH){ - GDebugEngine::Instance()->gui_debug_x(pos,COLOR_GREEN); - GDebugEngine::Instance()->gui_debug_msg(pos, fmt::format("t:{:.1f}", t1-t2), COLOR_GREEN, 0, 30); + GDebugEngine::Instance()->gui_debug_x(_robot_pos,COLOR_GREEN, 0, 10); + GDebugEngine::Instance()->gui_debug_msg(_robot_pos+CVector(50,50), fmt::format("t:{:.1f},{:.1f}", t1, t2), COLOR_RED, 0, 10, 30); } if(t1 < t2 || t1 < 0.1){ - bestPos = pos; + bestMousePos = _mousePos; break; } } - const auto getBallPos = bestPos; // TODO : replace with GetBallPos after skillutils + const auto getBallPos = bestMousePos; // TODO : replace with GetBallPos after skillutils const CGeoSegment ballRunningSeg(ballPos, ballStopPose.Pos()); const auto me2segTime = predictedTime(me, projectionRobotPos);