[Core] update CircleRun

Robocup-ssl-China · Apr 12, 2024 · eb8b22f · eb8b22f
1 parent bac2ea9
commit eb8b22f
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 15 deletions.
diff --git a/Core/tactics/play/TestMyRun.lua b/Core/tactics/play/TestMyRun.lua
@@ -14,7 +14,13 @@ local DIR = function()
 end
 
 return {
-    firstState = "run1",
+    firstState = "skill",
+    ["skill"] = {
+        switch = function()
+        end,
+        Leader = {CircleRun{pos=CGeoPoint(0,100), rotVel=4}},
+        match = "[L]"
+    },
     ["run1"] = {
         switch = function()
             if bufcnt(player.toTargetDist("a")<5,time) then

diff --git a/Core/tactics/skill/CircleRun.cpp b/Core/tactics/skill/CircleRun.cpp
@@ -4,11 +4,19 @@
 #include "DribbleStatus.h"
 #include <CommandFactory.h>
 #include "RobotCapability.h"
+#include "parammanager.h"
 #include "CircleRun.h"
 namespace {
-    const double MAX_ACC = 6000;//mm/s^2
-    const double MAX_ROT_ACC = 50;
+    double MAX_ACC = 6000;//mm/s^2
+    double MAX_ROT_ACC = 50;
+    bool DEBUG = false;
 }
+CCircleRun::CCircleRun(){
+    ZSS::ZParamManager::instance()->loadParam(MAX_ACC,"CircleRun/MAX_ACC",6000);
+    ZSS::ZParamManager::instance()->loadParam(MAX_ROT_ACC,"CircleRun/MAX_ROT_ACC",50);
+    ZSS::ZParamManager::instance()->loadParam(DEBUG,"CircleRun/DEBUG",false);
+}
+
 void CCircleRun::plan(const CVisionModule* pVision){
     return ;
 }
@@ -18,13 +26,11 @@ CPlayerCommand* CCircleRun::execute(const CVisionModule* pVision){
     const double targetRotVel = task().player.rotvel; // omega
 
     const PlayerVisionT& me = pVision->ourPlayer(vecNumber);
-    const CVector meVel = me.Vel().rotate(-me.Dir());
     const double meRotVel = me.RotVel();
-    const double dRotVel = std::clamp(targetRotVel - meRotVel, -MAX_ROT_ACC/PARAM::Vision::FRAME_RATE, MAX_ROT_ACC/PARAM::Vision::FRAME_RATE);
-    const double rotVel = meRotVel + dRotVel;
+    const CVector meVel = me.Vel().rotate(-(me.Dir()-me.RotVel()/PARAM::Vision::FRAME_RATE));
     const CVector me2center = center - CGeoPoint(0,0);
-    const double targetVelMod = me2center.mod() * rotVel;
-    const double targetVelDir = me2center.rotate(-std::copysign(PARAM::Math::PI/2, targetRotVel)).dir();
+    const double targetVelMod = me2center.mod() * targetRotVel;
+    const double targetVelDir = me2center.rotate(-PARAM::Math::PI/2).dir();
     const CVector targetVel = Utils::Polar2Vector(targetVelMod, targetVelDir);
 
     const CVector velDiff = targetVel - meVel;
@@ -33,14 +39,21 @@ CPlayerCommand* CCircleRun::execute(const CVisionModule* pVision){
 
     const CVector localVel = meVel + dv;
 
+    const double limitTargetRotVel = targetRotVel*localVel.mod()/targetVelMod;
+    const double dRotVel = std::clamp(limitTargetRotVel - meRotVel, -MAX_ROT_ACC/PARAM::Vision::FRAME_RATE, MAX_ROT_ACC/PARAM::Vision::FRAME_RATE);
+    const double rotVel = meRotVel + dRotVel;
+
     const bool needDribble = task().player.flag & PlayerStatus::DRIBBLING;
     auto dribblePower = DribbleStatus::Instance()->getDribbleCommand(vecNumber);
     if (needDribble) {
         dribblePower = DRIBBLE_HIGHEST;
     }
-    GDebugEngine::instance()->gui_debug_msg(me.Pos(), fmt::format("localVel: ({:.2f}, {:.2f}), rotVel: {:.2f}", localVel.x(), localVel.y(), rotVel), COLOR_GREEN);
-    GDebugEngine::instance()->gui_debug_line(me.Pos(), me.Pos() + localVel.rotate(me.Dir()), COLOR_GREEN);
-    GDebugEngine::instance()->gui_debug_line(me.Pos(), me.Pos() + me2center.rotate(me.Dir()), COLOR_GREEN);
-    GDebugEngine::instance()->gui_debug_x(me.Pos() + me2center.rotate(me.Dir()), COLOR_GREEN);
+    GDebugEngine::instance()->gui_debug_msg(me.RawPos(), fmt::format("localVel: ({:.2f}, {:.2f}), rotVel: {:.2f}", localVel.x(), localVel.y(), rotVel), COLOR_GREEN);
+    GDebugEngine::instance()->gui_debug_line(me.RawPos(), me.RawPos() + meVel.rotate(me.Dir()), COLOR_PURPLE);
+    GDebugEngine::instance()->gui_debug_line(me.RawPos(), me.RawPos() + targetVel.rotate(me.Dir()), COLOR_RED);
+    GDebugEngine::instance()->gui_debug_line(me.RawPos(), me.RawPos() + localVel.rotate(me.Dir()), COLOR_BLUE);
+    // GDebugEngine::instance()->gui_debug_line(me.RawPos(), me.RawPos() + me2center.rotate(me.Dir()), COLOR_GREEN);
+    GDebugEngine::instance()->gui_debug_x(me.RawPos() + me2center.rotate(me.Dir()), COLOR_GREEN);
+    GDebugEngine::instance()->gui_debug_msg(me.RawPos() + me2center.rotate(me.Dir()), fmt::format("v:{:.1f},tv:{:.1f}",meVel.mod(),localVel.mod()),COLOR_GREEN);
     return CmdFactory::Instance()->newCommand(CPlayerSpeedV2(vecNumber, localVel.x(), localVel.y(), rotVel, dribblePower));
 }
diff --git a/Core/tactics/skill/CircleRun.h b/Core/tactics/skill/CircleRun.h
@@ -3,7 +3,7 @@
 
 class CCircleRun : public CPlayerTask{
 public:
-    CCircleRun() = default;
+    CCircleRun();
     virtual void plan(const CVisionModule* pVision);
 	virtual CPlayerCommand* execute(const CVisionModule* pVision);
     virtual void toStream(std::ostream& os) const { os << "CircleRun"; }

diff --git a/Core/tactics/skill/CircleRun.lua b/Core/tactics/skill/CircleRun.lua
@@ -6,8 +6,8 @@ function CircleRun(task)
 	execute = function(runner)
 		task_param = TaskT:new_local()
 		task_param.executor = runner
-		task_param.player.pos = CGeoPoint(100,100)
-        task_param.player.rotvel = 4
+		task_param.player.pos = task.pos
+        task_param.player.rotvel = task.rotVel
 		return skillapi:run("CircleRun", task_param)
 	end