Merge branch 'fix_touch' into dev

Robocup-ssl-China · Apr 17, 2024 · de6b432 · de6b432
2 parents 6d97105 + 8c98588
commit de6b432
Show file tree

Hide file tree

Showing 7 changed files with 253 additions and 247 deletions.
diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
diff --git a/Core/src/MotionControl/CMmotion.h b/Core/src/MotionControl/CMmotion.h
@@ -1,38 +1,29 @@
 #ifndef CM_MOTION_H
 #define CM_MOTION_H
 #include <WorldDefine.h>
-//inline bool finite(double num)
-//{
-//	return fabs(num)<9999;
-//}
-enum nonZeroMode {
-    FAST,
-    ACCURATE
-};
-
-enum planType {
-    MOVE_X,
-    MOVE_Y,
-    ROTATE
-};
 
 float motion_time_1d(float dx,float vel0,float vel1,
 					 float max_vel,float max_accel,
 					 float &t_accel,float &t_cruise,float &t_decel);
 void compute_motion_1d(double x0, double v0, double v1,
                        double a_max, double d_max, double v_max, double a_factor, double vel_factor,
-                       double &traj_accel, double &traj_time, double &traj_time_acc, double &traj_time_dec, double &traj_time_flat, planType pT, nonZeroMode mode = FAST);
+                       double &traj_accel, double &traj_time);
 void compute_motion_2d(CVector x0, CVector v0, CVector v1,
 					   double a_max, double d_max, double v_max, double a_factor,
-                       CVector &traj_accel, double &time, double &time_acc, double &time_dec, double &time_flat, nonZeroMode mode = FAST);
+                       CVector &traj_accel, double &time);
 double compute_stop(double v, double max_a);
 void goto_point_omni( const PlayerPoseT& start,
 					 const PlayerPoseT& final,
 					 const PlayerCapabilityT& capability,
 					 const double& accel_factor,
 					 const double& angle_accel_factor,
-                     PlayerPoseT& nextStep,
-                     nonZeroMode mode = FAST);
+                     PlayerPoseT& nextStep);
+void __new_goto_point_omni(const PlayerPoseT& start,
+						const PlayerPoseT& final,
+						const PlayerCapabilityT& CAPABILITY,
+						const double& ACC_FACTOR,
+						const double& ANGLE_ACC_FACTOR,
+						PlayerPoseT& nextStep);
 double expectedCMPathTime(const PlayerPoseT& start, const CGeoPoint& final, double maxAccel, double maxVelocity, double accel_factor);
 double predictedTime(const PlayerVisionT& start, const CGeoPoint& Target, const CVector& targetVel = CVector(0, 0));
 double predictedTimeWithRawVel(const PlayerVisionT& start, const CGeoPoint & Target, const CVector& targetVel = CVector(0, 0));

diff --git a/Core/src/MotionControl/ControlModel.cpp b/Core/src/MotionControl/ControlModel.cpp
@@ -62,7 +62,7 @@ void CControlModel::makeZeroFinalVelocityTheta(const PlayerPoseT& start, const P
 //}
 
 /// Trapezoidal control from CMU : none-zero final velocity trajectory
-void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, nonZeroMode mode)
+void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability)
 {
     _pathList.clear();
     double accel_factor = 1.5;
@@ -75,7 +75,19 @@ void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT
     //cout << "ControlModel"<< endl;
     //cout << start.X()<<" "<<start.Y()<<" "<<start.Vel().mod()<<" "<<start.RotVel()<< endl;
     //cout << final.X() << " " << final.Y() << " " << final.Vel().mod() << endl;
-    goto_point_omni(start,final,capability,accel_factor,angle_accel_factor,_nextStep, mode);
+    goto_point_omni(start,final,capability,accel_factor,angle_accel_factor,_nextStep);
+}
+
+void CControlModel::makeNewCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability)
+{
+    _pathList.clear();
+    double accel_factor = 1.5;
+    double angle_accel_factor = 1.5;
+    if(IS_SIMULATION) {
+        accel_factor = 1.0;
+        angle_accel_factor = 1.0;
+    }
+    __new_goto_point_omni(start,final,capability,accel_factor,angle_accel_factor,_nextStep);
 }
 
 //////////////////////////////////////////////////////////////////////////

diff --git a/Core/src/MotionControl/ControlModel.h b/Core/src/MotionControl/ControlModel.h
@@ -51,7 +51,8 @@ class CControlModel {
 	//void makeProportionalGainTheta(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
 
 	/// Trapezoidal control from CMU : none-zero final velocity trajectory
-    void makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, nonZeroMode mode = FAST);
+    void makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
+    void makeNewCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
 
 	/// Get the real-time next step
 	const PlayerPoseT& getNextStep() const { return _nextStep; }

diff --git a/Core/src/Strategy/skill/GotoPosition.cpp b/Core/src/Strategy/skill/GotoPosition.cpp
@@ -120,7 +120,6 @@ CPlayerCommand* CGotoPositionV2::execute(const CVisionModule* pVision)
     CGeoPoint target = task().player.pos;							// 目标的位置
     playerFlag = task().player.flag;
     const bool needBreakRotate = (playerFlag & PlayerStatus::BREAK_THROUGH);
-    nonZeroMode mode =  FAST;
     const bool isBack = (vecNumber == TaskMediator::Instance()->leftBack()) ||
                          (vecNumber == TaskMediator::Instance()->rightBack()) ||
                         (vecNumber == TaskMediator::Instance()->singleBack()) ||
@@ -193,7 +192,7 @@ CPlayerCommand* CGotoPositionV2::execute(const CVisionModule* pVision)
     float usedtime = target.dist(self.Pos()) / capability.maxSpeed / 1.414;	// 单位：秒
 
     /// 进行轨迹生成并记录理想执行时间
-    control.makeCmTrajectory(self, final, capability, mode);					// CMU 非零速到点
+    control.makeCmTrajectory(self, final, capability);					// CMU 非零速到点
 
     const double time_factor = 1.5;
 

diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
@@ -43,12 +43,14 @@ void CTouch::plan(const CVisionModule* pVision){
 
     // stupid version of getballPos
     CGeoPoint bestPos = BallSpeedModel::instance()->poseForTime(1.0).Pos();
-    for(double dist = 0; dist < 3000; dist += 100){
+    for(double dist = 0; dist < 3000; dist += 20){
         auto pos = ballPos + Utils::Polar2Vector(dist, ballVelDir);
         double t1 = predictedTime(me, pos);
         double t2 = BallSpeedModel::Instance()->timeForDist(dist);
-        GDebugEngine::Instance()->gui_debug_x(pos,COLOR_GREEN);
-        GDebugEngine::Instance()->gui_debug_msg(pos, fmt::format("t:{:.2f},{:.2f}", t1, t2), COLOR_GREEN);
+        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);
+        }
         if(t1 < t2 || t1 < 0.1){
             bestPos = pos;
             break;
@@ -64,7 +66,7 @@ void CTouch::plan(const CVisionModule* pVision){
     const double ballVel_ball2Target_ad = ballVelMod > 500 ? angleDiff(ballVelDir, (target - ballPos).dir()) : 180;
     const bool angleCanTouch = std::abs(ballVel_ball2Target_ad) > 100 / 180.0 * PARAM::Math::PI;
 
-    const CVector targetRunVel = canWaitForBall ? CVector(0, 0) : Utils::Polar2Vector(200, ballVelDir);
+    // const CVector targetRunVel = canWaitForBall ? CVector(0, 0) : Utils::Polar2Vector(200, ballVelDir);
     const CGeoPoint targetMousePos = canWaitForBall ? projectionMousePos : predictPos;
     const double targetRunDir = (useInter || !angleCanTouch) ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - targetMousePos).dir();
     const CGeoPoint targetRunPos = targetMousePos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER, targetRunDir + PARAM::Math::PI);
@@ -92,15 +94,14 @@ void CTouch::plan(const CVisionModule* pVision){
     TaskT newTask(task());
     newTask.player.pos = targetRunPos;
     newTask.player.angle = targetRunDir;
-    newTask.player.vel = targetRunVel;
+    // newTask.player.vel = targetRunVel;
     newTask.player.flag = taskFlag;
     newTask.ball.avoid_dist = avoid_dist;
     setSubTask("SmartGoto", newTask);
 
     if(DEBUG_SWITCH){
         auto endPos = ballPos + Utils::Polar2Vector(ballVelMod,ballVelDir);
         GDebugEngine::Instance()->gui_debug_line(ballPos,endPos,4);
-        GDebugEngine::Instance()->gui_debug_msg(targetRunPos, fmt::format("TVel:{:.0f},me2SegT:{:3.1f},b2SegT:{:3.1f}", targetRunVel.mod(), me2segTime, ball2segTime));
         GDebugEngine::Instance()->gui_debug_msg(targetRunPos+CVector(0,120), fmt::format("modeDif:{:.1f}", ballVel_ball2Target_ad / PARAM::Math::PI * 180.0));
     }
 

diff --git a/ZBin/lua_scripts/skill/GoCmuRush.lua b/ZBin/lua_scripts/skill/GoCmuRush.lua
@@ -5,7 +5,6 @@ function GoCmuRush(task)
 	local msender = task.sender or 0
 	local mrole   = task.srole or ""
 	local macc    = task.acc or 0
-	local mrec    = task.rec or 0 --mrec判断是否吸球  gty 2016-6-15
 	local mvel
 	local mspeed  = task.speed or 0
 	local mforce_maunal_set_running_param = task.force_manual or false