From 6d298c7e4e7314cc4a5589073b5eecdea618155a Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Mon, 8 Apr 2024 16:33:46 +0800
Subject: [PATCH 01/30] fix bug of missing header

---
 Core/src/Strategy/PlayerTask.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Core/src/Strategy/PlayerTask.h b/Core/src/Strategy/PlayerTask.h
index bbe84ac..c28cc5f 100644
--- a/Core/src/Strategy/PlayerTask.h
+++ b/Core/src/Strategy/PlayerTask.h
@@ -5,6 +5,7 @@
 * 这个类表示队员的各种任务，包括带球，传球，跑位，面向，拦截，卡位等
 * 这个类的执行结果是产3系列基本动作
 */
+#include <memory>
 #include <PlayerCommand.h>
 #include <misc_types.h>
 #include <string>

From c1ee379348ed74bddb4e741addf1faec1966651b Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Tue, 9 Apr 2024 16:56:02 +0800
Subject: [PATCH 02/30] [Core] add new lua version bufcnt & add subplay fw

---
 Core/src/WorldModel/WorldModel_basic.cpp |  2 +
 Core/tactics/play/TestSubScript.lua      | 58 ++++++++++++++++
 ZBin/lua_scripts/Play.lua                | 56 +++++++---------
 ZBin/lua_scripts/RoleMatch.lua           |  1 +
 ZBin/lua_scripts/SelectPlay.lua          |  4 +-
 ZBin/lua_scripts/SubPlay.lua             | 85 ++++++++++++++++++++++++
 ZBin/lua_scripts/Zeus.lua                |  7 +-
 ZBin/lua_scripts/play/Test/TestRun.lua   |  4 --
 ZBin/lua_scripts/utils/bufcnt.lua        | 74 ++++++++++++++++-----
 ZBin/lua_scripts/utils/utils.lua         | 11 +++
 10 files changed, 249 insertions(+), 53 deletions(-)
 create mode 100644 Core/tactics/play/TestSubScript.lua
 create mode 100644 ZBin/lua_scripts/SubPlay.lua

diff --git a/Core/src/WorldModel/WorldModel_basic.cpp b/Core/src/WorldModel/WorldModel_basic.cpp
index b421a33..7467186 100644
--- a/Core/src/WorldModel/WorldModel_basic.cpp
+++ b/Core/src/WorldModel/WorldModel_basic.cpp
@@ -63,6 +63,8 @@ void CWorldModel::SPlayFSMSwitchClearAll(bool clear_flag)
     // 暂时只有清理 球被提出的状态
     KickStatus::Instance()->resetKick2ForceClose(true,this->vision()->getCycle());
     BallStatus::Instance()->clearKickCmd();
+
+    // not used, replaced by new lua bufcnt
     BufferCounter::Instance()->clear();
     // TODO
     return ;
diff --git a/Core/tactics/play/TestSubScript.lua b/Core/tactics/play/TestSubScript.lua
new file mode 100644
index 0000000..a448d35
--- /dev/null
+++ b/Core/tactics/play/TestSubScript.lua
@@ -0,0 +1,58 @@
+local start_pos = CGeoPoint(0,0)
+local dist = 1000
+local rotateSpeed = 1 -- rad/s
+
+local runPos = function()
+    local angle = rotateSpeed * vision:getCycle() / param.frameRate
+    return start_pos + Utils.Polar2Vector(dist, angle)
+end
+local subScript = false
+local DSS_FLAG = bit:_or(flag.allow_dss, flag.dodge_ball)
+
+local PLAY_NAME = ""
+return {
+
+    __init__ = function(name, args)
+        print("in __init__ func : ",name, args)
+        start_pos = args.pos or CGeoPoint(0,0)
+        dist = args.dist or 1000
+        subScript = true
+        PLAY_NAME = name
+    end,
+
+    firstState = "init",
+    ["init"] = {
+        switch = function()
+            if bufcnt(true,30) then 
+                if not subScript then
+                    gSubPlay.new(PLAY_NAME .. "task1", "TestSubScript", {pos=start_pos + Utils.Polar2Vector(2*dist, math.pi/4), dist=2000})
+                    gSubPlay.new(PLAY_NAME .. "task2", "TestSubScript", {pos=start_pos + Utils.Polar2Vector(2*dist, -math.pi/4*3), dist=500})
+                    rotateSpeed = -1
+                end
+                return "run"
+            end
+        end,
+        Leader = task.stop(),
+        match = "[L]"
+    },
+    ["run"] = {
+        switch = function()
+            -- print("markdebug : ",gSubPlayFiles)
+            -- for key, value in pairs(gSubPlayFiles) do
+            --     print("printFileTable: ", key, value)
+            -- end
+        end,
+        Assister = gSubPlay.roleTask("task1","Leader"),
+        Fronter = gSubPlay.roleTask("task2","Leader"),
+        Leader = task.goCmuRush(runPos, 0, nil, DSS_FLAG),
+        match = "(LAF)"
+    },
+
+    name = "TestSubScript",
+    applicable = {
+        exp = "a",
+        a = true
+    },
+    attribute = "attack",
+    timeout = 99999
+}
\ No newline at end of file
diff --git a/ZBin/lua_scripts/Play.lua b/ZBin/lua_scripts/Play.lua
index 9d4093e..9816c0c 100644
--- a/ZBin/lua_scripts/Play.lua
+++ b/ZBin/lua_scripts/Play.lua
@@ -24,11 +24,11 @@ gIsRefPlayExit = false
 gCurrentBallStatus="None"
 gLastBallStatus=""
 
---gCurrentOurBallAction="None"
------add by zhyaic 2014.5.22-----
 gExternStopCycle = 0
 gExternExitCycle = 0
 
+gPlayFileTable = {}
+
 function gPlay.Next()
 	local index = 1
 	return function()
@@ -116,10 +116,7 @@ function DoRolePosMatch(curPlay, isPlaySwitched, isStateSwitched)
 	end
 	gActiveRole = {}
 	for rolename, itask in pairs(curPlay[gRealState]) do
-		--YuN 2016.03.30 增加函数模式下的匹配
 		if(type(itask) == "function" and rolename ~= "match" and rolename~="switch") then
-			--print("closure of task!!!!!!!!!!!!!")
-			--curPlay[gRealState][rolename]=task()
 			itask = itask()
 		end
 
@@ -133,27 +130,25 @@ function DoRolePosMatch(curPlay, isPlaySwitched, isStateSwitched)
 				end
 				curPlay[gRealState][rolename].name = "continue"
 			end
-			gRolePos[rolename] = itask[2]--curPlay[gRealState][rolename][2]()
+			gRolePos[rolename] = itask[2]
 		end
 	end
 
 	UpdateRole(curPlay[gRealState].match, isPlaySwitched, isStateSwitched)
-	-- SetRoleAndNumToCPlusPlus()
-	-- add by zhyaic for test 2013.5.24
-	-- if vision:getCurrentRefereeMsg() == "TheirIndirectKick" or
-	--    vision:getCurrentRefereeMsg() == "TheirDirectKick" or
-	--    vision:getCurrentRefereeMsg() == "GameStop" then
-	-- 	UsePenaltyCleaner(curPlay)
-	-- end
 end
 
 function SetNextPlay(name)
 	gNextPlay = name
 end
 
+function PlayFSMClearAll()
+	world:SPlayFSMSwitchClearAll(true)
+	bufcntClear()
+end
+
 function ResetPlay(name)
 	local curPlay = gPlayTable[name]
-	world:SPlayFSMSwitchClearAll(true)
+	PlayFSMClearAll()
 	--------------------------------
 	if curPlay.firstState ~= nil then
 		gCurrentState = curPlay.firstState
@@ -166,7 +161,7 @@ end
 
 function ResetPlayWithLastMatch(name)
 	local curPlay = gPlayTable[name]
-	world:SPlayFSMSwitchClearAll(true)
+	PlayFSMClearAll()
 	if curPlay.firstState ~= nil then
 		gCurrentState = curPlay.firstState
 	else
@@ -175,36 +170,35 @@ function ResetPlayWithLastMatch(name)
 	gTimeCounter = 0
 end
 
+function _RunPlaySwitch(curPlay, curState)
+	local newState
+	if curPlay.switch ~= nil then
+		newState = curPlay:switch()
+	else
+		if curState ~= "exit" and curState ~= "finish" then
+			newState = curPlay[curState]:switch()
+		end
+	end
+	return newState
+end
+
 function RunPlay(name)
 
 	if(gPlayTable[name] == nil) then
 		print("Error In RunPlay: "..name)
 	else
 		local curPlay = gPlayTable[name]
-		local curState
---		gLastState = gCurrentState
+		local curState = _RunPlaySwitch(curPlay, gCurrentState)
 		local isStateSwitched = false
-		if curPlay.switch ~= nil then
-			curState = curPlay:switch()
-			--gCurrentState = curPlay:switch()
-		else
-			if gCurrentState ~= "exit" and gCurrentState ~= "finish" then
-				curState = curPlay[gCurrentState]:switch()
-			end
-			--gCurrentState = curPlay[gCurrentState]:switch()
-		end
-
-		-- if gCurrentState == nil then
-		-- 	gCurrentState = gLastState
 		if curState ~= nil then
 			gLastState = gCurrentState
 			gCurrentState = curState
 			isStateSwitched = true
-			world:SPlayFSMSwitchClearAll(true)
+			PlayFSMClearAll()
 		end
 
 --		debugEngine:gui_debug_msg(vision:ourPlayer(gRoleNum[rolename]):Pos(), rolename)
-
+		gSubPlay.step()
 		DoRolePosMatch(curPlay, false, isStateSwitched)
 		gExceptionNum={}
 
diff --git a/ZBin/lua_scripts/RoleMatch.lua b/ZBin/lua_scripts/RoleMatch.lua
index 51a0bdd..3de08c6 100644
--- a/ZBin/lua_scripts/RoleMatch.lua
+++ b/ZBin/lua_scripts/RoleMatch.lua
@@ -40,6 +40,7 @@ gRoleNum = {
 	["z"] = -1
 }
 
+
 gLastRoleNum = {
 
 }
diff --git a/ZBin/lua_scripts/SelectPlay.lua b/ZBin/lua_scripts/SelectPlay.lua
index fcd5ee3..1545c55 100644
--- a/ZBin/lua_scripts/SelectPlay.lua
+++ b/ZBin/lua_scripts/SelectPlay.lua
@@ -154,7 +154,7 @@ end
 
 gLastPlay = gCurrentPlay
 gNextPlay = ""
-debugEngine:gui_debug_msg(CGeoPoint:new_local(-param.pitchLength*2/5, param.pitchWidth/2+200),gCurrentPlay)
+debugEngine:gui_debug_msg_fix(CGeoPoint:new_local(-param.pitchLength*2/5, param.pitchWidth/2+200),gCurrentPlay)
 RunPlay(gCurrentPlay)
 gLastTask = gRoleTask
 gRoleTask = {}
@@ -174,7 +174,7 @@ gRoleTask = {}
 --print("ball",ball.posX(),ball.valid())
 --print("raw",vision:rawBall():X(),vision:rawBall():Valid())
 --print(vision:getBallVelStable(),vision:ballVelValid())
-debugEngine:gui_debug_msg(CGeoPoint:new_local(-param.pitchLength*2/5, param.pitchWidth/2),gCurrentState)
+debugEngine:gui_debug_msg_fix(CGeoPoint:new_local(-param.pitchLength*2/5, param.pitchWidth/2),gCurrentState)
 --world:drawReflect(gRoleNum["Tier"])
 --defenceInfo:setNoChangeFlag()
 --print(vision:getCycle()..vision:getCurrentRefereeMsg(),vision:gameState():gameOn())
diff --git a/ZBin/lua_scripts/SubPlay.lua b/ZBin/lua_scripts/SubPlay.lua
new file mode 100644
index 0000000..75f3668
--- /dev/null
+++ b/ZBin/lua_scripts/SubPlay.lua
@@ -0,0 +1,85 @@
+SubPlay = {
+}
+function SubPlay.pack(name, play, param)
+    local curState = play.firstState
+    if play.__init__ ~= nil then
+        play.__init__(name, param)
+    end
+    return {
+        name = name,
+        roleMapping = {},
+        play = play,
+        curState = curState,
+        lastState = nil,
+    }
+end
+
+gSubPlay = {
+    playTable = {},
+    curScope = "",
+}
+gSubPlayFiles = {}
+
+function gSubPlay.new(name, playName, initParam)
+    print("in initPlay : ", name, initParam)
+    if gSubPlay.playTable[name] ~= nil then
+        warning("subPlay exist, reinit a new one - ", name)
+    end
+    if gSubPlayFiles[playName] == nil then
+        warning(string.format("subPlay file not exist - %s", playName))
+        return
+    end
+    spec = dofile(gSubPlayFiles[playName])
+    gSubPlay.playTable[name] = SubPlay.pack(name, spec, initParam)
+end
+
+function gSubPlay.step()
+    local debugX = -2000
+    local debugY = param.pitchWidth/2+200
+    local step = -130
+    for key, play in pairs(gSubPlay.playTable) do
+        gSubPlay.curScope = key
+        local _subPlay = play.play
+        local curState = play.curState
+        curState = _RunPlaySwitch(_subPlay, curState)
+        local isStateSwitched = false
+        if curState ~= nil then
+            print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
+            play.lastState = play.curState
+            play.curState = curState
+            isStateSwitched = true
+            PlayFSMClearAll()
+        end
+        -- print("ID : , play : , curState :", key,  _subPlay.name, play.curState)
+        gSubPlay.curScope = ""
+        debugEngine:gui_debug_msg_fix(CGeoPoint(debugX, debugY), key .. " - " .. play.curState, param.GREEN, 0,100)
+        debugY = debugY + step
+    end
+end
+
+function gSubPlay.getState(name)
+    if gSubPlay.playTable[name] == nil then
+        warning("(call gSubPlay.getState) subPlay not exist - ", name)
+        return nil
+    end
+    return gSubPlay.playTable[name].curState
+end
+
+function gSubPlay.roleTask(name, role)
+    return function()
+        if gSubPlay.playTable[name] == nil then
+            warning("roleTask not exist - ", name, role)
+            return
+        end
+        local _subPlayState = gSubPlay.playTable[name].play[gSubPlay.playTable[name].curState]
+        return _subPlayState[role]
+    end
+end
+
+-- function gSubPlay.getRoleNum(roleName)
+--     print("in getRoleNum : ", roleName)
+--     if gSubPlay.curScope == nil then
+--         return gRoleNum[roleName]
+--     end
+--     return -1
+-- end
\ No newline at end of file
diff --git a/ZBin/lua_scripts/Zeus.lua b/ZBin/lua_scripts/Zeus.lua
index 1a61a93..a4d4a63 100644
--- a/ZBin/lua_scripts/Zeus.lua
+++ b/ZBin/lua_scripts/Zeus.lua
@@ -12,6 +12,7 @@ math.random(1,10)
 require(OPPONENT_NAME)
 require("Skill")
 require("Play")
+require("SubPlay")
 require("cond")
 require("pos")
 require("dir")
@@ -104,7 +105,11 @@ for _, value in ipairs(tactic_packages) do
 		-- print("Skill Files : ",table.concat(play_files, ","))
 		for _, filename in ipairs(play_files) do
 			print("Init TPs Play : ",filename)
-			dofile(filename)
+			local res = dofile(filename)
+			if res ~= nil and res.name ~= nil then
+				gPlayTable.CreatePlay(res)
+				gSubPlayFiles[res.name] = filename
+			end
 		end
 	else
 		print("Tactic Dir Not Exists : ",tactic_dir)
diff --git a/ZBin/lua_scripts/play/Test/TestRun.lua b/ZBin/lua_scripts/play/Test/TestRun.lua
index 3e5b11d..a69888c 100644
--- a/ZBin/lua_scripts/play/Test/TestRun.lua
+++ b/ZBin/lua_scripts/play/Test/TestRun.lua
@@ -14,10 +14,6 @@ firstState = "run1",
 
 ["run1"] = {
 	switch = function()
-		print(skillapi:get_size())
-		for i=0,skillapi:get_size()-1 do
-			print(i .. " " .. skillapi:get_name(i))
-		end
 	end,
 	Assister = task.goCmuRush(testPos[1],0, nil, DSS_FLAG),
 	Leader = task.goCmuRush(testPos[2],0, nil, DSS_FLAG),
diff --git a/ZBin/lua_scripts/utils/bufcnt.lua b/ZBin/lua_scripts/utils/bufcnt.lua
index 172bcd6..7136364 100644
--- a/ZBin/lua_scripts/utils/bufcnt.lua
+++ b/ZBin/lua_scripts/utils/bufcnt.lua
@@ -1,4 +1,62 @@
-function bufcnt( cond, buf, cnt )
+local BufCnter = {}
+function BufCnter.new(buf, cnt)
+	return {
+		cndCnter = 0,
+		cycCnter = 0,
+		BUF = buf,
+		CNT = cnt or 9999,
+		-- lastCycle = vision:getCycle(),
+	}
+end
+function BufCnter.update(bc, cond, buf, cnt)
+	local cnt = cnt or 9999
+	-- bc.lastCycle = vision:getCycle()
+	bc.cycCnter = bc.cycCnter + 1
+	bc.cndCnter = cond and bc.cndCnter + 1 or 0
+	if bc.cycCnter >= cnt then
+		return true
+	end
+	if bc.cndCnter >= buf then
+		return true
+	end
+	return false
+end
+
+local __BCs = {}
+function bufcnt(cond, buf, cnt)
+	local scope = gSubPlay.curScope
+	if scope == nil or  scope == "" then
+		scope = "__G__"
+	end
+	if __BCs[scope] == nil then
+		__BCs[scope] = {
+			index = 0,
+			lastCycle = vision:getCycle(),
+		}
+	end
+	if __BCs[scope].lastCycle ~= vision:getCycle() then
+		__BCs[scope].index = 0
+	end
+
+	local index = __BCs[scope].index
+
+	if __BCs[scope][index] == nil then
+		__BCs[scope][index] = BufCnter.new(buf, cnt)
+	end
+	local res = BufCnter.update(__BCs[scope][index],cond, buf, cnt)
+	__BCs[scope].index = index + 1
+	return res
+end
+
+function bufcntClear()
+	local scope = gSubPlay.curScope
+	if scope == nil or  scope == "" then
+		scope = "__G__"
+	end
+	__BCs[scope] = nil
+end
+
+function ___bufcnt( cond, buf, cnt )
 	if buf == "normal" then
 		buf = 6
 	elseif buf == "slow" then
@@ -15,17 +73,3 @@ function bufcnt( cond, buf, cnt )
 		return false
 	end
 end
-
-local COND_FIX_STATUS = false
-
-function minbufcnt(c, min_cycle, buf, max_cycle)
-	if bufcnt(true, min_cycle) then
-    	COND_FIX_STATUS = true
-    end
-    if bufcnt(COND_FIX_STATUS and c, buf, max_cycle) then
-        COND_FIX_STATUS = false
-        return true
-    end
-
-    return false
-end
\ No newline at end of file
diff --git a/ZBin/lua_scripts/utils/utils.lua b/ZBin/lua_scripts/utils/utils.lua
index 2abbae6..2e041fb 100644
--- a/ZBin/lua_scripts/utils/utils.lua
+++ b/ZBin/lua_scripts/utils/utils.lua
@@ -1,3 +1,14 @@
+local warningX = 0
+local warningY = 0
+function warning(msg)
+    msg = "WARNING: " .. msg
+    print( msg)
+    debugEngine:gui_debug_msg(CGeoPoint(warningX, warningY), msg, param.CYAN)
+    warningY = warningY + 120
+    if warningY > param.pitchWidth/2 then
+        warningY = -param.pitchWidth/2
+    end
+end
 function _c(f,...) -- auto convert for closure
 	if type(f) == "function" then
 		return f(...)

From 066e7dc9631296446ace351f59fa618fcc288491 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Tue, 9 Apr 2024 21:29:28 +0800
Subject: [PATCH 03/30] [Core] link real number with subPlay Role

---
 Core/tactics/play/TestMyRun.lua        | 29 ++++++++
 Core/tactics/play/TestPassAndKick.lua  | 12 ++--
 Core/tactics/play/TestSubScript.lua    | 15 ++--
 ZBin/lua_scripts/Play.lua              | 58 +++++++++-------
 ZBin/lua_scripts/SelectPlay.lua        | 20 +-----
 ZBin/lua_scripts/SubPlay.lua           | 95 ++++++++++++++++++++++----
 ZBin/lua_scripts/utils/utils.lua       |  4 +-
 ZBin/lua_scripts/worldmodel/cond.lua   | 65 ------------------
 ZBin/lua_scripts/worldmodel/dir.lua    |  8 +--
 ZBin/lua_scripts/worldmodel/player.lua | 29 +++++---
 ZBin/lua_scripts/worldmodel/pos.lua    | 19 ------
 11 files changed, 181 insertions(+), 173 deletions(-)
 create mode 100644 Core/tactics/play/TestMyRun.lua

diff --git a/Core/tactics/play/TestMyRun.lua b/Core/tactics/play/TestMyRun.lua
new file mode 100644
index 0000000..8f09306
--- /dev/null
+++ b/Core/tactics/play/TestMyRun.lua
@@ -0,0 +1,29 @@
+local testPos = {CGeoPoint:new_local(1000, 1000), CGeoPoint:new_local(-1000, 1000), CGeoPoint:new_local(-1000, -1000),
+                 CGeoPoint:new_local(1000, -1000)}
+local vel = CVector:new_local(0, 0)
+local maxvel = 0
+local time = 120
+local DSS_FLAG = bit:_or(flag.allow_dss, flag.dodge_ball)
+
+local DIR = function()
+    return (player.pos('Assister') - ball.pos()):dir()
+end
+
+return {
+    firstState = "run1",
+
+    ["run1"] = {
+        switch = function()
+        end,
+        Assister = task.goCmuRush(testPos[1], DIR, nil, DSS_FLAG),
+        match = "[A]"
+    },
+
+    name = "TestMyRun",
+    applicable = {
+        exp = "a",
+        a = true
+    },
+    attribute = "attack",
+    timeout = 99999
+}
diff --git a/Core/tactics/play/TestPassAndKick.lua b/Core/tactics/play/TestPassAndKick.lua
index adc2372..ef516d7 100644
--- a/Core/tactics/play/TestPassAndKick.lua
+++ b/Core/tactics/play/TestPassAndKick.lua
@@ -1,11 +1,12 @@
 local waitPos = ball.antiYPos(CGeoPoint:new_local(2600,1500))
 local waitPos2 = ball.antiYPos(CGeoPoint:new_local(1800,1500))
 local mode = true
-gPlayTable.CreatePlay{
+
+return {
 firstState = "init",
 ["init"] = {
     switch = function()
-        if player.toTargetDist("Assister") < 1000 then
+        if player.toTargetDist("Assister") < 1000 and player.toTargetDist("Leader") < 100 then
             return "pass"
         end
     end,
@@ -19,15 +20,18 @@ firstState = "init",
             return "shoot"
         end
 	end,
-	Leader = task.touchKick(waitPos,false,540,mode),
+	Leader = task.touchKick(waitPos,false,3500,mode),
     Assister = task.goCmuRush(waitPos2),
 	match = ""
 },
 ["shoot"] = {
     switch = function()
+        if player.kickBall("Assister") then
+            return "init"
+        end
     end,
     Leader = task.stop(),
-    Assister = task.touchKick(pos.theirGoal(), false,300,mode),
+    Assister = task.touchKick(pos.theirGoal(), false,6000,mode),
     match = ""
 },
 
diff --git a/Core/tactics/play/TestSubScript.lua b/Core/tactics/play/TestSubScript.lua
index a448d35..8ccb32b 100644
--- a/Core/tactics/play/TestSubScript.lua
+++ b/Core/tactics/play/TestSubScript.lua
@@ -25,15 +25,14 @@ return {
         switch = function()
             if bufcnt(true,30) then 
                 if not subScript then
-                    gSubPlay.new(PLAY_NAME .. "task1", "TestSubScript", {pos=start_pos + Utils.Polar2Vector(2*dist, math.pi/4), dist=2000})
-                    gSubPlay.new(PLAY_NAME .. "task2", "TestSubScript", {pos=start_pos + Utils.Polar2Vector(2*dist, -math.pi/4*3), dist=500})
-                    rotateSpeed = -1
+                    gSubPlay.new("kickTask", "TestPassAndKick")
                 end
                 return "run"
             end
         end,
+        Assister = task.stop(),
         Leader = task.stop(),
-        match = "[L]"
+        match = "[LA]"
     },
     ["run"] = {
         switch = function()
@@ -42,10 +41,10 @@ return {
             --     print("printFileTable: ", key, value)
             -- end
         end,
-        Assister = gSubPlay.roleTask("task1","Leader"),
-        Fronter = gSubPlay.roleTask("task2","Leader"),
-        Leader = task.goCmuRush(runPos, 0, nil, DSS_FLAG),
-        match = "(LAF)"
+        b = gSubPlay.roleTask("kickTask", "Assister"),
+        c = gSubPlay.roleTask("kickTask", "Leader"),
+        a = task.goCmuRush(runPos, 0, nil, DSS_FLAG),
+        match = "(abc)"
     },
 
     name = "TestSubScript",
diff --git a/ZBin/lua_scripts/Play.lua b/ZBin/lua_scripts/Play.lua
index 9816c0c..f5dc7c9 100644
--- a/ZBin/lua_scripts/Play.lua
+++ b/ZBin/lua_scripts/Play.lua
@@ -85,28 +85,28 @@ end
 
 -- 注意，此处只是针对间接和直接定位球的防守
 -- 此时，Leader和Goalie不参与第二次防碰撞检测
-function UsePenaltyCleaner(curPlay)
-	for rolename, task in pairs(curPlay[gRealState]) do
-		if(type(task) == "table" and rolename ~= "match" and rolename ~= "Goalie" and rolename ~= "Kicker") then
-			local p
-			if type(gRolePos[rolename]) == "function" then
-				p = gRolePos[rolename]()
-			else
-				p = gRolePos[rolename]
-			end
-			CAddPenaltyCleaner(string.sub(rolename,1,1), gRoleNum[rolename], p:x(), p:y())
-		end
-	end
-	CCleanPenalty()
-	for rolename, task in pairs(curPlay[gRealState]) do
-		if(type(task) == "table" and rolename ~= "match" and rolename ~= "Goalie" and rolename ~= "Kicker") then
-			local x, y = CGetPenaltyCleaner(string.sub(rolename,1,1))
-			gRolePos[rolename] = CGeoPoint:new_local(x,y)
-		end
-	end
-	-- print(gCurrentState, CGetResetMatchStr())
-	DoRoleMatchReset(CGetResetMatchStr())
-end
+-- function UsePenaltyCleaner(curPlay)
+-- 	for rolename, task in pairs(curPlay[gRealState]) do
+-- 		if(type(task) == "table" and rolename ~= "match" and rolename ~= "Goalie" and rolename ~= "Kicker") then
+-- 			local p
+-- 			if type(gRolePos[rolename]) == "function" then
+-- 				p = gRolePos[rolename]()
+-- 			else
+-- 				p = gRolePos[rolename]
+-- 			end
+-- 			CAddPenaltyCleaner(string.sub(rolename,1,1), gRoleNum[rolename], p:x(), p:y())
+-- 		end
+-- 	end
+-- 	CCleanPenalty()
+-- 	for rolename, task in pairs(curPlay[gRealState]) do
+-- 		if(type(task) == "table" and rolename ~= "match" and rolename ~= "Goalie" and rolename ~= "Kicker") then
+-- 			local x, y = CGetPenaltyCleaner(string.sub(rolename,1,1))
+-- 			gRolePos[rolename] = CGeoPoint:new_local(x,y)
+-- 		end
+-- 	end
+-- 	-- print(gCurrentState, CGetResetMatchStr())
+-- 	DoRoleMatchReset(CGetResetMatchStr())
+-- end
 
 function DoRolePosMatch(curPlay, isPlaySwitched, isStateSwitched)
 	if gCurrentState == "exit" or gCurrentState == "finish" then
@@ -116,6 +116,13 @@ function DoRolePosMatch(curPlay, isPlaySwitched, isStateSwitched)
 	end
 	gActiveRole = {}
 	for rolename, itask in pairs(curPlay[gRealState]) do
+		local task = itask
+		-- unpack for subPlayTask
+		if (type(task) == "table" and task.name == "subPlayTask" and task.task ~= nil) then
+			gSubPlay.register("", rolename, task.args)
+			itask = task.task
+		end
+
 		if(type(itask) == "function" and rolename ~= "match" and rolename~="switch") then
 			itask = itask()
 		end
@@ -196,7 +203,7 @@ function RunPlay(name)
 			isStateSwitched = true
 			PlayFSMClearAll()
 		end
-
+		
 --		debugEngine:gui_debug_msg(vision:ourPlayer(gRoleNum[rolename]):Pos(), rolename)
 		gSubPlay.step()
 		DoRolePosMatch(curPlay, false, isStateSwitched)
@@ -210,6 +217,10 @@ function RunPlay(name)
 --~		1 ---> task, 2 --> matchpos, 3--->kick, 4 --->dir, 5 --->pre, 6 --->kp, 7--->cp, 8--->flag
 		kickStatus:clearAll()
 		for rolename, task in pairs(curPlay[gRealState]) do
+			if (type(task) == "table" and task.name == "subPlayTask" and task.task ~= nil) then
+				gSubPlay.register("", rolename, task.args)
+				task = task.task
+			end
 			if (type(task) == "function" and rolename ~= "match" and (gRoleNum[rolename] ~= nil or type(rolename)=="function")) then
 				task = task(gRoleNum[rolename])
 			end
@@ -226,7 +237,6 @@ function RunPlay(name)
 				elseif type(rolename)=="function" then
 					roleNum = rolename()
 					--print("Here in function : "..roleNum)
-					
 				end
 				
 				if roleNum ~= -1 then
diff --git a/ZBin/lua_scripts/SelectPlay.lua b/ZBin/lua_scripts/SelectPlay.lua
index 1545c55..d6fe51b 100644
--- a/ZBin/lua_scripts/SelectPlay.lua
+++ b/ZBin/lua_scripts/SelectPlay.lua
@@ -158,23 +158,5 @@ debugEngine:gui_debug_msg_fix(CGeoPoint:new_local(-param.pitchLength*2/5, param.
 RunPlay(gCurrentPlay)
 gLastTask = gRoleTask
 gRoleTask = {}
--- local stateFile=io.open(".\\LOG\\"..gStateFileNameString..".txt","a")
--- if not stateFile then
--- 	print("-------- Can't create LOG FILE !!!!!!!!!!!!! --------")
--- end
--- stateFile:write(vision:getCycle().." "..gCurrentState.." "..gCurrentPlay.." me:"..skillUtils:getOurBestPlayer()
--- 	.." he:"..skillUtils:getTheirBestPlayer().."\n")
--- stateFile:close()
 
---print( world:getSuitSider())
---print("vel"..enemy.vel(skillUtils:getTheirGoalie()):y()*2)
---print(skillUtils:getTheirGoalie())
---print("hello",skillUtils:getOurBestPlayer())
---print(gCurrentState,vision:getCycle())
---print("ball",ball.posX(),ball.valid())
---print("raw",vision:rawBall():X(),vision:rawBall():Valid())
---print(vision:getBallVelStable(),vision:ballVelValid())
-debugEngine:gui_debug_msg_fix(CGeoPoint:new_local(-param.pitchLength*2/5, param.pitchWidth/2),gCurrentState)
---world:drawReflect(gRoleNum["Tier"])
---defenceInfo:setNoChangeFlag()
---print(vision:getCycle()..vision:getCurrentRefereeMsg(),vision:gameState():gameOn())
+debugEngine:gui_debug_msg_fix(CGeoPoint:new_local(-param.pitchLength*2/5, param.pitchWidth/2),gCurrentState)
\ No newline at end of file
diff --git a/ZBin/lua_scripts/SubPlay.lua b/ZBin/lua_scripts/SubPlay.lua
index 75f3668..4398b40 100644
--- a/ZBin/lua_scripts/SubPlay.lua
+++ b/ZBin/lua_scripts/SubPlay.lua
@@ -33,6 +33,10 @@ function gSubPlay.new(name, playName, initParam)
     gSubPlay.playTable[name] = SubPlay.pack(name, spec, initParam)
 end
 
+function gSubPlay.del(name)
+    gSubPlay.playTable[name] = nil
+end
+
 function gSubPlay.step()
     local debugX = -2000
     local debugY = param.pitchWidth/2+200
@@ -44,7 +48,6 @@ function gSubPlay.step()
         curState = _RunPlaySwitch(_subPlay, curState)
         local isStateSwitched = false
         if curState ~= nil then
-            print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
             play.lastState = play.curState
             play.curState = curState
             isStateSwitched = true
@@ -66,20 +69,82 @@ function gSubPlay.getState(name)
 end
 
 function gSubPlay.roleTask(name, role)
-    return function()
-        if gSubPlay.playTable[name] == nil then
-            warning("roleTask not exist - ", name, role)
-            return
-        end
-        local _subPlayState = gSubPlay.playTable[name].play[gSubPlay.playTable[name].curState]
-        return _subPlayState[role]
+    return {
+        name = "subPlayTask",
+        task = function()
+            if gSubPlay.playTable[name] == nil then
+                warning("roleTask not exist - " .. name .. " " .. role)
+                return
+            end
+            local _subPlayState = gSubPlay.playTable[name].play[gSubPlay.playTable[name].curState]
+            subTask = _subPlayState[role]
+            if subTask ~= nil and type(subTask) == "table" and subTask.name == "subPlayTask" then -- subtask call subtask
+                gSubPlay.register(name, role, subTask.args)
+                return subTask.task()
+            end
+            return _subPlayState[role]
+        end,
+        args = {
+            name = name,
+            role = role,
+        }
+    }
+end
+
+function gSubPlay.register(playName, rolename, args)
+    local msg = string.format("%s:%s - %s:%s ",args.name, args.role, playName, rolename)
+    -- print("register : ",msg)
+    if gSubPlay.playTable[args.name] == nil then
+        warning("register failed, play not exist - " .. msg)
+        return
     end
+    gSubPlay.playTable[args.name].roleMapping[args.role] = {playName, rolename}
 end
 
--- function gSubPlay.getRoleNum(roleName)
---     print("in getRoleNum : ", roleName)
---     if gSubPlay.curScope == nil then
---         return gRoleNum[roleName]
---     end
---     return -1
--- end
\ No newline at end of file
+function gSubPlay.getRole(roleName)
+    local role, num = gSubPlay.getRoleAndNum(roleName)
+    return role
+end
+
+function gSubPlay.getRoleNum(roleName)
+    local role, num = gSubPlay.getRoleAndNum(roleName)
+    return num
+end
+
+function gSubPlay.getRoleAndNum(roleName)
+    local scope = gSubPlay.curScope
+    local role = roleName
+    local iterCount = 0
+    local findPath = {}
+    local pathOutput = function(path)
+        local str = ""
+        for i, v in ipairs(path) do
+            str = str .. " -> " .. v[1] .. ":" .. v[2]
+        end
+        return str
+    end
+    -- print("getRoleNum - ", scope, role)
+    while true do
+        if scope == "" then
+            return role, gRoleNum[role]
+        else
+            if gSubPlay.playTable[scope] == nil then
+                warning("subPlay not exist - " .. scope .. " " .. role .. pathOutput(findPath))
+                return -1
+            end
+            if gSubPlay.playTable[scope].roleMapping[role] == nil then
+                warning("role not exist - " .. scope .. " " .. role .. pathOutput(findPath))
+                return -1
+            end
+            local tRoleMap = gSubPlay.playTable[scope].roleMapping[role]
+            scope, role = tRoleMap[1], tRoleMap[2]
+        end
+        table.insert(findPath, {scope,role})
+        -- prevent infinite loop
+        iterCount = iterCount + 1
+        if iterCount > 10 then
+            warning("getRoleNum failed - " .. pathOutput(findPath))
+            return -1
+        end
+    end
+end
\ No newline at end of file
diff --git a/ZBin/lua_scripts/utils/utils.lua b/ZBin/lua_scripts/utils/utils.lua
index 2e041fb..4548bd7 100644
--- a/ZBin/lua_scripts/utils/utils.lua
+++ b/ZBin/lua_scripts/utils/utils.lua
@@ -1,9 +1,9 @@
-local warningX = 0
+local warningX = -param.pitchLength/2
 local warningY = 0
 function warning(msg)
     msg = "WARNING: " .. msg
     print( msg)
-    debugEngine:gui_debug_msg(CGeoPoint(warningX, warningY), msg, param.CYAN)
+    debugEngine:gui_debug_msg_fix(CGeoPoint(warningX, warningY), msg, param.CYAN)
     warningY = warningY + 120
     if warningY > param.pitchWidth/2 then
         warningY = -param.pitchWidth/2
diff --git a/ZBin/lua_scripts/worldmodel/cond.lua b/ZBin/lua_scripts/worldmodel/cond.lua
index d5582c2..93d979a 100644
--- a/ZBin/lua_scripts/worldmodel/cond.lua
+++ b/ZBin/lua_scripts/worldmodel/cond.lua
@@ -36,38 +36,10 @@ function isNormalStart()
 	return vision:gameState():canEitherKickBall()
 end
 
---~ -----------------------------------------------
---~ geometry condition
---~ -----------------------------------------------
-
-
---~ -----------------------------------------------
---~ other condition
---~ -----------------------------------------------
-function bestPlayerChanged()
-	return world:IsBestPlayerChanged()
-end
-
-function canShootOnBallPos(role)
-	return world:canShootOnBallPos(vision:getCycle(),gRoleNum[role])
-end
-
-function canPassOnBallPos(role,passPos,guisePos)
-	return world:canPassOnBallPos(vision:getCycle(),passPos,guisePos,gRoleNum[role])
-end
-
--- function canKickAtEnemy(role,kickDir)
--- 	return world:canKickAtEnemy(vision:getCycle(),kickDir,gRoleNum[role])
--- end
-
 function validNum()
 	return vision:getValidNum()
 end
 
-function canDefenceExit()
-	return world:CanDefenceExit()
-end
-
 function timeRemain()
 	return vision:timeRemain()
 end
@@ -242,43 +214,6 @@ function  kickOffEnemyNumChanged()
 end
 
 --------------------------------------For Freekick--------------------------------------------------
-function canPassAndShoot(role)
-		if gRoleNum[role] ~= 0 and not world:isPassLineBlocked(gRoleNum[role]) and not world:isShootLineBlocked(gRoleNum[role]) then
-			return true
-		else
-			return false
-		end
-end
-
--- 优先级从role1至role5降低, 可少角色
-function findChance(role1, role2, role3, role4, role5)
-	if role1 ~= nil and type(role1) == "string" then
-		if gRoleNum[role1] ~= 0 and not world:isBeingMarked(gRoleNum[role1]) and canPassAndShoot(role1) then
-			return role1
-		end
-	end
-	if role2 ~= nil and type(role2) == "string" then
-		if gRoleNum[role2] ~= 0 and not world:isBeingMarked(gRoleNum[role2]) and canPassAndShoot(role2) then
-			return role2
-		end
-	end
-	if role3 ~= nil and type(role3) == "string" then
-		if gRoleNum[role3] ~= 0 and not world:isBeingMarked(gRoleNum[role3]) and canPassAndShoot(role3) then
-			return role3
-		end
-	end
-	if role4 ~= nil and type(role4) == "string" then
-		if gRoleNum[role4] ~= 0 and not world:isBeingMarked(gRoleNum[role4]) and canPassAndShoot(role4) then
-			return role4
-		end
-	end
-	if role5 ~= nil and type(role5) == "string" then
-		if gRoleNum[role5] ~= 0 and not world:isBeingMarked(gRoleNum[role5]) and canPassAndShoot(role5) then
-			return role5
-		end
-	end
-	return "None"
-end
 
 -- str 为所在的区域
 -- script 为所使用的脚本
diff --git a/ZBin/lua_scripts/worldmodel/dir.lua b/ZBin/lua_scripts/worldmodel/dir.lua
index 5617a0e..bc0c713 100644
--- a/ZBin/lua_scripts/worldmodel/dir.lua
+++ b/ZBin/lua_scripts/worldmodel/dir.lua
@@ -90,7 +90,7 @@ function evaluateTouch(p)
 	end
 	return function (role)
 		if type(role) == "string" then
-			role = gRoleNum[role]
+			role = gSubPlay.getRoleNum(role)
 		elseif type(role) == "number" and
 			role >= 1 and role <= param.maxPlayer then
 			role = role
@@ -269,12 +269,6 @@ function sideBackDir()
 	return (pos.sideBackPos()- pos.ourGoal()):dir()
 end
 
-function getTandemDir(role)
-	return function ()
-		return world:getTandemDir(gRoleNum[role])
-	end
-end
-
 function reflectDir(d)
 	return ball.refSyntYDir(d)
 end
diff --git a/ZBin/lua_scripts/worldmodel/player.lua b/ZBin/lua_scripts/worldmodel/player.lua
index 86792d2..533e962 100644
--- a/ZBin/lua_scripts/worldmodel/player.lua
+++ b/ZBin/lua_scripts/worldmodel/player.lua
@@ -3,7 +3,7 @@ module(..., package.seeall)
 function instance(role)
 	local realIns
 	if type(role) == "string" then
-		realIns = vision:ourPlayer(gRoleNum[role])
+		realIns = vision:ourPlayer(num(role))
 	elseif type(role) == "number" then
 --	and	role >= 1 and role <= param.maxPlayer then
 		realIns = vision:ourPlayer(role)
@@ -16,8 +16,12 @@ end
 
 function num(role)
 	local retNum
+	if type(role) == "function" then
+		role = role()
+	end
 	if type(role) == "string" then
-		retNum = gRoleNum[role]
+		retNum = gSubPlay.getRoleNum(role)
+		-- retNum = gRoleNum[role]
 	elseif type(role) == "number" then
 		retNum = role
 	else
@@ -150,10 +154,14 @@ end
 
 function toTargetDist(role)
 	local p
-	if type(gRolePos[role]) == "function" then
-		p = gRolePos[role]()
+	local realrole = gSubPlay.getRole(role)
+	if type(gRolePos[realrole]) == "function" then
+		p = gRolePos[realrole]()
 	else
-		p = gRolePos[role]
+		p = gRolePos[realrole]
+	end
+	if p == nil then
+		return 9999
 	end
 	return player.pos(role):dist(p)
 end
@@ -241,12 +249,13 @@ function canBreak(role)
 	for i=1,param.maxPlayer do
 		if enemy.valid(i) then
 			local p
-			if type(gRolePos[role]) == "function" then
-				p = gRolePos[role]()
+			local realrole = gSubPlay.getRole(role)
+			if type(gRolePos[realrole]) == "function" then
+				p = gRolePos[realrole]()
 			else
-				p = gRolePos[role]
+				p = gRolePos[realrole]
 			end
-			local breakSeg = CGeoSegment:new_local(player.pos(role), p)
+			local breakSeg = CGeoSegment:new_local(player.pos(realrole), p)
 			local projP = breakSeg:projection(enemy.pos(i))
 			if breakSeg:IsPointOnLineOnSegment(projP) then
 				if enemy.pos(i):dist(projP) < 40 then
@@ -353,7 +362,7 @@ function canFlatPassToPos(role, targetpos)
 		end
 	end
 	for j = 1, param.maxPlayer do
-		if player.valid(j) and j ~= gRoleNum["Leader"] and player.pos(j):dist(p2) > 20 then
+		if player.valid(j) and j ~= num("Leader") and player.pos(j):dist(p2) > 20 then
 			local dist = seg:projection(player.pos(j)):dist(player.pos(j))
 			local isprjon = seg:IsPointOnLineOnSegment(seg:projection(player.pos(j)))
 			if dist < 12 and isprjon then
diff --git a/ZBin/lua_scripts/worldmodel/pos.lua b/ZBin/lua_scripts/worldmodel/pos.lua
index 4706682..b6b097c 100644
--- a/ZBin/lua_scripts/worldmodel/pos.lua
+++ b/ZBin/lua_scripts/worldmodel/pos.lua
@@ -254,12 +254,6 @@ function theirBestPlayer()
 	return enemy.pos(oppNum)
 end
 
-function getTandemPos(role)
-	return function ()
-		return world:getTandemPos(gRoleNum[role])
-	end
-end
-
 -- x, y分别为相对于球的偏移量, yys 2014-06-11
 function reflectPos(x, y)
 	return function ()
@@ -272,19 +266,6 @@ function reflectPos(x, y)
 	end
 end
 
-function reflectPassPos(role)
-	return function ()
-		return world:getReflectPos(gRoleNum[role])
-	end
-end
-
--- y表示在距离中线多远的地方做touch, yys 2014-06-20
-function reflectTouchPos(role, y)
-	return function ()
-		return world:getReflectTouchPos(gRoleNum[role], y)
-	end
-end
-
 -- 定位球传球点, yys 2014-06-16
 function generateFreeKickPassPos(role)
 	return function ()

From d08f954abc7ab9cd49e014eb3330d5bfac428c1e Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 10 Apr 2024 15:56:47 +0800
Subject: [PATCH 04/30] [Core] tidy ballspeedmodel

---
 Core/src/Algorithm/BallSpeedModel.cpp   | 138 ++++++------------------
 Core/src/Algorithm/BallSpeedModel.h     |  47 +++-----
 Core/src/Algorithm/runtimepredictor.cpp |   6 --
 Core/src/Algorithm/runtimepredictor.h   |  11 --
 Core/src/LuaModule/ballspeedmodel.pkg   |   7 ++
 Core/src/LuaModule/global.pkg           |   3 +-
 Core/src/LuaModule/zeus.pkg             |   3 +-
 Core/src/Main/Global.cpp                |   2 +
 Core/src/Main/Global.h                  |   2 +
 Core/src/Main/zeus_main.cpp             |   1 +
 ZBin/lua_scripts/worldmodel/param.lua   |   2 +-
 11 files changed, 67 insertions(+), 155 deletions(-)
 delete mode 100644 Core/src/Algorithm/runtimepredictor.cpp
 delete mode 100644 Core/src/Algorithm/runtimepredictor.h
 create mode 100644 Core/src/LuaModule/ballspeedmodel.pkg

diff --git a/Core/src/Algorithm/BallSpeedModel.cpp b/Core/src/Algorithm/BallSpeedModel.cpp
index 65f3612..5fe849d 100644
--- a/Core/src/Algorithm/BallSpeedModel.cpp
+++ b/Core/src/Algorithm/BallSpeedModel.cpp
@@ -1,4 +1,5 @@
 #include "BallSpeedModel.h"
+#include "VisionModule.h"
 #include "staticparams.h"
 #include <cmath>
 #include <string>
@@ -8,122 +9,53 @@
 using namespace std;
 
 namespace{
-	static int FRICTION;
+	// _DEC > 0 from friction
+	static double _DEC;
 }
-static double oneFromFive(const double x1,const double y1,const double x2,const double y2,const double x3);
 
 CBallSpeedModel::CBallSpeedModel():_ballVel(0,0),_ballPos(-9999,-9999){
     bool IS_SIMULATION;
     ZSS::ZParamManager::instance()->loadParam(IS_SIMULATION,"Alert/IsSimulation",false);
     if (IS_SIMULATION)
-        ZSS::ZParamManager::instance()->loadParam(FRICTION,"AlertParam/Friction4Sim",800);
-    else
-        ZSS::ZParamManager::instance()->loadParam(FRICTION,"AlertParam/Friction4Real",1520);
+		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Sim", 1200);
+	else
+		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Real", 400);
 }
 CBallSpeedModel::~CBallSpeedModel(){
 }
-
-//todo
-CVector CBallSpeedModel::speedForTime(double frame, const CVisionModule* pVision ){
-	update(pVision);
-    return speedForTime_FM(frame);
-}
-
-//todo
-CVector CBallSpeedModel::speedForDist(double dist, const CVisionModule* pVision) {
-    update(pVision);
-    return speedForDist_FM(dist);
+double CBallSpeedModel::timeForDist(const double dist){
+	return std::get<0>(predictForDist(dist));
 }
-
-//use new speed mode
-double CBallSpeedModel::timeForDist(double dist, const CVisionModule* pVision ){
-    update(pVision);
-    return timeForDist_FM(dist);
-}
-
-//use new speed mode
-CGeoPoint CBallSpeedModel::posForTime(double frame, const CVisionModule* pVision ){
-    update(pVision);
-    return posForTime_FM(frame);
+CVector CBallSpeedModel::speedForDist(const double dist){
+	return std::get<1>(predictForDist(dist));
 }
-CVector CBallSpeedModel::speedForTime_FM(double frame) {
-	auto vel = _ballVel.mod() - 0.5 * FRICTION * frame*1.0 / PARAM::Vision::FRAME_RATE;
-	if (vel < 0) vel = 0;
-	return _ballVel / _ballVel.mod() * vel;
-}
-CVector CBallSpeedModel::speedForDist_FM(double dist) {
-	auto v = _ballVel.mod();
-	auto t = v*v - FRICTION*dist;
-	if (t <= 0) return CVector(0, 0);
-	auto vel = std::sqrt(t);
-	return _ballVel / _ballVel.mod() * vel;
-}
-double CBallSpeedModel::timeForDist_FM(double dist) {
-	auto v = _ballVel.mod();
-	auto a = 0.25*FRICTION;
-	auto b = -v;
-	auto c = dist;
-	auto delta = b*b - 4 * a*c;
-	if (delta < 0) return -1;
-    return 1.0 / (2 * a)*(-b - std::sqrt(delta));
-}
-CGeoPoint CBallSpeedModel::posForTime_FM(double frame) {
+std::tuple<double, CVector> CBallSpeedModel::predictForDist(const double dist){
+	this->update();
+	auto maxDist = _ballVel.mod() * _ballVel.mod() / (2 * _DEC);
+	if (dist > maxDist) {
+		return {-1, CVector(0,0)};
+	}
 	auto v0 = _ballVel.mod();
-	auto v1 = v0 - 2 * FRICTION*frame/PARAM::Vision::FRAME_RATE;
-	double d;
-	if (v1 < 0)
-		d = v0*v0 / FRICTION;
-	else
-		d = (v0 + v1)*frame / PARAM::Vision::FRAME_RATE / 2;
-	return _ballPos + (_ballVel / _ballVel.mod()*d);
+	auto v1 = sqrt(v0 * v0 - 2 * _DEC * dist);
+	auto time = (v0 - v1) / _DEC;
+	return {time, Utils::Polar2Vector(v1, _ballVel.dir())};
 }
-//todo
-//double CBallSpeedModel::CalKickInitSpeed(const double dist)
-//{
-//	double vt = 100;
-//	if (dist < 100) {
-//		vt = 120;
-//	} else if (dist >= 100 && dist < 200) {
-//		vt = 200;
-//	} else if (dist >= 200 && dist < 300) {
-//		vt = 350;
-//	} else if (dist >= 300 && dist < 400) {
-//		vt = 350;
-//	} else if (dist >= 400) {
-//		vt = 350;
-//	}
-//	vt = vt+100;//让传球速度变小一点
-//	double ballVO = sqrt(vt * vt + 2 * 20 * dist);
-//	//迭代法计算初速度
-//	for (int i = 1;i<=3;i++){
-//		if(ballVO > 240){
-//			cal_acc = 10;
-//		} else if(ballVO > 190 && ballVO <= 240){
-//			cal_acc = ( 0.0025 * ballVO + 0.18 ) * 60;
-//		} else if(ballVO > 140 && ballVO <= 190){
-//			cal_acc = ( (0.0025 + ( 190 - ballVO) *0.00001)  * ballVO + 0.18 ) * 60;
-//		} else if (ballVO > 10 && ballVO <= 140){
-//			cal_acc = ( 0.003  * ballVO + 0.18 ) * 60;
-//		}
-//		ballVO = sqrt(vt * vt + 2 * cal_acc * dist);
-//	}
-//	return ballVO;
-//}
-void CBallSpeedModel::update( const CVisionModule* pVision ){
-	//lastBallPos = _ballPos;
-    _ballPos = pVision->ball().Valid() ? pVision->ball().RawPos() : pVision->ball().Pos();
-    _ballVel = pVision->ball().Vel();
+MobileVisionT CBallSpeedModel::poseForTime(const double time){
+	this->update();
+	MobileVisionT predictPose;
+	auto v0 = _ballVel.mod();
+	auto v1 = std::max(v0 - _DEC * time, 0.0);
+	auto runtime = std::min(v0 / _DEC, time);
+	double dist = (v0 + v1) * runtime / 2;
+	predictPose.SetPos(_ballPos + Utils::Polar2Vector(dist, _ballVel.dir()));
+	predictPose.SetVel(Utils::Polar2Vector(v1, _ballVel.dir()));
+	return predictPose;
 }
 
-//double CBallSpeedModel::getAcc(double speed) const{
-//	if(speed >= 500) return 1.7;
-//	if(speed > 300) return speed/200.0 - 0.8;	//300 - 500 line : (300,0.7) , (500,1.7)
-//	if(speed >= 0.7) return 0.7;
-//	if(speed >= 0) return speed;
-//	cout << "BallSpeedModel.cpp : Speed Mod < 0 in getAcc Function" << endl;
-//	return -1;
-//}
+void CBallSpeedModel::update(){
+	if(lastCycle == vision->getCycle()) return;
+	lastCycle = vision->getCycle();
 
-//static double oneFromFive(const double x1,const double y1,const double x2,const double y2,const double x3){
-//	return (y2-y1)/(x2-x1)*(x3-x2)+y2;
-//}
+	_ballPos = vision->ball().Valid() ? vision->ball().RawPos() : vision->ball().Pos();
+	_ballVel = vision->ball().Vel();
+}
diff --git a/Core/src/Algorithm/BallSpeedModel.h b/Core/src/Algorithm/BallSpeedModel.h
index 8adb776..9a33ce3 100644
--- a/Core/src/Algorithm/BallSpeedModel.h
+++ b/Core/src/Algorithm/BallSpeedModel.h
@@ -1,43 +1,26 @@
 #ifndef _BALL_SPEED_MODEL_H_
 #define _BALL_SPEED_MODEL_H_
+#include <tuple>
+#include "singleton.hpp"
+#include "WorldDefine.h"
 
-#include "VisionModule.h"
-
-class CBallSpeedModel{
+class CVisionModule;
+class CBallSpeedModel
+{
 public:
 	CBallSpeedModel();
 	~CBallSpeedModel();
-	CVector speedForTime(double frame, const CVisionModule* pVision);   //计算若干帧以后的速度
-	CVector speedForDist(double dist, const CVisionModule* pVision); //计算若干距离后的速度
-	double timeForDist(double dist, const CVisionModule* pVision);      //计算球运动若干距离的时间
-	CGeoPoint posForTime(double frame, const CVisionModule* pVision);   //计算若干帧以后的绝对位置
-	void update( const CVisionModule* pVision );
-//	double CalKickInitSpeed(const double dist);
-private:
-    CVector speedForTime_FM(double frame);   //计算若干帧以后的速度
-    CVector speedForDist_FM(double dist); //计算若干距离后的速度
-    double timeForDist_FM(double dist);      //计算球运动若干距离的时间
-    CGeoPoint posForTime_FM(double frame);   //计算若干帧以后的绝对位置
+	inline void registerVision(CVisionModule *vision) { this->vision = vision; }
+	std::tuple<double,CVector> predictForDist(const double dist);	// 计算若干距离后的速度
+	double timeForDist(const double dist);      //计算球运动若干距离的时间
+	CVector speedForDist(const double dist);	// 计算球运动若干距离的速度
+	MobileVisionT poseForTime(const double time); // 计算若干帧以后的绝对位置
 private:
-	CVector _ballVel;
-//	CVector _lastBallVel;
+	void update();
 	CGeoPoint _ballPos;
-//	double cal_acc;
-//	double cal_acc_high_speed;
-//	double cal_acc_low_speed;
-//	double _last_dist;
-private:
-	//add some private function to calculate ACC --hzy 2016.6.12 test
-	//double getAcc(double speed) const;
-//	const bool readTableFile_CheckTable(const string& filename);
-//	const double timeForDist_CheckTable(const double dist) const;
-//	const double distForTime_CheckTable(const int frame) const;
-//	const double checkTime_CheckTable(const double const* _array,const double data) const;
-//	const double checkData_CheckTable(const double const* _array,const double time) const;
-//	double *_speed_data;
-//	double *_dist_data;
-//	int _num;
+	CVector _ballVel;
+	int lastCycle = 0;
+	CVisionModule *vision = nullptr;
 };
 typedef Singleton< CBallSpeedModel > BallSpeedModel;
 #endif // _BALL_SPEED_MODEL_H_
-
diff --git a/Core/src/Algorithm/runtimepredictor.cpp b/Core/src/Algorithm/runtimepredictor.cpp
deleted file mode 100644
index 9212355..0000000
--- a/Core/src/Algorithm/runtimepredictor.cpp
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "runtimepredictor.h"
-
-RunTimePredictor::RunTimePredictor()
-{
-
-}
diff --git a/Core/src/Algorithm/runtimepredictor.h b/Core/src/Algorithm/runtimepredictor.h
deleted file mode 100644
index cc0acd5..0000000
--- a/Core/src/Algorithm/runtimepredictor.h
+++ /dev/null
@@ -1,11 +0,0 @@
-#ifndef RUNTIMEPREDICTOR_H
-#define RUNTIMEPREDICTOR_H
-
-
-class RunTimePredictor
-{
-public:
-    RunTimePredictor();
-};
-
-#endif // RUNTIMEPREDICTOR_H
\ No newline at end of file
diff --git a/Core/src/LuaModule/ballspeedmodel.pkg b/Core/src/LuaModule/ballspeedmodel.pkg
new file mode 100644
index 0000000..39ba87a
--- /dev/null
+++ b/Core/src/LuaModule/ballspeedmodel.pkg
@@ -0,0 +1,7 @@
+$#include <WorldDefine.h>
+class CBallSpeedModel
+{
+	double timeForDist(const double dist);
+	CVector speedForDist(const double dist);
+	MobileVisionT poseForTime(const double time);
+};
\ No newline at end of file
diff --git a/Core/src/LuaModule/global.pkg b/Core/src/LuaModule/global.pkg
index 6073f2b..2dbc246 100644
--- a/Core/src/LuaModule/global.pkg
+++ b/Core/src/LuaModule/global.pkg
@@ -5,4 +5,5 @@ CKickStatus*    kickStatus;
 CDribbleStatus* dribbleStatus;
 CGDebugEngine*  debugEngine;
 CWorldModel*	world;
-CSkillAPI*      skillapi;
\ No newline at end of file
+CSkillAPI*      skillapi;
+CBallSpeedModel* ballModel;
\ No newline at end of file
diff --git a/Core/src/LuaModule/zeus.pkg b/Core/src/LuaModule/zeus.pkg
index e9756db..d750d18 100644
--- a/Core/src/LuaModule/zeus.pkg
+++ b/Core/src/LuaModule/zeus.pkg
@@ -10,4 +10,5 @@ $pfile "gamestate.pkg"
 $pfile "dribblestatus.pkg"
 $pfile "utils.pkg"
 $pfile "munkres.pkg"
-$pfile "skillapi.pkg"
\ No newline at end of file
+$pfile "skillapi.pkg"
+$pfile "ballspeedmodel.pkg"
\ No newline at end of file
diff --git a/Core/src/Main/Global.cpp b/Core/src/Main/Global.cpp
index d9405a5..0200877 100644
--- a/Core/src/Main/Global.cpp
+++ b/Core/src/Main/Global.cpp
@@ -6,6 +6,7 @@ CDribbleStatus* dribbleStatus;
 CGDebugEngine*  debugEngine;
 CWorldModel* world;
 CSkillAPI* skillapi;
+CBallSpeedModel *ballModel;
 void initializeSingleton()
 {
 	vision        = VisionModule::Instance();
@@ -14,4 +15,5 @@ void initializeSingleton()
     debugEngine   = GDebugEngine::Instance();
     world         = WorldModel::Instance();
 	skillapi	  = SkillAPI::Instance();
+	ballModel = BallSpeedModel::Instance();
 }
diff --git a/Core/src/Main/Global.h b/Core/src/Main/Global.h
index f544f51..656209a 100644
--- a/Core/src/Main/Global.h
+++ b/Core/src/Main/Global.h
@@ -8,6 +8,7 @@
 #include "singleton.hpp"
 #include "WorldModel.h"
 #include "skillapi.h"
+#include "BallSpeedModel.h"
 
 extern CVisionModule*  vision;
 extern CKickStatus*    kickStatus;
@@ -15,5 +16,6 @@ extern CDribbleStatus* dribbleStatus;
 extern CGDebugEngine*  debugEngine;
 extern CWorldModel* world;
 extern CSkillAPI* skillapi;
+extern CBallSpeedModel* ballModel;
 void initializeSingleton();
 #endif
diff --git a/Core/src/Main/zeus_main.cpp b/Core/src/Main/zeus_main.cpp
index c16d5ec..41a1cf9 100644
--- a/Core/src/Main/zeus_main.cpp
+++ b/Core/src/Main/zeus_main.cpp
@@ -60,6 +60,7 @@ int runLoop() {
     CCommandInterface::instance(option);
     vision->registerOption(option);
     vision->startReceiveThread();
+    ballModel->registerVision(vision);
     skillapi->registerVision(vision);
     decision = new CDecisionModule(vision);
     action = new CActionModule(vision, decision);
diff --git a/ZBin/lua_scripts/worldmodel/param.lua b/ZBin/lua_scripts/worldmodel/param.lua
index d8ee113..b5c5fb7 100644
--- a/ZBin/lua_scripts/worldmodel/param.lua
+++ b/ZBin/lua_scripts/worldmodel/param.lua
@@ -32,7 +32,7 @@ playerFrontToCenter = 76
 lengthRatio	= 1.5
 widthRatio	= 1.5
 stopRatio = 1.1
-frameRate = 75
+frameRate = 73
 --~ -------------------------------------------
 --~ used for debug
 --~ -------------------------------------------

From bf0243cc4c219d720340b98af8bcd136434f7eac Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 10 Apr 2024 20:07:37 +0800
Subject: [PATCH 05/30] [Core] update touch

---
 Core/src/Algorithm/BallSpeedModel.cpp         |  2 +-
 Core/src/MotionControl/CMmotion.cpp           | 67 ++++++++++---------
 Core/src/MotionControl/ControlModel.cpp       |  2 +-
 Core/src/Strategy/skill/SmartGotoPosition.cpp |  2 -
 Core/tactics/play/TestGetBall.lua             | 32 +++++++++
 Core/tactics/play/TestMyRun.lua               | 49 ++++++++++++--
 Core/tactics/play/TestPassAndKick.lua         | 10 +--
 Core/tactics/skill/Touch.cpp                  | 55 ++++++++++-----
 Core/tactics/skill/Touch.lua                  |  1 -
 Core/tt_params/play/ParamBallDec.lua          | 57 ++++++++++++++++
 share/geomcalc.cpp                            |  5 ++
 share/geomcalc.h                              |  7 ++
 share/staticparams.h                          | 10 +--
 13 files changed, 231 insertions(+), 68 deletions(-)
 create mode 100644 Core/tactics/play/TestGetBall.lua
 create mode 100644 Core/tt_params/play/ParamBallDec.lua
 create mode 100644 share/geomcalc.cpp
 create mode 100644 share/geomcalc.h

diff --git a/Core/src/Algorithm/BallSpeedModel.cpp b/Core/src/Algorithm/BallSpeedModel.cpp
index 5fe849d..4532ecb 100644
--- a/Core/src/Algorithm/BallSpeedModel.cpp
+++ b/Core/src/Algorithm/BallSpeedModel.cpp
@@ -17,7 +17,7 @@ CBallSpeedModel::CBallSpeedModel():_ballVel(0,0),_ballPos(-9999,-9999){
     bool IS_SIMULATION;
     ZSS::ZParamManager::instance()->loadParam(IS_SIMULATION,"Alert/IsSimulation",false);
     if (IS_SIMULATION)
-		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Sim", 1200);
+		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Sim", 1100);
 	else
 		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Real", 400);
 }
diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
index 73bbb38..a294d76 100644
--- a/Core/src/MotionControl/CMmotion.cpp
+++ b/Core/src/MotionControl/CMmotion.cpp
@@ -13,7 +13,7 @@ using namespace std;
 namespace {
 const double FRAME_PERIOD = 1.0 / PARAM::Vision::FRAME_RATE;
 
-bool DEBUG_NO_ZERO_VEL = false;
+bool DEBUG_NO_ZERO_VEL = ZSS::ZParamManager::instance()->value("Debug/NoZeroVel", QVariant(false)).toBool();
 const double DEC_FACTOR = 2.0;
 const double lowerBoundSpeedLimitRotate = 0;
 const double upperBoundSpeedLimitRotate = 300*10;
@@ -76,7 +76,7 @@ void compute_motion_1d(double x0, double v0, double v1,
     }
 
 
-    double decFactor = (pT == ROTATE ? 1.0 : DEC_FACTOR);
+    double decFactor = DEC_FACTOR;
     a_max /= a_factor;
     d_max /= (1.0 * a_factor);
 
@@ -231,27 +231,27 @@ void compute_motion_2d(CVector x0, CVector v0, CVector v1,
     double rotangle = 0;
     double traj_accel_x = 0;
     double traj_accel_y = 0;
-    if(v0 * x0 > 0) {           //如果发现正在反方向走，则不再零速到点，防止车冲出去
-//        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1.0, 0.0), QString("Limit v1").toLatin1());
-        v1 = CVector(0.0, 0.0);
-    }
-    if (v1.mod() == 0 || mode == FAST) {
+//     if(v0 * x0 > 0) {           //如果发现正在反方向走，则不再零速到点，防止车冲出去
+// //        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1.0, 0.0), QString("Limit v1").toLatin1());
+//         v1 = CVector(0.0, 0.0);
+//     }
+    // if (v1.mod() == 0 || mode == FAST) {
         rotangle = x0.dir();
-    }
-    else {
-        rotangle = v1.dir();
-    }
+    // }
+    // else {
+        // rotangle = v1.dir();
+    // }
     x0 = x0.rotate(-rotangle);
     v0 = v0.rotate(-rotangle);
     v1 = v1.rotate(-rotangle); //坐标系转换，转换到末速度方向为x轴的坐标系中
 
     double velFactorX = 1.0, velFactorY = 1.0;
-    velFactorX = (fabs(v1.x()) > 1e-8 ? 2.8 : 1.0);
-    velFactorY = (fabs(v1.y()) > 1e-8 ? 2.8 : 1.0);
-    if(v1.mod() > 0 && mode == FAST) {
-        v1.setVector(copysign(v1.mod(), v1.x()), 0);
-//        v_max = v1.mod();
-    }
+    // velFactorX = (fabs(v1.x()) > 1e-8 ? 2.8 : 1.0);
+    // velFactorY = (fabs(v1.y()) > 1e-8 ? 2.8 : 1.0);
+//     if(v1.mod() > 0 && mode == FAST) {
+//         v1.setVector(copysign(v1.mod(), v1.x()), 0);
+// //        v_max = v1.mod();
+//     }
 
     timeItor = 0;
     isX = 1;
@@ -262,27 +262,31 @@ void compute_motion_2d(CVector x0, CVector v0, CVector v1,
     compute_motion_1d(x0.y(), v0.y(), v1.y(), a_max, d_max, v_max, a_factor, velFactorY,
                       traj_accel_y, time_y, time_y_acc, time_y_dec, time_y_flat, MOVE_Y, mode);//两轴同样的最大速度、加速度独立考虑求两轴运动时间
 
-    if(v1.mod() > 1e-8 && mode == ACCURATE) {
-        if (time_x - time_y > FRAME_PERIOD) {
-            compute_motion_1d(x0.y(), v0.y(), 0, a_max, d_max, v_max, a_factor, velFactorX,
-                              traj_accel_y, time_y, time_y_acc, time_y_dec, time_y_flat, MOVE_X, mode);
-        } else if (time_y - time_x > FRAME_PERIOD) {
-            compute_motion_1d(x0.x(), v0.x(), 0, a_max, d_max, v_max, a_factor, velFactorY,
-                              traj_accel_x, time_x, time_x_acc, time_x_dec, time_x_flat, MOVE_X, mode);
-        }
-    }
+    // if(v1.mod() > 1e-8 && mode == ACCURATE) {
+    //     if (time_x - time_y > FRAME_PERIOD) {
+    //         compute_motion_1d(x0.y(), v0.y(), 0, a_max, d_max, v_max, a_factor, velFactorX,
+    //                           traj_accel_y, time_y, time_y_acc, time_y_dec, time_y_flat, MOVE_X, mode);
+    //     } else if (time_y - time_x > FRAME_PERIOD) {
+    //         compute_motion_1d(x0.x(), v0.x(), 0, a_max, d_max, v_max, a_factor, velFactorY,
+    //                           traj_accel_x, time_x, time_x_acc, time_x_dec, time_x_flat, MOVE_X, mode);
+    //     }
+    // }
     if(v1.mod() > 0 && DEBUG_NO_ZERO_VEL) {
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 0.0), QString("xVel: %1").arg(v0.x()).toLatin1());
+        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 0.0), QString("Vel: %1 %2").arg(v0.x()).arg(v0.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 20.0*10), QString("xVelFinal: %1").arg(v0.x() + traj_accel_x * FRAME_PERIOD).toLatin1());
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 40.0*10), QString("targetVel:  %1").arg(v1.mod()).toLatin1());
+        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 40.0*10), QString("targetVel:  %1 %2 %3").arg(v1.mod()).arg(v1.x()).arg(v1.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 60.0*10), QString("yVel: %1").arg(v0.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 80.0*10), QString("yVelFinal: %1").arg(v0.y() + traj_accel_y * FRAME_PERIOD).toLatin1());
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 100.0*10), QString("v_max: %1").arg(v_max).toLatin1());
+        // GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 100.0*10), QString("v_max: %1").arg(v_max).toLatin1());
+        auto S = CGeoPoint(-1000,-1000);
+        GDebugEngine::Instance()->gui_debug_line(S,S+v0.rotate(rotangle),COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_msg(S + v0.rotate(rotangle), "v0", COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_line(S, S + v1.rotate(rotangle), COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_msg(S + v1.rotate(rotangle), "v1", COLOR_BLUE);
     }
 
     traj_accel = CVector(traj_accel_x, traj_accel_y);
-    if (traj_accel.mod())
-        traj_accel = traj_accel.rotate(rotangle);
+    traj_accel = traj_accel.rotate(rotangle);
     if(time_x < 1e-5 || time_x > 50) time_x = 0;
     if(time_y < 1e-5 || time_y > 50) time_y = 0;
     if(time_x < time_y) {
@@ -362,6 +366,7 @@ void goto_point_omni( const PlayerPoseT& start,
     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, time_acc, time_dec, time_flat, mode);
+    // a = Utils::Polar2Vector(std::min(1.0,a.mod()),a.dir());
     factor_a = 1;
 
     double rotateFactor;
diff --git a/Core/src/MotionControl/ControlModel.cpp b/Core/src/MotionControl/ControlModel.cpp
index 9fc40ef..4da6480 100644
--- a/Core/src/MotionControl/ControlModel.cpp
+++ b/Core/src/MotionControl/ControlModel.cpp
@@ -68,7 +68,7 @@ void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT
 {
     _pathList.clear();
     double accel_factor = 1.5;
-    double angle_accel_factor = 6.5;
+    double angle_accel_factor = 5.5;
     if(IS_SIMULATION) {
         accel_factor = 1.0;
         angle_accel_factor = 1.0;
diff --git a/Core/src/Strategy/skill/SmartGotoPosition.cpp b/Core/src/Strategy/skill/SmartGotoPosition.cpp
index 76bfcd2..ac6c1ca 100644
--- a/Core/src/Strategy/skill/SmartGotoPosition.cpp
+++ b/Core/src/Strategy/skill/SmartGotoPosition.cpp
@@ -193,7 +193,6 @@ void CSmartGotoPositionV2::plan(const CVisionModule* pVision)
 
     //处理无效目标点:在禁区内、在车身内、在场地外
     validateFinalTarget(finalTargetPos, myPos, avoidLength, isGoalie, avoidBallCircle, obs, onlyPlanInCircle, planCircleCenter, planCircleRadius);
-    CVector velNew;
 
     validateStartPoint(myPos, avoidLength, isGoalie, obs);
     GDebugEngine::Instance()->gui_debug_x(finalTargetPos, COLOR_YELLOW);
@@ -211,7 +210,6 @@ void CSmartGotoPositionV2::plan(const CVisionModule* pVision)
 
     TaskT newTask(task());
     newTask.player.pos = finalTargetPos;
-    newTask.player.vel = velNew;
 
     if(self.Pos().dist(finalTargetPos) < startToRotateToTargetDirDist && needBreakRotate)
         newTask.player.flag &= (!PlayerStatus::BREAK_THROUGH);
diff --git a/Core/tactics/play/TestGetBall.lua b/Core/tactics/play/TestGetBall.lua
new file mode 100644
index 0000000..e43bad7
--- /dev/null
+++ b/Core/tactics/play/TestGetBall.lua
@@ -0,0 +1,32 @@
+local testPos = {CGeoPoint:new_local(1000, 1000), CGeoPoint:new_local(-1000, 1000), CGeoPoint:new_local(-1000, -1000),
+                 CGeoPoint:new_local(1000, -1000)}
+local vel = CVector:new_local(0, 0)
+local maxvel = 0
+local time = 120
+local DSS_FLAG = bit:_or(flag.allow_dss, flag.dodge_ball)
+
+local DIR = function()
+    return (player.pos('Assister') - ball.pos()):dir()
+end
+
+return {
+    firstState = "run1",
+
+    ["run1"] = {
+        switch = function()
+            local dist = (ball.pos() - player.pos("Assister")):mod()
+            debugEngine:gui_debug_msg(ball.pos()+Utils.Polar2Vector(300,math.pi/2),"Dist : " .. dist)
+        end,
+        Assister = task.touchKick(CGeoPoint(6000,0),nil,1000,true),
+        -- Assister = task.stop(),
+        match = "[A]"
+    },
+
+    name = "TestGetBall",
+    applicable = {
+        exp = "a",
+        a = true
+    },
+    attribute = "attack",
+    timeout = 99999
+}
diff --git a/Core/tactics/play/TestMyRun.lua b/Core/tactics/play/TestMyRun.lua
index 8f09306..5b4d968 100644
--- a/Core/tactics/play/TestMyRun.lua
+++ b/Core/tactics/play/TestMyRun.lua
@@ -1,8 +1,12 @@
-local testPos = {CGeoPoint:new_local(1000, 1000), CGeoPoint:new_local(-1000, 1000), CGeoPoint:new_local(-1000, -1000),
-                 CGeoPoint:new_local(1000, -1000)}
+local testPos = {
+    CGeoPoint:new_local(3000, 3100), 
+    CGeoPoint:new_local(-3000, 3100),
+    CGeoPoint:new_local(-3000,-3100),
+    CGeoPoint:new_local(3000,-3100)
+}
 local vel = CVector:new_local(0, 0)
 local maxvel = 0
-local time = 120
+local time = 1
 local DSS_FLAG = bit:_or(flag.allow_dss, flag.dodge_ball)
 
 local DIR = function()
@@ -11,12 +15,45 @@ end
 
 return {
     firstState = "run1",
-
     ["run1"] = {
         switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..2
+            end
+        end,
+        a = task.goCmuRush(testPos[1],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[1]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
+    },
+    ["run2"] = {
+        switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..3
+            end
+        end,
+        a = task.goCmuRush(testPos[2],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[2]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
+    },
+    ["run3"] = {
+        switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..4
+            end
+        end,
+        a = task.goCmuRush(testPos[3],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[3]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
+    },
+    ["run4"] = {
+        switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..1
+            end
         end,
-        Assister = task.goCmuRush(testPos[1], DIR, nil, DSS_FLAG),
-        match = "[A]"
+        a = task.goCmuRush(testPos[4],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[4]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
     },
 
     name = "TestMyRun",
diff --git a/Core/tactics/play/TestPassAndKick.lua b/Core/tactics/play/TestPassAndKick.lua
index ef516d7..2f01bd5 100644
--- a/Core/tactics/play/TestPassAndKick.lua
+++ b/Core/tactics/play/TestPassAndKick.lua
@@ -1,5 +1,5 @@
-local waitPos = ball.antiYPos(CGeoPoint:new_local(2600,1500))
-local waitPos2 = ball.antiYPos(CGeoPoint:new_local(1800,1500))
+local waitPos = ball.antiYPos(CGeoPoint:new_local(2800,2800))
+local waitPos2 = ball.antiYPos(CGeoPoint:new_local(2800,2800))
 local mode = true
 
 return {
@@ -20,9 +20,9 @@ firstState = "init",
             return "shoot"
         end
 	end,
-	Leader = task.touchKick(waitPos,false,3500,mode),
+	Leader = task.touchKick(waitPos,false,2500,mode),
     Assister = task.goCmuRush(waitPos2),
-	match = ""
+	match = "{LA}"
 },
 ["shoot"] = {
     switch = function()
@@ -32,7 +32,7 @@ firstState = "init",
     end,
     Leader = task.stop(),
     Assister = task.touchKick(pos.theirGoal(), false,6000,mode),
-    match = ""
+    match = "{LA}"
 },
 
 name = "TestPassAndKick",
diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
index 4aeb393..0154e64 100644
--- a/Core/tactics/skill/Touch.cpp
+++ b/Core/tactics/skill/Touch.cpp
@@ -1,8 +1,12 @@
+#include <fmt/core.h>
 #include "Touch.h"
 #include "parammanager.h"
 #include "WorldDefine.h"
 #include "VisionModule.h"
 #include "staticparams.h"
+#include "geomcalc.h"
+#include "BallSpeedModel.h"
+#include "CMmotion.h"
 namespace{
     const CGeoPoint THEIR_GOAL = CGeoPoint(PARAM::Field::PITCH_LENGTH/2,0);
     bool DEBUG_SWITCH;
@@ -19,41 +23,60 @@ void CTouch::plan(const CVisionModule* pVision){
     const int runner = task().executor;
     auto taskFlag = task().player.flag;
     const PlayerVisionT& me = pVision->ourPlayer(runner);
-    const auto mousePos = me.Pos() + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,me.Dir());
+    const auto mouseVec = Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER, me.Dir());
+    const auto mousePos = me.Pos() + mouseVec;
     const MobileVisionT& ball = pVision->ball();
     const double ballVelDir = ball.Vel().dir();
     const CGeoPoint& ballPos = ball.RawPos();
     const CGeoLine ballVelLine(ballPos, ballVelDir);
     const double ballVelMod = ball.Vel().mod();
-    const CGeoPoint projectionPos = ballVelLine.projection(mousePos);
-    const double toBallDist = (mousePos - ballPos).mod();
-    CGeoPoint targetPos;
-    double targetDir;
-    if(ballVelMod < 300){
-        targetDir = (target - ballPos).dir();
-        targetPos = ballPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,targetDir + PARAM::Math::PI);
+    const CGeoPoint projectionMousePos = ballVelLine.projection(mousePos);
+    const CGeoPoint projectionRobotPos = projectionMousePos + mouseVec*-1;
+    const auto mouse2Ball = ballPos - mousePos;
+    const double toBallDist = mouse2Ball.mod();
+    // predict ball pos
+    const auto ballStopPose = BallSpeedModel::instance()->poseForTime(9999);
+    const auto getBallPos = BallSpeedModel::instance()->poseForTime(1.0).Pos(); // TODO : replace with GetBallPos after skillutils
+    const CGeoSegment ballRunningSeg(ballPos, ballStopPose.Pos());
+    const auto me2segTime = predictedTimeWithRawVel(me, projectionRobotPos);
+    const auto ball2segTime = BallSpeedModel::Instance()->timeForDist(ballPos.dist(projectionMousePos));
+    const bool canWaitForBall = ballRunningSeg.IsPointOnLineOnSegment(projectionMousePos) && me2segTime < ball2segTime;
+    const auto predictPos = ballVelMod > 100 ? getBallPos : ballPos;
 
-    }else{
-        targetDir = useInter ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - mousePos).dir();
-        targetPos = projectionPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,targetDir + PARAM::Math::PI);
+    CGeoPoint targetRunPos;
+    double targetRunDir;
+    CVector targetRunVel;
+    if (!canWaitForBall){
+        targetRunDir = (target - predictPos).dir();
+        targetRunPos = predictPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER - 5, targetRunDir + PARAM::Math::PI);
+        targetRunVel = Utils::Polar2Vector(300, mouse2Ball.dir());
+    }
+    else{
+        targetRunDir = useInter ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - mousePos).dir();
+        targetRunPos = projectionMousePos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER-5,targetRunDir + PARAM::Math::PI);
     }
 
+    const auto me2target = targetRunPos - me.Pos();
+    const auto runTarget2kickTarget = target - targetRunPos;
     // add avoid ball flag
-    if(toBallDist>200){
+    auto angle_diff = angleDiff(me2target.dir(), runTarget2kickTarget.dir());
+    GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-800,-800),fmt::format("anglediff  {:.2f}",angle_diff/PARAM::Math::PI*180.0));
+    if (toBallDist > 150 && std::abs(angle_diff) > (100.0) / 180.0 * PARAM::Math::PI)
+    {
         taskFlag |= PlayerStatus::DODGE_BALL;
     }
 
-
     TaskT newTask(task());
-    newTask.player.pos = targetPos;
-    newTask.player.angle = targetDir;
+    newTask.player.pos = targetRunPos;
+    newTask.player.angle = targetRunDir;
+    newTask.player.vel = targetRunVel;
     newTask.player.flag = taskFlag;
-    // setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(newTask));
     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:{},me2SegT:{:3.1f},b2SegT:{:3.1f}", targetRunVel.mod(), me2segTime, ball2segTime));
     }
 
     _lastCycle = pVision->getCycle();
diff --git a/Core/tactics/skill/Touch.lua b/Core/tactics/skill/Touch.lua
index d22278c..e4b1704 100644
--- a/Core/tactics/skill/Touch.lua
+++ b/Core/tactics/skill/Touch.lua
@@ -11,7 +11,6 @@ function Touch(task)
 		task_param.executor = runner
 		task_param.player.pos = mpos
 		task_param.is_specify_ctrl_method = useInter
-		-- return CTouch(runner, mpos:x(), mpos:y(), useInter)
 		return skillapi:run("Touch", task_param)
 	end
 
diff --git a/Core/tt_params/play/ParamBallDec.lua b/Core/tt_params/play/ParamBallDec.lua
new file mode 100644
index 0000000..beb10da
--- /dev/null
+++ b/Core/tt_params/play/ParamBallDec.lua
@@ -0,0 +1,57 @@
+local count = 0
+local predict = {}
+local observation = {}
+local debug = function()
+    local __debug = function(info, color, symbol)
+        for k, v in pairs(info) do
+            local msg = symbol .. tostring(k) .. ":" .. v[2]
+            local pos = v[1]:Pos()
+            local vel = v[1]:Vel()
+            debugEngine:gui_debug_x(pos, color)
+            debugEngine:gui_debug_line(pos,pos+vel, color)
+            debugEngine:gui_debug_msg(pos, msg, color)
+        end
+    end
+    __debug(predict, param.GREEN, "P")
+    __debug(observation, param.ORANGE, "O")
+end
+return {
+    firstState = "reset",
+    ["reset"] = {
+        switch = function()
+            debug()
+            if bufcnt(ball.velMod() < 100, 30) then
+                pos = {}
+                msg = {}
+                return "wait"
+            end
+        end,
+        match = ""
+    },
+    ["wait"] = {
+        switch = function()
+            debug()
+            if bufcnt(ball.valid() and ball.velMod() > 500, 20) then
+                count = 0
+                for i = 1, 4 do
+                    local time = i/4.0
+                    local pose = ballModel:poseForTime(time)
+                    predict[i] = {pose, tostring(time)..'s'}
+                end
+                predict['FF'] = {ballModel:poseForTime(9999), 'FinalPos'}
+                return "test"
+            end
+        end,
+        match = ""
+    },
+    ["test"] = {
+        switch = function()
+            debug()
+            if bufcnt(true,param.frameRate) then
+                return "reset"
+            end
+        end,
+        match = ""
+    },
+    name = "ParamBallDec",
+}
\ No newline at end of file
diff --git a/share/geomcalc.cpp b/share/geomcalc.cpp
new file mode 100644
index 0000000..198c572
--- /dev/null
+++ b/share/geomcalc.cpp
@@ -0,0 +1,5 @@
+#include <cmath>
+#include "geomcalc.h"
+double angleDiff(double angle1, double angle2){
+    return std::atan2(std::sin(angle2 - angle1), std::cos(angle2 - angle1));
+}
\ No newline at end of file
diff --git a/share/geomcalc.h b/share/geomcalc.h
new file mode 100644
index 0000000..6b59afd
--- /dev/null
+++ b/share/geomcalc.h
@@ -0,0 +1,7 @@
+#ifndef __GEOMETRY_CALC_H__
+#define __GEOMETRY_CALC_H__
+
+// angle1 + diffResult = angle2 , diffResult is in [-pi, pi]
+double angleDiff(double angle1, double angle2);
+
+#endif // __GEOMETRY_CALC_H__
\ No newline at end of file
diff --git a/share/staticparams.h b/share/staticparams.h
index 822941f..2b20beb 100644
--- a/share/staticparams.h
+++ b/share/staticparams.h
@@ -26,16 +26,16 @@ namespace PARAM {
         const double BALL_DECAY = -0.8; // 阻力对球的加速度和速度成正比,单位为 /s
         /* Player */
         const double MAX_PLAYER_SIZE = 180;
-        const double PITCH_LENGTH = 9000;                 // 场地长
-        const double PITCH_WIDTH = 6000;                  // 场地宽
+        const double PITCH_LENGTH = 12000;                 // 场地长
+        const double PITCH_WIDTH = 9000;                  // 场地宽
         const double PITCH_MARGIN = 10;                   // 边界宽度
         const double CENTER_CIRCLE_R = 500;               // 中圈半径
 
         const double GOAL_POST_AVOID_LENGTH = 20;         //伸进场地内门柱的避障长度
         const double GOAL_POST_THICKNESS = 20;           //门柱宽度
         const bool   IF_USE_ELLIPSE = false;             // whether use ellipse penalty
-        const double PENALTY_AREA_WIDTH = 2000;           // rectangle禁区宽度
-        const double PENALTY_AREA_DEPTH = 1000;            // rectangle禁区深度
+        const double PENALTY_AREA_WIDTH = 2400;           // rectangle禁区宽度
+        const double PENALTY_AREA_DEPTH = 1200;            // rectangle禁区深度
         const double PENALTY_AREA_R = 800;               // ellipse penalty 两个圆弧
         const double PENALTY_AREA_L = 350;          // ellipse penalty 连接两个圆弧的线段
         const double PENALTY_L = 500;						//代替PENALTY_AREA_L
@@ -52,7 +52,7 @@ namespace PARAM {
         const double MaxDribbleDist = 500; // 最大带球距离, 0代表没有限制
     }
     namespace Vision{
-        const double FRAME_RATE = 61;
+        const double FRAME_RATE = 73;
     }
     namespace Latency{
         // 下面分开的部分是给server作仿真用的

From 4d43c23de6e5c735b9b498869fb21b949627d4e8 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 10 Apr 2024 20:14:31 +0800
Subject: [PATCH 06/30] [Controller] add timestamp for recv data

---
 Controller/src/serialobject.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/Controller/src/serialobject.cpp b/Controller/src/serialobject.cpp
index b617927..3c6fde2 100644
--- a/Controller/src/serialobject.cpp
+++ b/Controller/src/serialobject.cpp
@@ -1,4 +1,5 @@
 #include <QtSerialPort/QSerialPortInfo>
+#include <QDateTime>
 #include "serialobject.h"
 SerialObject::SerialObject(QObject *parent):QObject(parent),radioPacket(&serial){
     // add needed settings
@@ -111,7 +112,8 @@ void SerialObject::readData(){
             infrared = (quint8)data[3] & 0x40;
             flat     = (quint8)data[3] & 0x20;
             chip     = (quint8)data[3] & 0x10;
-            qDebug() << id << ' ' << infrared << ' ' << flat << ' ' << chip << ' ' << battery << ' ' << capacitance;
+            // get current time
+            qDebug() << QDateTime::currentDateTime() << id << ' ' << infrared << ' ' << flat << ' ' << chip << ' ' << battery << ' ' << capacitance;
         }
     }
     rx = "";

From 8ec1d12d4d7ce7670508029c01ba22e08281d6d9 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 10 Apr 2024 22:32:00 +0800
Subject: [PATCH 07/30] [Client & Core] cameramatrix autoupdate & update touch

---
 Client/plugins/sim/configwidget.cpp           |  3 +
 Client/src/globaldata.cpp                     | 68 ++++++++-----------
 Client/src/globaldata.h                       |  4 +-
 Client/src/vision/visionmodule.cpp            |  2 +-
 Core/src/Algorithm/BallSpeedModel.cpp         |  2 +-
 Core/src/Strategy/skill/SmartGotoPosition.cpp |  2 +-
 Core/src/Utils/misc_types.h                   |  1 +
 Core/tactics/play/TestGetBall.lua             |  2 +-
 Core/tactics/skill/Touch.cpp                  | 66 ++++++++++++------
 9 files changed, 84 insertions(+), 66 deletions(-)

diff --git a/Client/plugins/sim/configwidget.cpp b/Client/plugins/sim/configwidget.cpp
index caaeae3..3247a37 100644
--- a/Client/plugins/sim/configwidget.cpp
+++ b/Client/plugins/sim/configwidget.cpp
@@ -1,6 +1,7 @@
 #include "configwidget.h"
 namespace{
     auto ppm = ZSS::pm::instance();
+    auto zpm = ZSS::ZParamManager::instance();
 }
 //#define END_ENUM(parents, name) \
 //    parents->addChild(v_##name);
@@ -15,6 +16,8 @@ namespace{
 #define ADD_VALUE(parent,type,name,defaultvalue,namestring) \
     ppm->loadParam(v_##name,#parent"/"#name,defaultvalue);
 
+#define ADD_CLIENT_VALUE(parent, type, name, defaultvalue, namestring) \
+    zpm->loadParam(v_##name, #parent "/" #name, defaultvalue);
 
 ConfigWidget::ConfigWidget()
 {
diff --git a/Client/src/globaldata.cpp b/Client/src/globaldata.cpp
index 64f1d8f..656f191 100644
--- a/Client/src/globaldata.cpp
+++ b/Client/src/globaldata.cpp
@@ -98,48 +98,19 @@ void CGlobalData::saoConvertEdge() {
 void CGlobalData::setCameraMatrix(bool real) {
     zpm->loadParam(saoAction, "Alert/SaoAction", 0);
     if (real) {
-        /*
-        //NEED CHANGE IN CANADA
-        for (int i = 0; i < PARAM::CAMERA; i++) {
-            double x, y;
-            vpm->loadParam(x, "Camera" + QString::number(i) + "CenterX", 0);
-            vpm->loadParam(y, "Camera" + QString::number(i) + "CenterY", 0);
-            cameraMatrix[i].fillCenter(saoConvert(CGeoPoint(x, y)));
-        }
-
-        cameraMatrix[0].fillCenter(saoConvert(CGeoPoint(2410,1579)));//(-4500,2250);
-        cameraMatrix[1].fillCenter(saoConvert(CGeoPoint(2162,-1529)));//(-1500,2250);
-        cameraMatrix[2].fillCenter(saoConvert(CGeoPoint(-2195,-1404)));//(1500,2250);
-        cameraMatrix[3].fillCenter(saoConvert(CGeoPoint(-2310,1477)));//(4500,2250);
-        //        cameraMatrix[4].fillCenter(saoConvert(CGeoPoint(-4500,-2250)));
-        //        cameraMatrix[5].fillCenter(saoConvert(CGeoPoint(-1500,-2250)));
-        //        cameraMatrix[6].fillCenter(saoConvert(CGeoPoint(1500,-2250)));
-        //        cameraMatrix[7].fillCenter(saoConvert(CGeoPoint(4500,-2250)));
-        for (int i = 0; i < PARAM::CAMERA; i++) {
-            vpm->loadParam(cameraMatrix[i].leftedge.min,    "Camera" + QString::number(i) + "Leftmin",	cameraMatrix[i].campos.x());
-            vpm->loadParam(cameraMatrix[i].leftedge.max,    "Camera" + QString::number(i) + "Leftmax",	cameraMatrix[i].campos.x());
-            vpm->loadParam(cameraMatrix[i].rightedge.min,   "Camera" + QString::number(i) + "Rightmin",	cameraMatrix[i].campos.x());
-            vpm->loadParam(cameraMatrix[i].rightedge.max,   "Camera" + QString::number(i) + "Rightmax",	cameraMatrix[i].campos.x());
-            vpm->loadParam(cameraMatrix[i].upedge.min,      "Camera" + QString::number(i) + "Upmin",	cameraMatrix[i].campos.y());
-            vpm->loadParam(cameraMatrix[i].upedge.max,      "Camera" + QString::number(i) + "Upmax",	cameraMatrix[i].campos.y());
-            vpm->loadParam(cameraMatrix[i].downedge.min,    "Camera" + QString::number(i) + "Downmin",	cameraMatrix[i].campos.y());
-            vpm->loadParam(cameraMatrix[i].downedge.max,    "Camera" + QString::number(i) + "Downmax",	cameraMatrix[i].campos.y());
-        }
-        saoConvertEdge();
-        */
-        cameraMatrix[0].fillCenter(saoConvert(CGeoPoint(-3170, 0)));
-        cameraMatrix[1].fillCenter(saoConvert(CGeoPoint(3000, 0))); //(-4500,2250);
-
-        for (int i = 0; i < PARAM::CAMERA; i = i + 1) {
-            cameraMatrix[i].leftedge.min = cameraMatrix[i].campos.x() - 2800;
-            cameraMatrix[i].leftedge.max = cameraMatrix[i].campos.x() - 3400;
-            cameraMatrix[i].rightedge.min = cameraMatrix[i].campos.x() + 2800;
-            cameraMatrix[i].rightedge.max = cameraMatrix[i].campos.x() + 3400;
-            cameraMatrix[i].downedge.min = cameraMatrix[i].campos.y() - 4800;
-            cameraMatrix[i].downedge.max = cameraMatrix[i].campos.y() - 4800;
-            cameraMatrix[i].upedge.min = cameraMatrix[i].campos.y() + 4900;
-            cameraMatrix[i].upedge.max = cameraMatrix[i].campos.y() + 4900;
+        for (int i = 0; i < PARAM::CAMERA; i++)
+        {
+            cameraMatrix[i].fillCenter((CGeoPoint(0.0, 0.0)));
+            cameraMatrix[i].leftedge.min = 0;
+            cameraMatrix[i].leftedge.max = 0;
+            cameraMatrix[i].rightedge.min = 0;
+            cameraMatrix[i].rightedge.max = 0;
+            cameraMatrix[i].downedge.min = 0;
+            cameraMatrix[i].downedge.max = 0;
+            cameraMatrix[i].upedge.min = 0;
+            cameraMatrix[i].upedge.max = 0;
         }
+        CameraInit();
     } else {            //for Sim
         cameraMatrix[0].fillCenter(saoConvert(CGeoPoint(2410, 1579))); //(-4500,2250);
         cameraMatrix[1].fillCenter(saoConvert(CGeoPoint(2162, -1529))); //(-1500,2250);
@@ -167,3 +138,18 @@ void CGlobalData::setCameraMatrix(bool real) {
         }
     }
 }
+
+void CGlobalData::CameraInit()
+{
+    for (int i = 0; i < PARAM::CAMERA; i++)
+    {
+        cameraMatrix[i].leftedge.min = 0;
+        cameraMatrix[i].leftedge.max = 0;
+        cameraMatrix[i].rightedge.min = 0;
+        cameraMatrix[i].rightedge.max = 0;
+        cameraMatrix[i].downedge.min = 0;
+        cameraMatrix[i].downedge.max = 0;
+        cameraMatrix[i].upedge.min = 0;
+        cameraMatrix[i].upedge.max = 0;
+    }
+}
diff --git a/Client/src/globaldata.h b/Client/src/globaldata.h
index 3265450..f6531b6 100644
--- a/Client/src/globaldata.h
+++ b/Client/src/globaldata.h
@@ -58,7 +58,9 @@ class CGlobalData {
     bool ctrlC;
     QMutex ctrlCMutex;
 
-  private:
+    void CameraInit();
+
+private:
     CGeoPoint saoConvert(CGeoPoint);
     void  saoConvertEdge();
     int saoAction;
diff --git a/Client/src/vision/visionmodule.cpp b/Client/src/vision/visionmodule.cpp
index 19234c3..d509692 100644
--- a/Client/src/vision/visionmodule.cpp
+++ b/Client/src/vision/visionmodule.cpp
@@ -186,7 +186,7 @@ void CVisionModule::readRemoteSimData(){
  */
 bool CVisionModule::dealWithData() {
     counter++;
-    if (IF_EDGE_TEST) edgeTest();
+    edgeTest();
     DealBall::instance()->run();
     DealRobot::instance()->run();
     Maintain::instance()->run();
diff --git a/Core/src/Algorithm/BallSpeedModel.cpp b/Core/src/Algorithm/BallSpeedModel.cpp
index 4532ecb..7c00f89 100644
--- a/Core/src/Algorithm/BallSpeedModel.cpp
+++ b/Core/src/Algorithm/BallSpeedModel.cpp
@@ -33,7 +33,7 @@ std::tuple<double, CVector> CBallSpeedModel::predictForDist(const double dist){
 	this->update();
 	auto maxDist = _ballVel.mod() * _ballVel.mod() / (2 * _DEC);
 	if (dist > maxDist) {
-		return {-1, CVector(0,0)};
+		return {99999, CVector(0,0)};
 	}
 	auto v0 = _ballVel.mod();
 	auto v1 = sqrt(v0 * v0 - 2 * _DEC * dist);
diff --git a/Core/src/Strategy/skill/SmartGotoPosition.cpp b/Core/src/Strategy/skill/SmartGotoPosition.cpp
index ac6c1ca..0a13387 100644
--- a/Core/src/Strategy/skill/SmartGotoPosition.cpp
+++ b/Core/src/Strategy/skill/SmartGotoPosition.cpp
@@ -179,7 +179,7 @@ void CSmartGotoPositionV2::plan(const CVisionModule* pVision)
 
     obstacles obs(avoidLength);
     if(!(playerFlag & PlayerStatus::BREAK_THROUGH))
-        obs.addObs(pVision, task(), DRAW_OBSTACLES, OPP_AVOID_DIST, TEAMMATE_AVOID_DIST, BALL_AVOID_DIST);
+        obs.addObs(pVision, task(), DRAW_OBSTACLES, OPP_AVOID_DIST, TEAMMATE_AVOID_DIST, std::max(task().ball.avoid_dist, BALL_AVOID_DIST));
     else
         obs.addObs(pVision, task(), DRAW_OBSTACLES, PARAM::Vehicle::V2::PLAYER_SIZE + PARAM::Field::BALL_SIZE + 20, PARAM::Vehicle::V2::PLAYER_SIZE + PARAM::Field::BALL_SIZE + 20.0, PARAM::Field::BALL_SIZE);
 
diff --git a/Core/src/Utils/misc_types.h b/Core/src/Utils/misc_types.h
index dc638e7..038f6ac 100644
--- a/Core/src/Utils/misc_types.h
+++ b/Core/src/Utils/misc_types.h
@@ -120,6 +120,7 @@ struct stBallStatus{
 	int Sender;                 // 出球者号码（added by shizhy)
 	double angle;
 	bool front;
+	double avoid_dist = -1;
 };
 
 /// 任务结构
diff --git a/Core/tactics/play/TestGetBall.lua b/Core/tactics/play/TestGetBall.lua
index e43bad7..cccdb1c 100644
--- a/Core/tactics/play/TestGetBall.lua
+++ b/Core/tactics/play/TestGetBall.lua
@@ -17,7 +17,7 @@ return {
             local dist = (ball.pos() - player.pos("Assister")):mod()
             debugEngine:gui_debug_msg(ball.pos()+Utils.Polar2Vector(300,math.pi/2),"Dist : " .. dist)
         end,
-        Assister = task.touchKick(CGeoPoint(6000,0),nil,1000,true),
+        Assister = task.touchKick(CGeoPoint(6000,0),nil,6000,true),
         -- Assister = task.stop(),
         match = "[A]"
     },
diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
index 0154e64..04f7a9c 100644
--- a/Core/tactics/skill/Touch.cpp
+++ b/Core/tactics/skill/Touch.cpp
@@ -10,6 +10,10 @@
 namespace{
     const CGeoPoint THEIR_GOAL = CGeoPoint(PARAM::Field::PITCH_LENGTH/2,0);
     bool DEBUG_SWITCH;
+    template<typename T>
+    T clip(T x, T min, T max){
+        return std::max(min, std::min(max, x));
+    }
 }
 CTouch::CTouch(){
     ZSS::ZParamManager::instance()->loadParam(DEBUG_SWITCH,"Debug/Touch",false);
@@ -36,34 +40,54 @@ void CTouch::plan(const CVisionModule* pVision){
     const double toBallDist = mouse2Ball.mod();
     // predict ball pos
     const auto ballStopPose = BallSpeedModel::instance()->poseForTime(9999);
-    const auto getBallPos = BallSpeedModel::instance()->poseForTime(1.0).Pos(); // TODO : replace with GetBallPos after skillutils
+
+
+    // stupid version of getballPos
+    CGeoPoint bestPos = BallSpeedModel::instance()->poseForTime(1.0).Pos();
+    for(double dist = 0; dist < 3000; dist += 100){
+        auto pos = ballPos + Utils::Polar2Vector(dist, ballVelDir);
+        double t1 = predictedTimeWithRawVel(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(t1 < t2 || t1 < 0.1){
+            bestPos = pos;
+            break;
+        }
+    }
+    const auto getBallPos = bestPos; // TODO : replace with GetBallPos after skillutils
+
     const CGeoSegment ballRunningSeg(ballPos, ballStopPose.Pos());
     const auto me2segTime = predictedTimeWithRawVel(me, projectionRobotPos);
     const auto ball2segTime = BallSpeedModel::Instance()->timeForDist(ballPos.dist(projectionMousePos));
     const bool canWaitForBall = ballRunningSeg.IsPointOnLineOnSegment(projectionMousePos) && me2segTime < ball2segTime;
-    const auto predictPos = ballVelMod > 100 ? getBallPos : ballPos;
+    const auto predictPos = ballVelMod > 300 ? getBallPos : ballPos;
+    const double ballVel_ball2Target_ad = ballVelMod > 300 ? angleDiff(ballVelDir, (target - ballPos).dir()) : 180;
+    const bool angleCanTouch = std::abs(ballVel_ball2Target_ad) > 100 / 180.0 * PARAM::Math::PI;
 
-    CGeoPoint targetRunPos;
-    double targetRunDir;
-    CVector targetRunVel;
-    if (!canWaitForBall){
-        targetRunDir = (target - predictPos).dir();
-        targetRunPos = predictPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER - 5, targetRunDir + PARAM::Math::PI);
-        targetRunVel = Utils::Polar2Vector(300, mouse2Ball.dir());
-    }
-    else{
-        targetRunDir = useInter ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - mousePos).dir();
-        targetRunPos = projectionMousePos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER-5,targetRunDir + PARAM::Math::PI);
-    }
+    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);
 
+    // add avoid ball flag
     const auto me2target = targetRunPos - me.Pos();
+    const auto me2TargetSeg = CGeoSegment(me.Pos(), targetRunPos);  
     const auto runTarget2kickTarget = target - targetRunPos;
-    // add avoid ball flag
-    auto angle_diff = angleDiff(me2target.dir(), runTarget2kickTarget.dir());
-    GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-800,-800),fmt::format("anglediff  {:.2f}",angle_diff/PARAM::Math::PI*180.0));
-    if (toBallDist > 150 && std::abs(angle_diff) > (100.0) / 180.0 * PARAM::Math::PI)
-    {
+
+    const auto BALL_STATIC = ballVelMod < 300;
+    const auto runTargetDiff = angleDiff(me2target.dir(), runTarget2kickTarget.dir());
+    const auto needAvoidStatic = BALL_STATIC && runTargetDiff;
+    const auto avoidDistStatic = needAvoidStatic ? 30.0 : 0.0;
+
+    const auto diff4avoid_ball = std::abs(angleDiff(me2target.dir(), (targetMousePos - ballPos).dir()));
+    const auto needAvoidDynamic = !BALL_STATIC && me2TargetSeg.IsPointOnLineOnSegment(ballPos) && diff4avoid_ball < 90 / 180.0 * PARAM::Math::PI;
+    const auto avoidDistDynamic = needAvoidDynamic ? 3*clip(90-diff4avoid_ball *180.0 / PARAM::Math::PI,0.0,90.0) : 0;
+
+    double avoid_dist = 0;
+    if (toBallDist > 120 && (needAvoidStatic || needAvoidDynamic)){
         taskFlag |= PlayerStatus::DODGE_BALL;
+        avoid_dist = std::max(avoidDistStatic, avoidDistDynamic);
     }
 
     TaskT newTask(task());
@@ -71,12 +95,14 @@ void CTouch::plan(const CVisionModule* pVision){
     newTask.player.angle = targetRunDir;
     newTask.player.vel = targetRunVel;
     newTask.player.flag = taskFlag;
+    newTask.ball.avoid_dist = 300;
     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:{},me2SegT:{:3.1f},b2SegT:{:3.1f}", targetRunVel.mod(), me2segTime, ball2segTime));
+        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));
     }
 
     _lastCycle = pVision->getCycle();

From d5c0c49dc962b21d4499effc1c5621894409a101 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Thu, 11 Apr 2024 19:51:19 +0800
Subject: [PATCH 08/30] [Core] add MoveTime test

---
 .gitignore                                    |  5 +
 Core/tt_params/play/ParamPredictTime.lua      | 74 ++++++++++++++
 ZBin/lua_scripts/StartZeus.lua                |  1 +
 .../rocos/dataset/MoveTimeDataset.py          | 91 +++++++++++++++++
 ZBin/py_playground/rocos/dataset/__init__.py  |  3 +
 ZBin/py_playground/rocos/utils/Geom.py        | 52 ++++++++++
 ZBin/py_playground/trainMoveTime.py           | 79 +++++++++++++++
 ZBin/py_playground/vizMoveTime.py             | 98 +++++++++++++++++++
 8 files changed, 403 insertions(+)
 create mode 100644 Core/tt_params/play/ParamPredictTime.lua
 create mode 100644 ZBin/py_playground/rocos/dataset/MoveTimeDataset.py
 create mode 100644 ZBin/py_playground/rocos/dataset/__init__.py
 create mode 100644 ZBin/py_playground/rocos/utils/Geom.py
 create mode 100644 ZBin/py_playground/trainMoveTime.py
 create mode 100644 ZBin/py_playground/vizMoveTime.py

diff --git a/.gitignore b/.gitignore
index bac2220..c885792 100644
--- a/.gitignore
+++ b/.gitignore
@@ -127,6 +127,11 @@ ZBin/ballSpeedLog.txt
 ZBin/Core
 ZBin/Controller
 # python
+ZBin/__data
+ZBin/*/model
+ZBin/*/runs/
+ZBin/*/tbox
+ZBin/*/*.pth
 ZBin/__pycache__
 ZBin/*.py[cod]
 ZBin/*$py.class
diff --git a/Core/tt_params/play/ParamPredictTime.lua b/Core/tt_params/play/ParamPredictTime.lua
new file mode 100644
index 0000000..f4d4863
--- /dev/null
+++ b/Core/tt_params/play/ParamPredictTime.lua
@@ -0,0 +1,74 @@
+local runX = 0
+local runY = 0
+local runPos = function()
+    return CGeoPoint(runX, runY)
+end
+local getRand = function()
+    local r = 2*(math.random()-0.5) -- [-1,1]
+    return r * 4000
+end
+
+local getDataStr = function(v)
+    local data = {
+        os.clock(),
+        v:X(),v:Y(),v:VelX(),v:VelY(),
+        v:RawRotVel(),v:RotVel(),
+        -- v:RawPos():x(),v:RawPos():y(),
+        v:RawVel():x(),v:RawVel():y(),
+        runX,runY,
+    }
+    local str = ""
+    for i,value in pairs(data) do
+        str = str .. string.format("%.3f;",value)
+    end
+    return str .. '\n'
+end
+
+local recFile = nil
+
+return {
+    firstState = "reset",
+    ["reset"] = {
+        switch = function()
+            if recFile ~= nil then
+                recFile:close()
+                recFile = nil
+            end
+            runX = getRand()
+            runY = getRand()
+            return "randRun"
+        end,
+        Leader = task.stop(),
+        match = "[L]"
+    },
+    ["randRun"] = {
+        switch = function()
+            if bufcnt(true, 100) then
+                runX = getRand()
+                runY = getRand()
+                local fileName = "__data/robot_run/" .. os.date("%m%d%H%M%S") .. os.clock()
+                recFile = io.open(fileName, 'w')
+                recFile:write(getDataStr(player.instance("Leader")))
+                return "testData"
+            end
+        end,
+        Leader = task.goCmuRush(runPos,0),
+        match = "{L}"
+    },
+    ["testData"] = {
+        switch = function()
+            if player.toTargetDist("Leader") > 99999 then
+                return "reset"
+            end
+            local data = getDataStr(player.instance("Leader"))
+            debugEngine:gui_debug_msg(CGeoPoint(0,0),data)
+            recFile:write(getDataStr(player.instance("Leader")))
+            if bufcnt(player.toTargetDist("Leader") < 10,10) then
+                return "reset"
+            end
+        end,
+        Leader = task.goCmuRush(runPos,0),
+        match = "{L}"
+    },
+    name = "ParamPredictTime",
+}
\ No newline at end of file
diff --git a/ZBin/lua_scripts/StartZeus.lua b/ZBin/lua_scripts/StartZeus.lua
index 18b6b56..281df73 100644
--- a/ZBin/lua_scripts/StartZeus.lua
+++ b/ZBin/lua_scripts/StartZeus.lua
@@ -1,3 +1,4 @@
+math.randomseed(os.time())
 package.path = package.path .. ";./lua_scripts/?.lua"
 
 -- require("Judge")
diff --git a/ZBin/py_playground/rocos/dataset/MoveTimeDataset.py b/ZBin/py_playground/rocos/dataset/MoveTimeDataset.py
new file mode 100644
index 0000000..4072431
--- /dev/null
+++ b/ZBin/py_playground/rocos/dataset/MoveTimeDataset.py
@@ -0,0 +1,91 @@
+import os
+import numpy as np
+from rocos.utils.Geom import Vec
+
+# get all file with walk
+def get_all_files(path):
+    all_files = []
+    for root, dirs, files in os.walk(path):
+        for file in files:
+            all_files.append(os.path.join(root, file))
+    return all_files
+
+def parse_file(filename):
+    datas = []
+    with open(filename, 'r') as f:
+        lines = f.readlines()
+    for line in lines:
+        data = line[:-2].split(';')
+        data = [float(x) for x in data]
+        datas.append(data)
+    return datas
+
+
+# 0   :  os.clock(),
+# 1   :  v:X(),
+# 2   :  v:Y(),
+# 3   :  v:VelX(),
+# 4   :  v:VelY(),
+# 5   :  v:RawRotVel(),
+# 6   :  v:RotVel(),
+# 7   :  v:RawVel():x(),
+# 8   :  v:RawVel():y(),
+# 9   :  runX,
+# 10  :  runY,
+
+class State:
+    FRAME_RATE = 62.5
+    def __init__(self,data):
+        self.time = data[0]
+        self.pos = Vec(data[1], data[2])
+        self.vel = Vec(data[3], data[4])
+        self.rawVel = Vec(data[7], data[8])
+        self.target = Vec(data[9], data[10])
+
+def parse_data(data, skip = 10):
+    def get_train_data(startState, endState):
+        time = endState.time - startState.time
+        target = endState.pos - startState.pos
+        vel = startState.vel
+        rawVel = startState.rawVel
+        # rotate
+        rotAngle = target.dir
+        target = target.rotate(-rotAngle) / 3000
+        vel = vel.rotate(-rotAngle) / 3000
+        rawVel = rawVel.rotate(-rotAngle) / 3000
+        # return [target[0], vel.x, vel.y, rawVel.x, rawVel.y], [time]
+        # print("{}\t{}\t{}\t{}".format(target[0], rawVel.x, rawVel.y, time))
+        return [target[0], rawVel.x, rawVel.y], [time/5.0]
+    frame = len(data)
+    endData = data[-1]
+    endData[0] = (frame-1)/State.FRAME_RATE
+    endState = State(endData)
+
+    trainDatas = []
+    res = []
+    for i in range(0, frame-1, skip):
+        data[i][0] = i/State.FRAME_RATE
+        state = State(data[i])
+        trainData, time = get_train_data(state, endState)
+        trainDatas.append(trainData)
+        res.append(time)
+    return trainDatas, res
+
+import torch
+from torch.utils.data import Dataset
+class MoveTimeDataset(Dataset):
+    def __init__(self, path, skipFrame = 10):
+        self.data = []
+        self.output = []
+        files = get_all_files(path)
+        for file in files:
+            all_data = parse_file(file)
+            data, output = parse_data(all_data, skipFrame)
+            self.data.extend(data)
+            self.output.extend(output)
+    def __len__(self):
+        return len(self.data)
+    def __getitem__(self, idx):
+        input = torch.tensor(self.data[idx], dtype=torch.float32)
+        output = torch.tensor(self.output[idx], dtype=torch.float32)
+        return input, output
diff --git a/ZBin/py_playground/rocos/dataset/__init__.py b/ZBin/py_playground/rocos/dataset/__init__.py
new file mode 100644
index 0000000..00c221e
--- /dev/null
+++ b/ZBin/py_playground/rocos/dataset/__init__.py
@@ -0,0 +1,3 @@
+import sys, os
+sys.path.append(os.path.dirname(__file__))
+from MoveTimeDataset import MoveTimeDataset
\ No newline at end of file
diff --git a/ZBin/py_playground/rocos/utils/Geom.py b/ZBin/py_playground/rocos/utils/Geom.py
new file mode 100644
index 0000000..aeb7622
--- /dev/null
+++ b/ZBin/py_playground/rocos/utils/Geom.py
@@ -0,0 +1,52 @@
+import numpy as np
+class Vec:
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+    def __add__(self, other):
+        return Vec(self.x + other.x, self.y + other.y)
+    def __sub__(self, other):
+        return Vec(self.x - other.x, self.y - other.y)
+    def __mul__(self, other):
+        return Vec(self.x * other, self.y * other)
+    def __truediv__(self, other):
+        return Vec(self.x / other, self.y / other)
+    def __str__(self):
+        return f"({self.x}, {self.y})"
+    def __repr__(self):
+        return f"({self.x}, {self.y})"
+    def __iter__(self):
+        return iter([self.x, self.y])
+    def __getitem__(self, index):
+        return [self.x, self.y][index]
+    @property
+    def mod(self):
+        return np.sqrt(self.x**2 + self.y**2)
+    @mod.setter
+    def mod(self, value):
+        self.x = self.x / self.mod * value
+        self.y = self.y / self.mod * value
+    @property
+    def dir(self):
+        return np.arctan2(self.y, self.x)
+    @dir.setter
+    def dir(self, value):
+        _mod = self.mod
+        self.x = np.cos(value) * _mod
+        self.y = np.sin(value) * _mod
+    def _rotate(self, radians):
+        x, y = self.x, self.y
+        self.x = x * np.cos(radians) - y * np.sin(radians)
+        self.y = x * np.sin(radians) + y * np.cos(radians)
+    def rotate(self, radians):
+        newVec = Vec(self.x, self.y)
+        newVec._rotate(radians)
+        return newVec
+    
+if __name__ == "__main__":
+    v = Vec(1,1)
+    print(v)
+    v._rotate(np.pi/2)
+    print(v)
+    v._rotate(-v.dir)
+    print(v)
\ No newline at end of file
diff --git a/ZBin/py_playground/trainMoveTime.py b/ZBin/py_playground/trainMoveTime.py
new file mode 100644
index 0000000..2cd0af1
--- /dev/null
+++ b/ZBin/py_playground/trainMoveTime.py
@@ -0,0 +1,79 @@
+from rocos.dataset import MoveTimeDataset
+from torch.utils.data import DataLoader
+import torch
+from torch import nn
+from torch.utils.tensorboard import SummaryWriter
+writer = SummaryWriter()
+import matplotlib.pyplot as plt
+device = (
+    "cuda"
+    if torch.cuda.is_available()
+    else "mps"
+    if torch.backends.mps.is_available()
+    else "cpu"
+)
+print(f"Using {device} device")
+class MyModel(nn.Module):
+    def __init__(self, n_feature, n_hidden, n_output, p=0.1):
+        super().__init__()
+        self.linear_relu_stack = nn.Sequential(
+            nn.Linear(n_feature, n_hidden),
+            nn.LeakyReLU(),
+            nn.Dropout(p=p),
+            nn.Linear(n_hidden, n_hidden),
+            nn.LeakyReLU(),
+            nn.Dropout(p=p),
+            nn.Linear(n_hidden, n_output),
+        )
+    
+    def forward(self, x):
+        x = self.linear_relu_stack(x)
+        return x
+
+EPOCH = 10000
+
+if __name__ == '__main__':
+    from datetime import datetime
+    SYMBOL = str(datetime.now())
+    train_dataset = MoveTimeDataset('../__data/robot_run/train')
+    test_dataset = MoveTimeDataset('../__data/robot_run/test', skipFrame=30)
+    print("train dataset length : ", len(train_dataset))
+    print("test dataset length : ", len(test_dataset))
+
+    train_dataloader = DataLoader(train_dataset,batch_size=32, shuffle=True)
+    test_dataloader = DataLoader(test_dataset,batch_size=32, shuffle=True)
+
+    model = MyModel(3, 32, 1).to(device)
+    print(model)
+    optimizer = torch.optim.Adam(model.parameters(), lr=1e-3)
+    loss_func = torch.nn.MSELoss()
+
+    losses = []
+    for epoch in range(EPOCH):
+        sum_loss = 0.0
+        for i, (x,y) in enumerate(train_dataloader):
+            x, y = x.to(device), y.to(device)
+            output = model(x)
+            loss = loss_func(output, y)
+            optimizer.zero_grad()
+            loss.backward()
+            optimizer.step()
+            sum_loss += loss.data.cpu().numpy()
+        sum_loss = sum_loss / len(train_dataset)
+        writer.add_scalar("Loss/train", sum_loss, epoch)
+        if epoch % 20 == 0:
+            print(f"Train Loss : {sum_loss}")
+            with torch.no_grad():
+                sum_loss = 0.0
+                for i, (x,y) in enumerate(test_dataloader):
+                    x, y = x.to(device), y.to(device)
+                    output = model(x)
+                    loss = loss_func(output, y)
+                    sum_loss += loss.data.cpu().numpy()
+                sum_loss = sum_loss / len(test_dataset)
+                writer.add_scalar("Loss/test", sum_loss, epoch)
+            print(f"Test Loss : {sum_loss}")
+        if epoch % 100 == 0:
+            torch.save(model.state_dict(), "model/" + f'{SYMBOL}_model_{epoch//100}.pth')
+    plt.plot(losses)
+    plt.show()
\ No newline at end of file
diff --git a/ZBin/py_playground/vizMoveTime.py b/ZBin/py_playground/vizMoveTime.py
new file mode 100644
index 0000000..6fff927
--- /dev/null
+++ b/ZBin/py_playground/vizMoveTime.py
@@ -0,0 +1,98 @@
+import sys, signal
+signal.signal(signal.SIGINT, signal.SIG_DFL)
+import itertools as it
+import numpy as np
+import torch
+from PySide6.QtCore import Qt, QObject, Slot, Signal
+from PySide6.QtWidgets import (
+    QApplication,
+    QMainWindow,
+    QSplitter,
+    QCheckBox,
+)
+import pyqtgraph as pg
+from tbox.components.CustomTree import CustomTreeWidget, NodeExpand
+from tbox.components.Params import (
+    ParamBool,
+    ParamNumber,
+)
+from trainMoveTime import MyModel
+
+MODEL = MyModel(3,32,1,0.0)
+MODEL.load_state_dict(torch.load("MoveTime_3.pth"))
+
+# use for any class that has ParamBase as attribute
+class InputExpand(NodeExpand):
+    def __init__(self, name, param, callback):
+        super().__init__()
+        self.param = param
+        self.checkbox = ParamBool(name, False, callback=lambda v : callback())
+        self.value = ParamNumber("", 0, param[0],param[1],param[2], callback=lambda v: callback())
+    def getWidgets(self, node, columns):
+        return [self.checkbox.widget, self.value.widget]
+    def getValue(self):
+        if self.checkbox.value:
+            step = int((self.param[1] - self.param[0]) // self.param[2])
+            return np.linspace(self.param[0],self.param[1],step)
+        return [self.value.value]
+
+def convert2TreeData(config, callback):
+    data = {}
+    for k,v in config.items():
+        data[k] = InputExpand(k,v,callback)
+    return data
+
+class MainWindow(QMainWindow):
+    def __init__(self):
+        super().__init__()
+
+        self.setWindowTitle("ModelViz")
+
+        self.plt = pg.PlotWidget()
+        self.plt.showGrid(x=True, y=True, alpha = 0.3)
+        self.plt.addLegend()
+        self.plt.setBackground('transparent')
+
+        self.inputs = convert2TreeData({
+            "Dist" : [0,1,0.002],
+            # "VelX" : [-1,1,0.05],
+            # "VelY" : [-1,1,0.05],
+            "RawVX" : [-1,1,0.01],
+            "RawVY" : [-1,1,0.01],
+        }, self.updatePlot)
+
+        self.tree = CustomTreeWidget(data=self.inputs,titles=["Property","Value"])
+
+        self.splitter = QSplitter(Qt.Vertical)
+        self.splitter.addWidget(self.plt)
+        self.splitter.addWidget(self.tree)
+        
+        self.setCentralWidget(self.splitter)
+
+    @torch.no_grad
+    def updatePlot(self):
+        expandAxis = 0
+        modelInput = []
+        for i,(_,input) in enumerate(self.inputs.items()):
+            values = input.getValue()
+            if len(values) > 1:
+                expandAxis = i
+            modelInput.append(values)
+        all_input = list(it.product(*modelInput))
+        all_input = torch.Tensor(all_input)
+        output = MODEL(all_input)
+        # print(all_input,output)
+        self.plt.clear()
+        # self.plt.getViewBox().enableAutoRange(True)
+        self.plt.plot(modelInput[expandAxis],output.numpy().reshape(-1))
+
+
+if __name__ == "__main__":
+    import qdarktheme
+    qdarktheme.enable_hi_dpi()
+    app = QApplication(sys.argv)
+    qdarktheme.setup_theme("auto")
+    window = MainWindow()
+    window.resize(1200,800)
+    window.show()
+    app.exec()
\ No newline at end of file

From 2eb4c9def6318c1600102783861300f54063bf84 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Thu, 11 Apr 2024 23:27:50 +0800
Subject: [PATCH 09/30] [Skill] add CircleRun

---
 Core/tactics/skill/CircleRun.cpp | 46 ++++++++++++++++++++++++++++++++
 Core/tactics/skill/CircleRun.h   | 14 ++++++++++
 Core/tactics/skill/CircleRun.lua | 22 +++++++++++++++
 3 files changed, 82 insertions(+)
 create mode 100644 Core/tactics/skill/CircleRun.cpp
 create mode 100644 Core/tactics/skill/CircleRun.h
 create mode 100644 Core/tactics/skill/CircleRun.lua

diff --git a/Core/tactics/skill/CircleRun.cpp b/Core/tactics/skill/CircleRun.cpp
new file mode 100644
index 0000000..96b50b9
--- /dev/null
+++ b/Core/tactics/skill/CircleRun.cpp
@@ -0,0 +1,46 @@
+#include <fmt/core.h>
+#include <algorithm>
+#include "VisionModule.h"
+#include "DribbleStatus.h"
+#include <CommandFactory.h>
+#include "RobotCapability.h"
+#include "CircleRun.h"
+namespace {
+    const double MAX_ACC = 6000;//mm/s^2
+    const double MAX_ROT_ACC = 50;
+}
+void CCircleRun::plan(const CVisionModule* pVision){
+    return ;
+}
+CPlayerCommand* CCircleRun::execute(const CVisionModule* pVision){
+    const int vecNumber = task().executor;
+    const CGeoPoint center = task().player.pos; // rotate center in robot coordinate
+    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 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 CVector targetVel = Utils::Polar2Vector(targetVelMod, targetVelDir);
+    
+    const CVector velDiff = targetVel - meVel;
+
+    const CVector dv = Utils::Polar2Vector(std::min(velDiff.mod(), MAX_ACC/PARAM::Vision::FRAME_RATE), velDiff.dir());
+
+    const CVector localVel = meVel + dv;
+
+    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);
+    return CmdFactory::Instance()->newCommand(CPlayerSpeedV2(vecNumber, localVel.x(), localVel.y(), rotVel, dribblePower));
+}
\ No newline at end of file
diff --git a/Core/tactics/skill/CircleRun.h b/Core/tactics/skill/CircleRun.h
new file mode 100644
index 0000000..271077b
--- /dev/null
+++ b/Core/tactics/skill/CircleRun.h
@@ -0,0 +1,14 @@
+#pragma once
+#include "skill_registry.h"
+
+class CCircleRun : public CPlayerTask{
+public:
+    CCircleRun() = default;
+    virtual void plan(const CVisionModule* pVision);
+	virtual CPlayerCommand* execute(const CVisionModule* pVision);
+    virtual void toStream(std::ostream& os) const { os << "CircleRun"; }
+private:
+    int _lastCycle;
+};
+
+REGISTER_SKILL(CircleRun, CCircleRun);
diff --git a/Core/tactics/skill/CircleRun.lua b/Core/tactics/skill/CircleRun.lua
new file mode 100644
index 0000000..9f5c370
--- /dev/null
+++ b/Core/tactics/skill/CircleRun.lua
@@ -0,0 +1,22 @@
+function CircleRun(task)
+	matchPos = function()
+		return ball.pos()
+	end
+
+	execute = function(runner)
+		task_param = TaskT:new_local()
+		task_param.executor = runner
+		task_param.player.pos = CGeoPoint(100,100)
+        task_param.player.rotvel = 4
+		return skillapi:run("CircleRun", task_param)
+	end
+
+	return execute, matchPos
+end
+
+gSkillTable.CreateSkill{
+	name = "CircleRun",
+	execute = function (self)
+		print("This is in skill"..self.name)
+	end
+}
\ No newline at end of file

From bac2ea9f8f5803640d91891fc2ba82d8541bd037 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Fri, 12 Apr 2024 16:39:21 +0800
Subject: [PATCH 10/30] [Core] clear worlddefine & add rawpos

---
 Core/src/Algorithm/BallSpeedModel.cpp         |  4 +-
 Core/src/Algorithm/BallSpeedModel.h           |  2 +-
 Core/src/Algorithm/BallStatus.cpp             |  2 +-
 Core/src/Algorithm/ContactChecker.cpp         |  2 +-
 Core/src/Algorithm/ContactChecker.h           |  4 +-
 Core/src/LuaModule/ballspeedmodel.pkg         |  2 +-
 Core/src/LuaModule/visionmodule.pkg           |  4 +-
 Core/src/LuaModule/worlddefine.pkg            | 15 ++-----
 .../MotionControl/DynamicsSafetySearch.cpp    |  2 +-
 Core/src/OtherLibs/cmu/obstacle.cpp           |  4 +-
 Core/src/PathPlan/ObstacleNew.cpp             |  4 +-
 Core/src/Vision/BallPredictor.cpp             |  4 +-
 Core/src/Vision/BallPredictor.h               |  8 ++--
 Core/src/Vision/CollisionSimulator.h          |  4 +-
 Core/src/Vision/RobotPredictor.cpp            |  8 ++--
 Core/src/Vision/RobotPredictor.h              | 10 ++---
 Core/src/Vision/VisionModule.cpp              |  2 +-
 Core/src/Vision/VisionModule.h                |  4 +-
 Core/src/WorldModel/WorldDefine.h             | 41 ++++++-------------
 Core/src/WorldModel/WorldModel.h              |  2 +-
 Core/tactics/play/TestGetBall.lua             |  4 ++
 Core/tactics/skill/Touch.cpp                  |  2 +-
 ZBin/lua_scripts/worldmodel/player.lua        |  4 ++
 23 files changed, 62 insertions(+), 76 deletions(-)

diff --git a/Core/src/Algorithm/BallSpeedModel.cpp b/Core/src/Algorithm/BallSpeedModel.cpp
index 7c00f89..95a0b54 100644
--- a/Core/src/Algorithm/BallSpeedModel.cpp
+++ b/Core/src/Algorithm/BallSpeedModel.cpp
@@ -40,9 +40,9 @@ std::tuple<double, CVector> CBallSpeedModel::predictForDist(const double dist){
 	auto time = (v0 - v1) / _DEC;
 	return {time, Utils::Polar2Vector(v1, _ballVel.dir())};
 }
-MobileVisionT CBallSpeedModel::poseForTime(const double time){
+ObjectPoseT CBallSpeedModel::poseForTime(const double time){
 	this->update();
-	MobileVisionT predictPose;
+	ObjectPoseT predictPose;
 	auto v0 = _ballVel.mod();
 	auto v1 = std::max(v0 - _DEC * time, 0.0);
 	auto runtime = std::min(v0 / _DEC, time);
diff --git a/Core/src/Algorithm/BallSpeedModel.h b/Core/src/Algorithm/BallSpeedModel.h
index 9a33ce3..7b9a191 100644
--- a/Core/src/Algorithm/BallSpeedModel.h
+++ b/Core/src/Algorithm/BallSpeedModel.h
@@ -14,7 +14,7 @@ class CBallSpeedModel
 	std::tuple<double,CVector> predictForDist(const double dist);	// 计算若干距离后的速度
 	double timeForDist(const double dist);      //计算球运动若干距离的时间
 	CVector speedForDist(const double dist);	// 计算球运动若干距离的速度
-	MobileVisionT poseForTime(const double time); // 计算若干帧以后的绝对位置
+	ObjectPoseT poseForTime(const double time); // 计算若干帧以后的绝对位置
 private:
 	void update();
 	CGeoPoint _ballPos;
diff --git a/Core/src/Algorithm/BallStatus.cpp b/Core/src/Algorithm/BallStatus.cpp
index 0b86cf6..5e6bbad 100644
--- a/Core/src/Algorithm/BallStatus.cpp
+++ b/Core/src/Algorithm/BallStatus.cpp
@@ -55,7 +55,7 @@ void CBallStatus::UpdateBallStatus(const CVisionModule* pVision)
 
 void CBallStatus::UpdateBallMoving(const CVisionModule* pVision)
 {
-	const MobileVisionT& ball = pVision->ball(); // 球	
+	const ObjectPoseT& ball = pVision->ball(); // 球	
 	isNearPlayer = false;
 	for (int i=0; i<PARAM::Field::MAX_PLAYER*2; i++){
 		if (pVision->allPlayer(i).Valid() && pVision->allPlayer(i).Pos().dist(ball.Pos())< PARAM::Field::MAX_PLAYER_SIZE/2+5){
diff --git a/Core/src/Algorithm/ContactChecker.cpp b/Core/src/Algorithm/ContactChecker.cpp
index f8950d2..4ad734a 100644
--- a/Core/src/Algorithm/ContactChecker.cpp
+++ b/Core/src/Algorithm/ContactChecker.cpp
@@ -33,7 +33,7 @@ bool isPointOutField(CGeoPoint pos)
 }
 void ContactChecker::OutFieldJudge(const CVisionModule* pVision)
 {
-	MobileVisionT last_ball=pVision->ball(pVision->getCycle()-4);
+	ObjectPoseT last_ball=pVision->ball(pVision->getCycle()-4);
 	if (pVision->ball().Valid()&&isPointOutField(pVision->ball().Pos())){
 		_isBallOutField=true;
 	}
diff --git a/Core/src/Algorithm/ContactChecker.h b/Core/src/Algorithm/ContactChecker.h
index 0ccc531..a338ac5 100644
--- a/Core/src/Algorithm/ContactChecker.h
+++ b/Core/src/Algorithm/ContactChecker.h
@@ -23,8 +23,8 @@ class ContactChecker {
     std::vector<PlayerVisionT> AllPlayer;
     Last_Contact _last_contact;
 
-    MobileVisionT _ball;
-    MobileVisionT _lastball;
+    ObjectPoseT _ball;
+    ObjectPoseT _lastball;
     CVector ball_direction_before;
     CVector ball_direction_after;
     double speed_before ;
diff --git a/Core/src/LuaModule/ballspeedmodel.pkg b/Core/src/LuaModule/ballspeedmodel.pkg
index 39ba87a..e4c7e6a 100644
--- a/Core/src/LuaModule/ballspeedmodel.pkg
+++ b/Core/src/LuaModule/ballspeedmodel.pkg
@@ -3,5 +3,5 @@ class CBallSpeedModel
 {
 	double timeForDist(const double dist);
 	CVector speedForDist(const double dist);
-	MobileVisionT poseForTime(const double time);
+	ObjectPoseT poseForTime(const double time);
 };
\ No newline at end of file
diff --git a/Core/src/LuaModule/visionmodule.pkg b/Core/src/LuaModule/visionmodule.pkg
index 6698928..abf17e7 100644
--- a/Core/src/LuaModule/visionmodule.pkg
+++ b/Core/src/LuaModule/visionmodule.pkg
@@ -17,10 +17,10 @@ class CVisionModule{
 	const PlayerVisionT& allPlayer(int num) const;
 	const PlayerVisionT& ourPlayer(int num) const;
 	const PlayerVisionT& theirPlayer(int num) const;
-	const MobileVisionT& ball() const;
+	const ObjectPoseT& ball() const;
 	const PlayerVisionT& ourPlayer(int cycle, int num) const;
 	const PlayerVisionT& theirPlayer(int cycle, int num) const;
-	const MobileVisionT& ball(int cycle) const;
+	const ObjectPoseT& ball(int cycle) const;
 	const ObjectPoseT& rawBall() const;
         const CGeoPoint& getBallPlacementPosition() const;
 	const RobotRawVisionData& ourRawPlayer(int num) const;
diff --git a/Core/src/LuaModule/worlddefine.pkg b/Core/src/LuaModule/worlddefine.pkg
index bf6beb3..a8a4d6a 100644
--- a/Core/src/LuaModule/worlddefine.pkg
+++ b/Core/src/LuaModule/worlddefine.pkg
@@ -16,18 +16,11 @@ class ObjectPoseT{
 	double VelY() const;
 	void SetValid(bool v);
 	bool Valid() const;
-};
-
-class VisionObjectT{
 	const CGeoPoint& RawPos() const;
 	void SetRawPos(double x, double y);
 	void SetRawPos(const CGeoPoint& pos);
 };
 
-class MobileVisionT : public ObjectPoseT, public VisionObjectT{
-
-};
-
 struct PlayerPoseT : public ObjectPoseT{
 	PlayerPoseT();
 	double Dir() const;
@@ -39,12 +32,12 @@ struct PlayerPoseT : public ObjectPoseT{
 };
 
 class PlayerTypeT{
-	void SetType(int t);
-	int Type() const;
-};
 
-class PlayerVisionT : public PlayerPoseT, public VisionObjectT, public PlayerTypeT{
+};
 
+class PlayerVisionT : public PlayerPoseT {
+	void SetType(int t);
+	int Type() const;
 };
 
 struct PlayerCapabilityT{
diff --git a/Core/src/MotionControl/DynamicsSafetySearch.cpp b/Core/src/MotionControl/DynamicsSafetySearch.cpp
index 2ba4131..8fba1b8 100644
--- a/Core/src/MotionControl/DynamicsSafetySearch.cpp
+++ b/Core/src/MotionControl/DynamicsSafetySearch.cpp
@@ -383,7 +383,7 @@ bool CDynamicSafetySearch::CheckAccel (const int player, vector2f acc, const CVi
     if(pVision->ball().Valid() && (_flag & PlayerStatus::DODGE_BALL)) {
         vector2f pj = vector2f(pVision->ball().Pos().x(), pVision->ball().Pos().y());
         vector2f vj = vector2f(pVision->ball().VelX(), pVision->ball().VelY());
-        const MobileVisionT& last_ball = pVision->ball(pVision->getLastCycle());
+        const ObjectPoseT& last_ball = pVision->ball(pVision->getLastCycle());
         vector2f Aj;
         Aj = (vj - vector2f(last_ball.VelX(), last_ball.VelY())) /_C;
         if(CheckRobot(player, _pos, _vel, acc, -1, pj, vj, Aj, type, limitTime) == UNSAFE) {
diff --git a/Core/src/OtherLibs/cmu/obstacle.cpp b/Core/src/OtherLibs/cmu/obstacle.cpp
index e0c0550..bf7a1aa 100644
--- a/Core/src/OtherLibs/cmu/obstacle.cpp
+++ b/Core/src/OtherLibs/cmu/obstacle.cpp
@@ -442,12 +442,12 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra
 
     // ball
     if(flags & PlayerStatus::DODGE_BALL) {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         add_circle(vector2f(ball.Pos().x(), ball.Pos().y()), vector2f(ball.Vel().x(), ball.Vel().y()), ballAvoidDist, 1, drawObs);
     }
 
     if(WorldModel::Instance()->CurrentRefereeMsg() == "GameStop") {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         add_circle(vector2f(ball.Pos().x(), ball.Pos().y()), vector2f(0.0f, 0.0f), 50.0f, 1, drawObs);
     }
 
diff --git a/Core/src/PathPlan/ObstacleNew.cpp b/Core/src/PathPlan/ObstacleNew.cpp
index 45d4014..8d65eb7 100644
--- a/Core/src/PathPlan/ObstacleNew.cpp
+++ b/Core/src/PathPlan/ObstacleNew.cpp
@@ -486,13 +486,13 @@ void ObstaclesNew::addObs(const CVisionModule * pVision, const TaskT & task, boo
 
     // ball
     if (flags & PlayerStatus::DODGE_BALL) {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         addCircle(ball.RawPos(), ball.Vel(), ballAvoidDist, OBS_CIRCLE_NEW);
     }
 
     if (WorldModel::Instance()->CurrentRefereeMsg() == "GameStop" ||
             (flags & PlayerStatus::AVOID_STOP_BALL_CIRCLE)) {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         addCircle(ball.RawPos(), CVector(0.0f, 0.0f), stopBallAvoidDist, OBS_CIRCLE_NEW);
     }
 
diff --git a/Core/src/Vision/BallPredictor.cpp b/Core/src/Vision/BallPredictor.cpp
index b6dff01..9208390 100644
--- a/Core/src/Vision/BallPredictor.cpp
+++ b/Core/src/Vision/BallPredictor.cpp
@@ -86,8 +86,8 @@ bool CBallPredictor::checkValid(int cycle) {
     return true;
 }
 
-void CBallPredictor::setCollisionResult(int cycle, const MobileVisionT& ball) {
-    MobileVisionT& oldBall = _visionLogger.getVision(cycle);
+void CBallPredictor::setCollisionResult(int cycle, const ObjectPoseT& ball) {
+    ObjectPoseT& oldBall = _visionLogger.getVision(cycle);
     oldBall = ball;
     _hasCollision = true;
 
diff --git a/Core/src/Vision/BallPredictor.h b/Core/src/Vision/BallPredictor.h
index 5dd21d6..af31c29 100644
--- a/Core/src/Vision/BallPredictor.h
+++ b/Core/src/Vision/BallPredictor.h
@@ -21,7 +21,7 @@ const double MAX_DIST = 1000.0 / PARAM::Vision::FRAME_RATE * 4;
 const int MAX_LOGS = 16;
 }
 
-class BallVisionData : public MobileVisionT {
+class BallVisionData : public ObjectPoseT {
   public:
     BallVisionData(): cycle(-1) {}
     int cycle;
@@ -64,10 +64,10 @@ class CBallPredictor {
     BallVisionData& getData(int cycle) {
         return _visionLogger.getVision(cycle);
     }
-    const MobileVisionT& getResult(int cycle) const {
+    const ObjectPoseT& getResult(int cycle) const {
         return _visionLogger.getVision(cycle);
     }
-    void setCollisionResult(int cycle, const MobileVisionT& ball);
+    void setCollisionResult(int cycle, const ObjectPoseT& ball);
     int visibility() const {
         return _visibility;
     }
@@ -91,7 +91,7 @@ class CBallPredictor {
     bool checkValid(int cycle); // 去掉不合理的情况
   private:
     CBallVisionLogger _visionLogger;
-    MobileVisionT _ballLinePredictData[60];
+    ObjectPoseT _ballLinePredictData[60];
     int _ballLostTime; // 看不到球的次数
     int _ballInvalidMovedCycle; // 球的信息不合理的周期数
     int _visibility, _activity; // 可见度和活动度
diff --git a/Core/src/Vision/CollisionSimulator.h b/Core/src/Vision/CollisionSimulator.h
index e720ee2..c8a5328 100644
--- a/Core/src/Vision/CollisionSimulator.h
+++ b/Core/src/Vision/CollisionSimulator.h
@@ -9,10 +9,10 @@ class CCollisionSimulator{
 	CCollisionSimulator() : _hasCollision(false){ }
 	void reset(const CGeoPoint& ballRawPos, const CVector& ballVel);
 	void simulate(PlayerVisionT robot, const double time); // 模拟一定时间
-	const MobileVisionT& ball() const { return _ball; }
+	const ObjectPoseT& ball() const { return _ball; }
 	bool hasCollision() const { return _hasCollision; }
 private:
-	MobileVisionT _ball; // 球的信息
+	ObjectPoseT _ball; // 球的信息
 	CVector _ballRelToRobot; // 球在机器人局部坐标系中的位置
 	bool _hasCollision; // 是否碰撞
 };
diff --git a/Core/src/Vision/RobotPredictor.cpp b/Core/src/Vision/RobotPredictor.cpp
index 423d445..2d00ce0 100644
--- a/Core/src/Vision/RobotPredictor.cpp
+++ b/Core/src/Vision/RobotPredictor.cpp
@@ -34,7 +34,7 @@ CRobotPredictor::CRobotPredictor(bool isHasRotation) : _robotLostTime(0), _isHas
 //        _robotFilter.initialize(PARAM::File::RobotPosFilterDir + "slowMatrices.txt", PARAM::File::RobotPosFilterDir + "fastMatrices.txt");
 //    }
 }
-void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert) {
+void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert) {
     // !!!!这里好像有问题:
     // 如果是对手车也能识别朝向,就会调用updateOurVision, 但因为在该函数里取不到cmd(只有我方车才有cmd存下来), 结果就不对.
     // 这里需要一个辨别我方车与对方车的方式!!
@@ -95,7 +95,7 @@ void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const
     }
 }
 
-//void CRobotPredictor::updateOurVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert, int realNum) {
+//void CRobotPredictor::updateOurVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert, int realNum) {
 //    const double x = invert ? -player.pos.x : player.pos.x;
 //    const double y = invert ? -player.pos.y : player.pos.y;
 //    double dir = 0;
@@ -279,7 +279,7 @@ void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const
 //    predictedVision.SetType(player.type);
 //}
 
-//void CRobotPredictor::updateTheirVision(int cycle, const VehicleInfoT & player, const MobileVisionT & ball, bool invert, int realNum) {
+//void CRobotPredictor::updateTheirVision(int cycle, const VehicleInfoT & player, const ObjectPoseT & ball, bool invert, int realNum) {
 //    const double x = invert ? -player.pos.x : player.pos.x;
 //    const double y = invert ? -player.pos.y : player.pos.y;
 //    double dir = 0;
@@ -435,7 +435,7 @@ bool CRobotPredictor::checkValid(int cycle, const CGeoPoint & pos) {
     }
     return true;
 }
-void CRobotPredictor::predictLost(int cycle, const MobileVisionT & ball) {
+void CRobotPredictor::predictLost(int cycle, const ObjectPoseT & ball) {
     //cout<<cycle<<" ";
     // 图像当前帧丢车后的处理办法
     if( _visionLogger.visionValid(cycle - 1)) {
diff --git a/Core/src/Vision/RobotPredictor.h b/Core/src/Vision/RobotPredictor.h
index 485bfd7..56a4eeb 100644
--- a/Core/src/Vision/RobotPredictor.h
+++ b/Core/src/Vision/RobotPredictor.h
@@ -25,9 +25,9 @@ class CRobotPredictor {
     CRobotPredictor();
     CRobotPredictor (bool isHasRotaion);
     void updateCommand(int cycle, const CPlayerCommand* cmd); // 更新命令
-    void updateVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert); // 更新视觉信息
-    void updateOurVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert, int realNum); // 更新视觉信息
-    void updateTheirVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert, int realNum); // 更新视觉信息
+    void updateVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert); // 更新视觉信息
+    void updateOurVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert, int realNum); // 更新视觉信息
+    void updateTheirVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert, int realNum); // 更新视觉信息
 
     CRobotCommandLogger& getCommandLogger() {
         return _commandLogger;
@@ -50,7 +50,7 @@ class CRobotPredictor {
     bool collideWithBall() const {
         return _collisionSimulator.hasCollision();    // 只是单纯地考虑是否相撞与碰撞区域
     }
-    const MobileVisionT& ballCollidedResult() const {
+    const ObjectPoseT& ballCollidedResult() const {
         return _collisionSimulator.ball();
     }
     void setIsHasRotation(bool isHasRot) {
@@ -59,7 +59,7 @@ class CRobotPredictor {
   protected:
 //    void updateRawVision(int cycle, double x, double y, double dir, bool seen);
     bool checkValid(int cycle, const CGeoPoint& pos);
-    void predictLost(int cycle, const MobileVisionT& ball); // 预测看不到的信息
+    void predictLost(int cycle, const ObjectPoseT& ball); // 预测看不到的信息
   private:
 //    FilteredObject _robotFilter; // KalmanFilter
 //    FilteredObject _robotRotationFilter; // 进行robot朝向滤波
diff --git a/Core/src/Vision/VisionModule.cpp b/Core/src/Vision/VisionModule.cpp
index ba1232a..d7966ac 100644
--- a/Core/src/Vision/VisionModule.cpp
+++ b/Core/src/Vision/VisionModule.cpp
@@ -232,7 +232,7 @@ void CVisionModule::setNewVision() {
     /// @brief Step 2: 进行我方和对方机器人位置预测，关注滤波器
     /////////////////////////////////////////////////////////////////////////////
     //【#TODO】机器人碰撞检测器清空，
-    const MobileVisionT& thisBall = _ballPredictor.getData(_timeCycle);
+    const ObjectPoseT& thisBall = _ballPredictor.getData(_timeCycle);
     // 确定球员的信息是否需要反向，确保正确更新比赛双方球员的信息
     for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++ i) {
         const VehicleInfoT& ourPlayer = _info.player[0][i];
diff --git a/Core/src/Vision/VisionModule.h b/Core/src/Vision/VisionModule.h
index 9d937ff..d52b5cb 100644
--- a/Core/src/Vision/VisionModule.h
+++ b/Core/src/Vision/VisionModule.h
@@ -82,7 +82,7 @@ class CVisionModule{
 		}
 	}
 
-    const MobileVisionT& ball() const {
+    const ObjectPoseT& ball() const {
         return _ballPredictor.getResult(_timeCycle);
     }
 
@@ -94,7 +94,7 @@ class CVisionModule{
         return _theirPlayerPredictor[num].getResult(cycle);
     }
 
-    const MobileVisionT& ball(int cycle) const {
+    const ObjectPoseT& ball(int cycle) const {
         return _ballPredictor.getResult(cycle);
     }
 
diff --git a/Core/src/WorldModel/WorldDefine.h b/Core/src/WorldModel/WorldDefine.h
index 22bf39a..662fa97 100644
--- a/Core/src/WorldModel/WorldDefine.h
+++ b/Core/src/WorldModel/WorldDefine.h
@@ -6,7 +6,7 @@
 /************************************************************************/
 class ObjectPoseT {
   public:
-    ObjectPoseT() : _valid(false), _pos(CGeoPoint(-9999, -9999)) { }
+    ObjectPoseT() : _valid(false), _pos(CGeoPoint(-9999, -9999)), _rawPos(CGeoPoint(-9999, -9999)) { }
     const CGeoPoint& Pos() const {
         return _pos;
     }
@@ -67,19 +67,7 @@ class ObjectPoseT {
     bool Valid() const {
         return _valid;
     }
-  private:
-    CGeoPoint _pos;
-    CVector _vel;
-    CVector _rawVel;
-    CVector _acc;
-    bool _valid;
-};
-/************************************************************************/
-/*                      VisionObjectT                                   */
-/************************************************************************/
-class VisionObjectT {
-  public:
-    VisionObjectT() : _rawPos(CGeoPoint(-9999, -9999)) { }
+
     const CGeoPoint& RawPos() const {
         return _rawPos;
     }
@@ -105,16 +93,19 @@ class VisionObjectT {
         _rawDir = rawdir;
     }
   private:
+    CGeoPoint _pos;
+    CVector _vel;
+    CVector _rawVel;
+    CVector _acc;
+    bool _valid;
+
     CGeoPoint _rawPos; // 视觉的原始信息，没有经过预测
     CGeoPoint _chipPredict; //挑球预测
     double _rawDir;
 };
-/************************************************************************/
-/*                       MobileVisionT                                  */
-/************************************************************************/
-class MobileVisionT : public ObjectPoseT, public VisionObjectT {
 
-};
+// using ObjectPoseT = ObjectPoseT;
+
 /************************************************************************/
 /*                        机器人姿态数据结构                               */
 /************************************************************************/
@@ -145,11 +136,11 @@ struct PlayerPoseT : public ObjectPoseT { // 目标信息
     double _rawRotVel;
 };
 /************************************************************************/
-/*                      PlayerTypeT                                     */
+/*                       PlayerVisionT                                  */
 /************************************************************************/
-class PlayerTypeT {
+class PlayerVisionT : public PlayerPoseT {
   public:
-    PlayerTypeT(): _type(0) {}
+    PlayerVisionT() : _type(0) {}
     void SetType(int t) {
         _type = t;
     }
@@ -159,12 +150,6 @@ class PlayerTypeT {
   private:
     int _type;
 };
-/************************************************************************/
-/*                       PlayerVisionT                                  */
-/************************************************************************/
-class PlayerVisionT : public PlayerPoseT, public VisionObjectT, public PlayerTypeT {
-
-};
 
 /************************************************************************/
 /*                        机器人能力数据结构                               */
diff --git a/Core/src/WorldModel/WorldModel.h b/Core/src/WorldModel/WorldModel.h
index 8349549..623de8f 100644
--- a/Core/src/WorldModel/WorldModel.h
+++ b/Core/src/WorldModel/WorldModel.h
@@ -49,7 +49,7 @@ class CWorldModel{
 	const COptionModule* _pOption;
 	PlayerVisionT me;
 	PlayerVisionT he;
-	MobileVisionT ball;
+	ObjectPoseT ball;
 
 
 /************************************************** WorldModel_con ***************************************************/
diff --git a/Core/tactics/play/TestGetBall.lua b/Core/tactics/play/TestGetBall.lua
index cccdb1c..17fce07 100644
--- a/Core/tactics/play/TestGetBall.lua
+++ b/Core/tactics/play/TestGetBall.lua
@@ -16,6 +16,10 @@ return {
         switch = function()
             local dist = (ball.pos() - player.pos("Assister")):mod()
             debugEngine:gui_debug_msg(ball.pos()+Utils.Polar2Vector(300,math.pi/2),"Dist : " .. dist)
+            debugEngine:gui_debug_x(player.pos("Assister"), param.GREEN)
+            debugEngine:gui_debug_msg(player.pos("Assister"), "Pos", param.GREEN)
+            debugEngine:gui_debug_x(player.rawPos("Assister"), param.PURPLE)
+            debugEngine:gui_debug_msg(player.rawPos("Assister"), "RawPos", param.PURPLE)
         end,
         Assister = task.touchKick(CGeoPoint(6000,0),nil,6000,true),
         -- Assister = task.stop(),
diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
index 04f7a9c..b7a35c1 100644
--- a/Core/tactics/skill/Touch.cpp
+++ b/Core/tactics/skill/Touch.cpp
@@ -29,7 +29,7 @@ void CTouch::plan(const CVisionModule* pVision){
     const PlayerVisionT& me = pVision->ourPlayer(runner);
     const auto mouseVec = Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER, me.Dir());
     const auto mousePos = me.Pos() + mouseVec;
-    const MobileVisionT& ball = pVision->ball();
+    const ObjectPoseT& ball = pVision->ball();
     const double ballVelDir = ball.Vel().dir();
     const CGeoPoint& ballPos = ball.RawPos();
     const CGeoLine ballVelLine(ballPos, ballVelDir);
diff --git a/ZBin/lua_scripts/worldmodel/player.lua b/ZBin/lua_scripts/worldmodel/player.lua
index 533e962..fd42bbd 100644
--- a/ZBin/lua_scripts/worldmodel/player.lua
+++ b/ZBin/lua_scripts/worldmodel/player.lua
@@ -42,6 +42,10 @@ function posY(role)
 	return instance(role):Y()
 end
 
+function rawPos(role)
+	return instance(role):RawPos()
+end
+
 function dir(role)
 	return instance(role):Dir()
 end

From eb8b22f0a62538b4e422e5f6a59a5c209382ba44 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Fri, 12 Apr 2024 17:47:56 +0800
Subject: [PATCH 11/30] [Core] update CircleRun

---
 Core/tactics/play/TestMyRun.lua  |  8 +++++++-
 Core/tactics/skill/CircleRun.cpp | 35 ++++++++++++++++++++++----------
 Core/tactics/skill/CircleRun.h   |  2 +-
 Core/tactics/skill/CircleRun.lua |  4 ++--
 4 files changed, 34 insertions(+), 15 deletions(-)

diff --git a/Core/tactics/play/TestMyRun.lua b/Core/tactics/play/TestMyRun.lua
index 5b4d968..43e5921 100644
--- 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
index 96b50b9..b4b17a9 100644
--- 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));
 }
\ No newline at end of file
diff --git a/Core/tactics/skill/CircleRun.h b/Core/tactics/skill/CircleRun.h
index 271077b..49a1390 100644
--- 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
index 9f5c370..b363a1a 100644
--- 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
 

From b681e9c8f50546739df318594b58ca0e8db82806 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Fri, 12 Apr 2024 18:50:21 +0800
Subject: [PATCH 12/30] [Core] fix bug of CircleRun

---
 Core/tactics/skill/CircleRun.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Core/tactics/skill/CircleRun.cpp b/Core/tactics/skill/CircleRun.cpp
index b4b17a9..16deab0 100644
--- a/Core/tactics/skill/CircleRun.cpp
+++ b/Core/tactics/skill/CircleRun.cpp
@@ -39,7 +39,7 @@ CPlayerCommand* CCircleRun::execute(const CVisionModule* pVision){
 
     const CVector localVel = meVel + dv;
 
-    const double limitTargetRotVel = targetRotVel*localVel.mod()/targetVelMod;
+    const double limitTargetRotVel = std::abs(targetVelMod > 0.1) ? targetRotVel*localVel.mod()/targetVelMod : targetRotVel;
     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;
 

From f204974ac9332c75e4ceb776c54e1156629ef738 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Fri, 12 Apr 2024 20:28:08 +0800
Subject: [PATCH 13/30] [ALL] merge 4 pitch params into ini

---
 Client/plugins/sim/configwidget.cpp           |  16 +-
 Core/src/Strategy/skill/SmartGotoPosition.cpp |   1 -
 Core/src/Utils/utils.cpp                      | 389 ++++--------------
 ZBin/lua_scripts/worldmodel/param.lua         |  12 +-
 share/staticparams.cpp                        |  15 +
 share/staticparams.h                          |  50 +--
 6 files changed, 109 insertions(+), 374 deletions(-)
 create mode 100644 share/staticparams.cpp

diff --git a/Client/plugins/sim/configwidget.cpp b/Client/plugins/sim/configwidget.cpp
index 3247a37..0863b4d 100644
--- a/Client/plugins/sim/configwidget.cpp
+++ b/Client/plugins/sim/configwidget.cpp
@@ -16,26 +16,26 @@ namespace{
 #define ADD_VALUE(parent,type,name,defaultvalue,namestring) \
     ppm->loadParam(v_##name,#parent"/"#name,defaultvalue);
 
-#define ADD_CLIENT_VALUE(parent, type, name, defaultvalue, namestring) \
-    zpm->loadParam(v_##name, #parent "/" #name, defaultvalue);
+#define ADD_CLIENT_VALUE(parent, type, name, defaultvalue, namestring, ratio) \
+    v_##name = zpm->value(#parent "/" #namestring, QVariant(ratio * defaultvalue)).to##type() / ratio;
 
 ConfigWidget::ConfigWidget()
 {
     ADD_VALUE(game_vars,Int,Robots_Count, 16, "Robots Count")
     ADD_VALUE(field_vars,Double,Field_Line_Width,0.020,"Line Thickness")
-    ADD_VALUE(field_vars,Double,Field_Length,9.000,"Length")
-    ADD_VALUE(field_vars,Double,Field_Width,6.000,"Width")
+    ADD_CLIENT_VALUE(field,Double,Field_Length,9.000,width, 1000.0)
+    ADD_CLIENT_VALUE(field,Double,Field_Width,6.000,height, 1000.0)
     ADD_VALUE(field_vars,Double,Field_Rad,0.500,"Radius")
     ADD_VALUE(field_vars,Double,Field_Free_Kick,0.700,"Free Kick Distanse From Defense Area")
-    ADD_VALUE(field_vars,Double,Field_Penalty_Width,2.0,"Penalty width")
-    ADD_VALUE(field_vars,Double,Field_Penalty_Depth,1.0,"Penalty depth")
+    ADD_CLIENT_VALUE(field,Double,Field_Penalty_Width,2.0,penaltyLength, 1000.0)
+    ADD_CLIENT_VALUE(field,Double,Field_Penalty_Depth,1.0,penaltyWidth, 1000.0)
     ADD_VALUE(field_vars,Double,Field_Penalty_Point,1.0,"Penalty point")
     ADD_VALUE(field_vars,Double,Field_Margin,0.4,"Margin")
     ADD_VALUE(field_vars,Double,Field_Referee_Margin,0.0,"Referee margin")
     ADD_VALUE(field_vars,Double,Wall_Thickness,0.050,"Wall thickness")
     ADD_VALUE(field_vars,Double,Goal_Thickness,0.020,"Goal thickness")
-    ADD_VALUE(field_vars,Double,Goal_Depth,0.200,"Goal depth")
-    ADD_VALUE(field_vars,Double,Goal_Width,1.000,"Goal width")
+    ADD_CLIENT_VALUE(field,Double,Goal_Depth,0.200,goalDepth, 1000.0)
+    ADD_CLIENT_VALUE(field,Double,Goal_Width,1.000,goalWidth, 1000.0)
     ADD_VALUE(field_vars,Double,Goal_Height,0.160,"Goal height")
     ADD_VALUE(field_vars,Double,overlap,0.20,"Camera Overlap")
 
diff --git a/Core/src/Strategy/skill/SmartGotoPosition.cpp b/Core/src/Strategy/skill/SmartGotoPosition.cpp
index 0a13387..986bf88 100644
--- a/Core/src/Strategy/skill/SmartGotoPosition.cpp
+++ b/Core/src/Strategy/skill/SmartGotoPosition.cpp
@@ -66,7 +66,6 @@ namespace{
     CGeoPoint lastFinalPoint[PARAM::Field::MAX_PLAYER];
     CGeoPoint veryStart[PARAM::Field::MAX_PLAYER];
     bool isExecuting[PARAM::Field::MAX_PLAYER];
-    const double Fake_PENALTY_AREA_R = 150; // added by ftq
     const double TEAMMATE_AVOID_DIST = PARAM::Vehicle::V2::PLAYER_SIZE + 40.0f; // 2014/03/13 修改，为了减少stop的时候卡住的概率 yys
     const double OPP_AVOID_DIST = PARAM::Vehicle::V2::PLAYER_SIZE + 55.0f;
     const double BALL_AVOID_DIST = PARAM::Field::BALL_SIZE + 50.0f;
diff --git a/Core/src/Utils/utils.cpp b/Core/src/Utils/utils.cpp
index 5df9474..2755d78 100644
--- a/Core/src/Utils/utils.cpp
+++ b/Core/src/Utils/utils.cpp
@@ -330,301 +330,89 @@ namespace Utils{
 		return (pInter + Polar2Vector(delta, dir));
 	}
 
-	// GetDefendPos的处理细节
-	// 给定点和方向求它和禁区线的交点
-	//给定点需在禁区内
-	//modified by Wang in 2018/3/17
 	CGeoPoint GetInterPos(double dir, const CGeoPoint targetPoint) {
 		using namespace PARAM::Field;
-        if ( IF_USE_ELLIPSE ){
-            // ellipse penalty
-            // 禁区的两段圆弧,用圆来表示
-            CGeoCirlce c1(CGeoPoint(-PITCH_LENGTH/2,  PENALTY_AREA_L/2), PENALTY_AREA_R);
-            CGeoCirlce c2(CGeoPoint(-PITCH_LENGTH/2, -PENALTY_AREA_L/2), PENALTY_AREA_R);
-            CGeoPoint targetPointInstead = targetPoint;
-            if (dir >= PARAM::Math::PI/2 - 5/180*PARAM::Math::PI && dir <= PARAM::Math::PI)
-        return CGeoPoint(-PITCH_LENGTH/2,PENALTY_AREA_L/2+PENALTY_AREA_R);
-            else if (dir <= -PARAM::Math::PI/2 + 5/180*PARAM::Math::PI && dir >= -PARAM::Math::PI)
-                return CGeoPoint(-PITCH_LENGTH/2,-PENALTY_AREA_L/2-PENALTY_AREA_R);
-
-            // 连接两段圆弧的直线(pLine),用直线来表示
-            CGeoPoint pend1(-PITCH_LENGTH/2+PENALTY_AREA_R,  PENALTY_AREA_L/2);
-            CGeoPoint pend2(-PITCH_LENGTH/2+PENALTY_AREA_R, -PENALTY_AREA_L/2);
-            CGeoLine pLine(pend1, pend2);
-            // 过给定的点和方向, 作一条直线
-            CGeoLine dirLine(targetPointInstead, dir);
-
-            // 求该直线和c1的交点
-            if (targetPoint.y() == c1.Center().y())
-            {
-                if (dir>=0 && dir<PARAM::Math::PI/2)
-                {
-                    CGeoPoint p = c1.Center()+Polar2Vector(PENALTY_AREA_R,dir);
-                    return p;
-                }
-            }
-            else{
-                CGeoLineCircleIntersection dirLine_c1_inter(dirLine, c1);
-                if (dirLine_c1_inter.intersectant())
-                {
-                    CGeoPoint p1 = dirLine_c1_inter.point1();
-                    CGeoPoint p2 = dirLine_c1_inter.point2();
-                    double dir1 = Normalize((p1-c1.Center()).dir());
-                    double dir2 = Normalize((p2-c1.Center()).dir());
-                    if (dir1>=0 && dir1<=PARAM::Math::PI/2)
-                    {
-                        return p1;
-
-                    }
-                    else if (dir2>=0 && dir2<=PARAM::Math::PI/2)
-                    {
-                        return p2;
-
-                    }
-                }
-            }
-
-            // 求该直线和c2的交点
-            if (targetPoint.y() == c2.Center().y())
-            {
-                if ( dir<=0 && dir>(-PARAM::Math::PI/2))
-                {
-                    CGeoPoint p = c2.Center()+Polar2Vector(PENALTY_AREA_R,dir);
-                    return p;
-                }
-            }
-            else{
-                CGeoLineCircleIntersection dirLine_c2_inter(dirLine, c2);
-                if (dirLine_c2_inter.intersectant())
-                {
-                    CGeoPoint p1 = dirLine_c2_inter.point1();
-                    CGeoPoint p2 = dirLine_c2_inter.point2();
-                    double dir1 = Normalize((p1-c2.Center()).dir());
-                    double dir2 = Normalize((p2-c2.Center()).dir());
-                    if (dir1>=(-PARAM::Math::PI/2) && dir1<=0)
-                    {
-                        return p1;
-
-                    }
-                    else if (dir2>=(-PARAM::Math::PI/2) && dir2<=0)
-                    {
-                        return p2;
-
-                    }
-                }
-            }
-            // 求该直线和连接两条圆弧的线段pLine的交点
-            CGeoLineLineIntersection pline_dirline_inter(pLine, dirLine);
-            if (pline_dirline_inter.Intersectant())
-            {
-                CGeoPoint p = pline_dirline_inter.IntersectPoint();
-                if (p.y() <= pend1.y() && p.y() >= pend2.y())
-                {
-                    return p;
-
-                }
-            }
-            //// 返回一个默认点,禁区顶部的中点
-//            std::cout<<"our default pos!!"<<std::endl;
-            return CGeoPoint(-PITCH_LENGTH/2+PENALTY_AREA_R, 0);
+        // rectangle penalty
+        CGeoPoint p1(-PITCH_LENGTH / 2, -PENALTY_AREA_WIDTH / 2);//禁区左下
+        CGeoPoint p2(-PITCH_LENGTH / 2 + PENALTY_AREA_DEPTH, -PENALTY_AREA_WIDTH / 2);//禁区左上
+        CGeoPoint p3(-PITCH_LENGTH / 2 + PENALTY_AREA_DEPTH, PENALTY_AREA_WIDTH / 2);//禁区右上
+        CGeoPoint p4(-PITCH_LENGTH / 2, PENALTY_AREA_WIDTH / 2);//禁区右下
+        CGeoLine line1(p1, p2);//禁区左边线
+        CGeoLine line2(p2, p3);//禁区前边线
+        CGeoLine line3(p3, p4);//禁区右边线
+        CGeoLine dirLine(targetPoint, dir);
+
+        CGeoLineLineIntersection inter1(line1, dirLine);
+        CGeoLineLineIntersection inter2(line2, dirLine);
+        CGeoLineLineIntersection inter3(line3, dirLine);
+
+        CGeoPoint inter_p1 = inter1.IntersectPoint();
+        GDebugEngine::Instance()->gui_debug_x(inter_p1, 3);//黄
+        CGeoPoint inter_p2 = inter2.IntersectPoint();
+        GDebugEngine::Instance()->gui_debug_x(inter_p2, 4);//绿
+        CGeoPoint inter_p3 = inter3.IntersectPoint();
+        GDebugEngine::Instance()->gui_debug_x(inter_p3, 9);//黑
+        CGeoPoint returnPoint = targetPoint;//返回值
+
+        //if (targetPoint.x() >= -PITCH_LENGTH / 2 + PENALTY_AREA_DEPTH) {
+        if (targetPoint.y() <= 0) {//case 1
+            if (InOurPenaltyArea(inter_p1, 10)) returnPoint = inter_p1;
+            else returnPoint = inter_p2;
         }
-        else {
-            // rectangle penalty
-            CGeoPoint p1(-PITCH_LENGTH / 2, -PENALTY_AREA_WIDTH / 2);//禁区左下
-            CGeoPoint p2(-PITCH_LENGTH / 2 + PENALTY_AREA_DEPTH, -PENALTY_AREA_WIDTH / 2);//禁区左上
-            CGeoPoint p3(-PITCH_LENGTH / 2 + PENALTY_AREA_DEPTH, PENALTY_AREA_WIDTH / 2);//禁区右上
-            CGeoPoint p4(-PITCH_LENGTH / 2, PENALTY_AREA_WIDTH / 2);//禁区右下
-            CGeoLine line1(p1, p2);//禁区左边线
-            CGeoLine line2(p2, p3);//禁区前边线
-            CGeoLine line3(p3, p4);//禁区右边线
-            CGeoLine dirLine(targetPoint, dir);
-
-            CGeoLineLineIntersection inter1(line1, dirLine);
-            CGeoLineLineIntersection inter2(line2, dirLine);
-            CGeoLineLineIntersection inter3(line3, dirLine);
-
-            CGeoPoint inter_p1 = inter1.IntersectPoint();
-            GDebugEngine::Instance()->gui_debug_x(inter_p1, 3);//黄
-            CGeoPoint inter_p2 = inter2.IntersectPoint();
-            GDebugEngine::Instance()->gui_debug_x(inter_p2, 4);//绿
-            CGeoPoint inter_p3 = inter3.IntersectPoint();
-            GDebugEngine::Instance()->gui_debug_x(inter_p3, 9);//黑
-            CGeoPoint returnPoint = targetPoint;//返回值
-
-            //if (targetPoint.x() >= -PITCH_LENGTH / 2 + PENALTY_AREA_DEPTH) {
-            if (targetPoint.y() <= 0) {//case 1
-                if (InOurPenaltyArea(inter_p1, 10)) returnPoint = inter_p1;
-                else returnPoint = inter_p2;
-            }
-            else {//case 2
-                if (InOurPenaltyArea(inter_p3, 10)) returnPoint = inter_p3;
-                else returnPoint = inter_p2;//随便选的
-            }
-            GDebugEngine::Instance()->gui_debug_x(returnPoint, 0);
-            CGeoPoint p0(-PITCH_LENGTH / 2, 0);
-            GDebugEngine::Instance()->gui_debug_line(returnPoint, p0, 0);
-            return returnPoint;
+        else {//case 2
+            if (InOurPenaltyArea(inter_p3, 10)) returnPoint = inter_p3;
+            else returnPoint = inter_p2;//随便选的
         }
-		//}
-		/*
-		else if (std::fabs(targetPoint.y()) <= PENALTY_AREA_WIDTH / 2) {//case 3
-			if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
-			else return p2;//随便选的
-		}
-		else {
-			if (targetPoint.y() <= 0) {//case 4
-				if (InOurPenaltyArea(inter_p1, 0)) return inter_p1;
-				else if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
-				else return p2;//随便选的
-			}
-			else {//case 5
-				if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
-				else if (InOurPenaltyArea(inter_p3, 0)) return inter_p3;
-				else return p3;//随便选的
-			}
-		}
-		*/
+        GDebugEngine::Instance()->gui_debug_x(returnPoint, 0);
+        CGeoPoint p0(-PITCH_LENGTH / 2, 0);
+        GDebugEngine::Instance()->gui_debug_line(returnPoint, p0, 0);
+        return returnPoint;
 	}
-	//modified by Wang in 2018/3/17
     CGeoPoint GetTheirInterPos(double dir, const CGeoPoint& targetPoint) {
         using namespace PARAM::Field;
-        if ( IF_USE_ELLIPSE ){
-            // ellipse penalty
-            // 禁区的两段圆弧,用圆来表示
-            CGeoCirlce c1(CGeoPoint(-PITCH_LENGTH/2,  PENALTY_AREA_L/2), PENALTY_AREA_R);
-            CGeoCirlce c2(CGeoPoint(-PITCH_LENGTH/2, -PENALTY_AREA_L/2), PENALTY_AREA_R);
-            CGeoPoint targetPointInstead = targetPoint;
-            if (dir >= PARAM::Math::PI/2 - 5/180*PARAM::Math::PI && dir <= PARAM::Math::PI)
-                return CGeoPoint(-PITCH_LENGTH/2,PENALTY_AREA_L/2+PENALTY_AREA_R);
-            else if (dir <= -PARAM::Math::PI/2 + 5/180*PARAM::Math::PI && dir >= -PARAM::Math::PI)
-                return CGeoPoint(-PITCH_LENGTH/2,-PENALTY_AREA_L/2-PENALTY_AREA_R);
-
-            // 连接两段圆弧的直线(pLine),用直线来表示
-            CGeoPoint pend1(-PITCH_LENGTH/2+PENALTY_AREA_R,  PENALTY_AREA_L/2);
-            CGeoPoint pend2(-PITCH_LENGTH/2+PENALTY_AREA_R, -PENALTY_AREA_L/2);
-            CGeoLine pLine(pend1, pend2);
-            // 过给定的点和方向, 作一条直线
-            CGeoLine dirLine(targetPointInstead, dir);
-
-            // 求该直线和c1的交点
-            if (targetPoint.y() == c1.Center().y())
-            {
-                if (dir>=0 && dir<PARAM::Math::PI/2)
-                {
-                    CGeoPoint p = c1.Center()+Polar2Vector(PENALTY_AREA_R,dir);
-                    return p;
-                }
-            }
-            else{
-                CGeoLineCircleIntersection dirLine_c1_inter(dirLine, c1);
-                if (dirLine_c1_inter.intersectant())
-                {
-                    CGeoPoint p1 = dirLine_c1_inter.point1();
-                    CGeoPoint p2 = dirLine_c1_inter.point2();
-                    double dir1 = Normalize((p1-c1.Center()).dir());
-                    double dir2 = Normalize((p2-c1.Center()).dir());
-                    if (dir1>=0 && dir1<=PARAM::Math::PI/2)
-                    {
-                        return p1;
-
-                    }
-                    else if (dir2>=0 && dir2<=PARAM::Math::PI/2)
-                    {
-                        return p2;
-
-                    }
-                }
-            }
-
-            // 求该直线和c2的交点
-            if (targetPoint.y() == c2.Center().y())
-            {
-                if ( dir<=0 && dir>(-PARAM::Math::PI/2))
-                {
-                    CGeoPoint p = c2.Center()+Polar2Vector(PENALTY_AREA_R,dir);
-                    return p;
-                }
-            }
-            else{
-                CGeoLineCircleIntersection dirLine_c2_inter(dirLine, c2);
-                if (dirLine_c2_inter.intersectant())
-                {
-                    CGeoPoint p1 = dirLine_c2_inter.point1();
-                    CGeoPoint p2 = dirLine_c2_inter.point2();
-                    double dir1 = Normalize((p1-c2.Center()).dir());
-                    double dir2 = Normalize((p2-c2.Center()).dir());
-                    if (dir1>=(-PARAM::Math::PI/2) && dir1<=0)
-                    {
-                        return p1;
-
-                    }
-                    else if (dir2>=(-PARAM::Math::PI/2) && dir2<=0)
-                    {
-                        return p2;
-
-                    }
-                }
+        // rectangle penalty
+        CGeoPoint p1(PITCH_LENGTH / 2, -PENALTY_AREA_WIDTH / 2);//禁区左上
+        CGeoPoint p2(PITCH_LENGTH / 2 - PENALTY_AREA_DEPTH, -PENALTY_AREA_WIDTH / 2);//禁区左下
+        CGeoPoint p3(PITCH_LENGTH / 2 - PENALTY_AREA_DEPTH, PENALTY_AREA_WIDTH / 2);//禁区右下
+        CGeoPoint p4(PITCH_LENGTH / 2, PENALTY_AREA_WIDTH / 2);//禁区右上
+        CGeoLine line1(p1, p2);//禁区左边线
+        CGeoLine line2(p2, p3);//禁区下边线
+        CGeoLine line3(p3, p4);//禁区右边线
+        CGeoLine dirLine(targetPoint, dir);
+
+        CGeoLineLineIntersection inter1(line1, dirLine);
+        CGeoLineLineIntersection inter2(line2, dirLine);
+        CGeoLineLineIntersection inter3(line3, dirLine);
+
+        CGeoPoint inter_p1 = inter1.IntersectPoint();
+        CGeoPoint inter_p2 = inter2.IntersectPoint();
+        CGeoPoint inter_p3 = inter3.IntersectPoint();
+        CGeoPoint returnPoint = targetPoint;//返回值
+
+        if (targetPoint.x() >= PITCH_LENGTH / 2 - PENALTY_AREA_DEPTH) {
+            if (targetPoint.y() <= 0) {//case 1
+                if (InOurPenaltyArea(inter_p1, 0)) return inter_p1;
+                else return p2;//随便选的
             }
-            // 求该直线和连接两条圆弧的线段pLine的交点
-            CGeoLineLineIntersection pline_dirline_inter(pLine, dirLine);
-            if (pline_dirline_inter.Intersectant())
-            {
-                CGeoPoint p = pline_dirline_inter.IntersectPoint();
-                if (p.y() <= pend1.y() && p.y() >= pend2.y())
-                {
-                    return p;
-
-                }
+            else {//case 2
+                if (InOurPenaltyArea(inter_p3, 0)) return inter_p3;
+                else return p3;//随便选的
             }
-            //// 返回一个默认点,禁区顶部的中点
-//            std::cout<<"our default pos!!"<<std::endl;
-            return CGeoPoint(-PITCH_LENGTH/2+PENALTY_AREA_R, 0);
         }
-        else{
-            // rectangle penalty
-            CGeoPoint p1(PITCH_LENGTH / 2, -PENALTY_AREA_WIDTH / 2);//禁区左上
-            CGeoPoint p2(PITCH_LENGTH / 2 - PENALTY_AREA_DEPTH, -PENALTY_AREA_WIDTH / 2);//禁区左下
-            CGeoPoint p3(PITCH_LENGTH / 2 - PENALTY_AREA_DEPTH, PENALTY_AREA_WIDTH / 2);//禁区右下
-            CGeoPoint p4(PITCH_LENGTH / 2, PENALTY_AREA_WIDTH / 2);//禁区右上
-            CGeoLine line1(p1, p2);//禁区左边线
-            CGeoLine line2(p2, p3);//禁区下边线
-            CGeoLine line3(p3, p4);//禁区右边线
-            CGeoLine dirLine(targetPoint, dir);
-
-            CGeoLineLineIntersection inter1(line1, dirLine);
-            CGeoLineLineIntersection inter2(line2, dirLine);
-            CGeoLineLineIntersection inter3(line3, dirLine);
-
-            CGeoPoint inter_p1 = inter1.IntersectPoint();
-            CGeoPoint inter_p2 = inter2.IntersectPoint();
-            CGeoPoint inter_p3 = inter3.IntersectPoint();
-            CGeoPoint returnPoint = targetPoint;//返回值
-
-            if (targetPoint.x() >= PITCH_LENGTH / 2 - PENALTY_AREA_DEPTH) {
-                if (targetPoint.y() <= 0) {//case 1
-                    if (InOurPenaltyArea(inter_p1, 0)) return inter_p1;
-                    else return p2;//随便选的
-                }
-                else {//case 2
-                    if (InOurPenaltyArea(inter_p3, 0)) return inter_p3;
-                    else return p3;//随便选的
-                }
-            }
-            else if (std::fabs(targetPoint.y()) <= PENALTY_AREA_WIDTH / 2) {//case 3
-                if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
+        else if (std::fabs(targetPoint.y()) <= PENALTY_AREA_WIDTH / 2) {//case 3
+            if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
+            else return p2;//随便选的
+        }
+        else {
+            if (targetPoint.y() <= 0) {//case 4
+                if (InOurPenaltyArea(inter_p1, 0)) return inter_p1;
+                else if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
                 else return p2;//随便选的
             }
-            else {
-                if (targetPoint.y() <= 0) {//case 4
-                    if (InOurPenaltyArea(inter_p1, 0)) return inter_p1;
-                    else if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
-                    else return p2;//随便选的
-                }
-                else {//case 5
-                    if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
-                    else if (InOurPenaltyArea(inter_p3, 0)) return inter_p3;
-                    else return p3;//随便选的
-                }
+            else {//case 5
+                if (InOurPenaltyArea(inter_p2, 0)) return inter_p2;
+                else if (InOurPenaltyArea(inter_p3, 0)) return inter_p3;
+                else return p3;//随便选的
             }
         }
 	}
@@ -694,41 +482,6 @@ namespace Utils{
 					}
 				}
 			}
-            // 2019, china open, ellipse penalty
-            else if (PARAM::Rule::Version == 2019 &&
-                     PARAM::Field::IF_USE_ELLIPSE) {
-                CGeoCirlce c1(CGeoPoint(-PARAM::Field::PITCH_LENGTH/2,
-                                        PARAM::Field::PENALTY_AREA_L/2),
-                              PARAM::Field::PENALTY_AREA_R + avoidBuffer);
-                CGeoCirlce c2(CGeoPoint(-PARAM::Field::PITCH_LENGTH/2,
-                                        -PARAM::Field::PENALTY_AREA_L/2),
-                              PARAM::Field::PENALTY_AREA_R + avoidBuffer);
-                CGeoRectangle defenseBox(
-                            -PARAM::Field::PITCH_LENGTH / 2 +
-                            PARAM::Field::PENALTY_AREA_R +
-                            avoidBuffer,
-                            -PARAM::Field::PENALTY_AREA_L / 2,
-                            -PARAM::Field::PITCH_LENGTH / 2,
-                            PARAM::Field::PENALTY_AREA_L/ 2);
-                CGeoLineCircleIntersection intersection1(moving_seg, c1);
-                CGeoLineCircleIntersection intersection2(moving_seg, c2);
-                CGeoLineRectangleIntersection intersection3(moving_seg,
-                                                           defenseBox);
-                if (intersection1.intersectant() ||
-                        intersection2.intersectant() ||
-                        intersection3.intersectant()) {
-                    if (moving_seg.IsPointOnLineOnSegment(intersection1.point1()) ||
-                            moving_seg.IsPointOnLineOnSegment(intersection1.point2())||
-                            moving_seg.IsPointOnLineOnSegment(intersection2.point1())||
-                            moving_seg.IsPointOnLineOnSegment(intersection2.point2())||
-                            moving_seg.IsPointOnLineOnSegment(intersection3.point1())||
-                            moving_seg.IsPointOnLineOnSegment(intersection3.point2())) {
-                        _canGo = false; // 要经过禁区
-                        return _canGo;
-                    }
-                }
-
-            }
             else {// 2018年的规则禁区是矩形
 				CGeoRectangle defenseBox(-PARAM::Field::PITCH_LENGTH / 2 + PARAM::Field::PENALTY_AREA_DEPTH + avoidBuffer, -PARAM::Field::PENALTY_AREA_WIDTH / 2 - avoidBuffer, -PARAM::Field::PITCH_LENGTH / 2, PARAM::Field::PENALTY_AREA_WIDTH / 2 + avoidBuffer);
 				CGeoLineRectangleIntersection intersection(moving_seg, defenseBox);
diff --git a/ZBin/lua_scripts/worldmodel/param.lua b/ZBin/lua_scripts/worldmodel/param.lua
index b5c5fb7..c7804ac 100644
--- a/ZBin/lua_scripts/worldmodel/param.lua
+++ b/ZBin/lua_scripts/worldmodel/param.lua
@@ -18,14 +18,14 @@ module(..., package.seeall)
 
 
 maxPlayer   = 16
-pitchLength = 9000
-pitchWidth  = 6000
-goalWidth = 1000
-goalDepth = 200
+pitchLength = CGetSettings("field/width","Int")
+pitchWidth  = CGetSettings("field/height","Int")
+goalWidth = CGetSettings("field/goalWidth","Int")
+goalDepth = CGetSettings("field/goalDepth","Int")
 freeKickAvoidBallDist = 500
 playerRadius	= 90
-penaltyWidth    = 2000
-penaltyDepth	= 1000
+penaltyWidth    = CGetSettings("field/penaltyLength","Int")
+penaltyDepth	= CGetSettings("field/penaltyWidth","Int")
 penaltyRadius	= 1000
 penaltySegment	= 500
 playerFrontToCenter = 76
diff --git a/share/staticparams.cpp b/share/staticparams.cpp
new file mode 100644
index 0000000..62fc37c
--- /dev/null
+++ b/share/staticparams.cpp
@@ -0,0 +1,15 @@
+#include "staticparams.h"
+#include "parammanager.h"
+
+auto zpm = ZSS::ZParamManager::instance();
+
+namespace PARAM {
+namespace Field {
+const double PITCH_LENGTH = zpm->value("field/width",QVariant(9000)).toFloat();
+const double PITCH_WIDTH = zpm->value("field/height",QVariant(6000)).toFloat();
+const double PENALTY_AREA_WIDTH = zpm->value("field/penaltyLength",QVariant(2000)).toFloat();
+const double PENALTY_AREA_DEPTH = zpm->value("field/penaltyWidth",QVariant(1000)).toFloat();
+const double GOAL_WIDTH = zpm->value("field/goalWidth",QVariant(1000)).toFloat();
+const double GOAL_DEPTH = zpm->value("field/goalDepth",QVariant(200)).toFloat();
+}
+}
\ No newline at end of file
diff --git a/share/staticparams.h b/share/staticparams.h
index 2b20beb..ce4a0bb 100644
--- a/share/staticparams.h
+++ b/share/staticparams.h
@@ -12,7 +12,6 @@ namespace PARAM {
     const int BLUE = 0;
     const int YELLOW = 1;
     const int BALLMERGEDISTANCE = 0;
-    const int ROBOTMERGEDOSTANCE = 100;
     const int TEAMS = 2;
 
     namespace Field{
@@ -26,26 +25,23 @@ namespace PARAM {
         const double BALL_DECAY = -0.8; // 阻力对球的加速度和速度成正比,单位为 /s
         /* Player */
         const double MAX_PLAYER_SIZE = 180;
-        const double PITCH_LENGTH = 12000;                 // 场地长
-        const double PITCH_WIDTH = 9000;                  // 场地宽
+
+        extern const double PITCH_LENGTH;                 // 场地长
+        extern const double PITCH_WIDTH;                  // 场地宽
+        extern const double PENALTY_AREA_WIDTH;           // rectangle禁区宽度
+        extern const double PENALTY_AREA_DEPTH;            // rectangle禁区深度
+        extern const double GOAL_WIDTH;
+        extern const double GOAL_DEPTH;
+
         const double PITCH_MARGIN = 10;                   // 边界宽度
         const double CENTER_CIRCLE_R = 500;               // 中圈半径
 
         const double GOAL_POST_AVOID_LENGTH = 20;         //伸进场地内门柱的避障长度
         const double GOAL_POST_THICKNESS = 20;           //门柱宽度
-        const bool   IF_USE_ELLIPSE = false;             // whether use ellipse penalty
-        const double PENALTY_AREA_WIDTH = 2400;           // rectangle禁区宽度
-        const double PENALTY_AREA_DEPTH = 1200;            // rectangle禁区深度
-        const double PENALTY_AREA_R = 800;               // ellipse penalty 两个圆弧
-        const double PENALTY_AREA_L = 350;          // ellipse penalty 连接两个圆弧的线段
-        const double PENALTY_L = 500;						//代替PENALTY_AREA_L
+
         const double PENALTY_MARK_X = 4800;               // 点球点的X坐标
         const double OUTER_PENALTY_AREA_WIDTH = 1950;     // 外围禁区宽度(界外开球时不能站在该线内)
         const double FREE_KICK_AVOID_BALL_DIST = 500;     // 开任意球的时候,对方必须离球这么远
-    //  const double FIELD_WALL_DIST = 20;               // 场地护栏到边界的距离
-        const double GOAL_WIDTH = 1000;
-        const double GOAL_DEPTH = 200;
-        const double RATIO = 1.5;
     }
     namespace Rule{
         const int Version = 2019; // 规则的版本/年份
@@ -70,44 +66,16 @@ namespace PARAM {
             const double PLAYER_FRONT_TO_CENTER = 80.0;
             const double PLAYER_CENTER_TO_BALL_CENTER = 93;
             const double KICK_ANGLE = PARAM::Math::PI*30/180; // 可以击球的最大相对身体角度
-            const double DRIBBLE_SIZE = PLAYER_FRONT_TO_CENTER + PARAM::Field::BALL_SIZE; // 带球时离球的距离
             const double DRIBBLE_ANGLE = PARAM::Math::PI*17/180; // 可以带球的最大相对身体角度
             const double HEAD_ANGLE = 57*PARAM::Math::PI/180; // 前面的开口角度
-            //const double TOUCH_SHIFT_DIST = 10.06;		// Touch时后退的距离
-            const double TOUCH_SHIFT_DIST = 93;
         }
     }
     namespace AvoidDist{
         //避障所用参数
-        //const double TEAMMATE_AVOID_DIST =  PARAM::Field::MAX_PLAYER_SIZE/2+15.0f; // 厘米 12
         const double TEAMMATE_AVOID_DIST = PARAM::Vehicle::V2::PLAYER_SIZE*3;
         const double OPP_AVOID_DIST = PARAM::Field::MAX_PLAYER_SIZE; // 厘米 18
         const double BALL_AVOID_DIST = PARAM::Field::BALL_SIZE/2+20.0; // 厘米3
-        const double DEFENDKICK_MARKING_DIST = 65;
-    }
-    #ifdef _WIN32
-    namespace File{
-        const std::string DataDir = "data\\";
-        const char* const ParamDir = "params\\";
-        const std::string RobotPosFilterDir = "vision\\Robot_Param\\Pos\\";
-        const std::string RobotRotFilterDir = "vision\\Robot_Param\\Rotation\\";
-        const std::string BallFilterDir = "vision\\Ball_Param\\";
-        const std::string PlayBookPath = "play_books\\";
-        const std::string CBayesReader_SCRIPT_PATH = "params\\GameFilterParam\\";
-        const std::string OppConfigPath = "defence_config\\";
-    }
-    #else // not windows
-    namespace File{
-        const std::string DataDir = "data/";
-        const char* const ParamDir = "params/";
-        const std::string RobotPosFilterDir = "vision/Robot_Param/Pos/";
-        const std::string RobotRotFilterDir = "vision/Robot_Param/Rotation/";
-        const std::string BallFilterDir = "vision/Ball_Param/";
-        const std::string PlayBookPath = "play_books/";
-        const std::string CBayesReader_SCRIPT_PATH = "params/GameFilterParam/";
-        const std::string OppConfigPath = "defence_config/";
     }
-    #endif
 }
 namespace ZSS {
     const QString ZSS_ADDRESS = "233.233.233.233";

From 39eadcf7204b0eaaa6497948bcb095b4fbd23c48 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Sun, 14 Apr 2024 15:05:39 +0800
Subject: [PATCH 14/30] update .gitignore

---
 .gitignore | 73 +++++++++++++++++++++++++++---------------------------
 1 file changed, 36 insertions(+), 37 deletions(-)

diff --git a/.gitignore b/.gitignore
index c885792..1fd7515 100644
--- a/.gitignore
+++ b/.gitignore
@@ -52,56 +52,55 @@ Client/*.pb.*
 
 # C++ objects and libs
 
-Medusa/*.slo
-Medusa/*.lo
-Medusa/*.o
-Medusa/*.a
-Medusa/*.la
-Medusa/*.lai
-Medusa/*.so
-Medusa/*.dll
-Medusa/*.dylib
+Core/*.slo
+Core/*.lo
+Core/*.o
+Core/*.a
+Core/*.la
+Core/*.lai
+Core/*.so
+Core/*.dll
+Core/*.dylib
 
 # Qt-es
 
-Medusa/object_script.*.Release
-Medusa/object_script.*.Debug
-Medusa/*_plugin_import.cpp
-Medusa//.qmake.cache
-Medusa//.qmake.stash
-Medusa/*.pro.user
-Medusa/*.pro.user.*
-Medusa/*.qbs.user
-Medusa/*.qbs.user.*
-Medusa/*.moc
-Medusa/moc_*.cpp
-Medusa/moc_*.h
-Medusa/qrc_*.cpp
-Medusa/ui_*.h
-Medusa/Makefile*
-Medusa/*build*
+Core/object_script.*.Release
+Core/object_script.*.Debug
+Core/*_plugin_import.cpp
+Core//.qmake.cache
+Core//.qmake.stash
+Core/*.pro.user
+Core/*.pro.user.*
+Core/*.qbs.user
+Core/*.qbs.user.*
+Core/*.moc
+Core/moc_*.cpp
+Core/moc_*.h
+Core/qrc_*.cpp
+Core/ui_*.h
+Core/Makefile*
+Core/*build*
 
 # Qt unit tests
-Medusa/target_wrapper.*
+Core/target_wrapper.*
 
 
 # QtCreator
 
-Medusa/*.autosave
+Core/*.autosave
 
 # QtCtreator Qml
-Medusa/*.qmlproject.user
-Medusa/*.qmlproject.user.*
+Core/*.qmlproject.user
+Core/*.qmlproject.user.*
 
 # QtCtreator CMake
-Medusa/CMakeLists.txt.user*
+Core/CMakeLists.txt.user*
 
 # Protobuf
-Medusa/*.pb.*
-Medusa/*_pb2.py
-Medusa/src/LuaModule/lua_zeus.cpp
-Medusa/share/proto/cpp/
-
+Core/*.pb.*
+Core/*_pb2.py
+Core/src/LuaModule/lua_zeus.cpp
+Core/share/proto/cpp/
 
 
 
@@ -117,8 +116,8 @@ ZBin/LOG/*.log
 ZBin/Client.exp
 ZBin/Client.lib
 ZBin/Client
-ZBin/Medusa
-ZBin/MedusaD
+ZBin/Core
+ZBin/CoreD
 ZBin/grsim
 ZBin/Crazy
 ZBin/Client

From d03de8fec943a4553a21a40b5e98ed3a7ae590d1 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Sun, 14 Apr 2024 16:19:39 +0800
Subject: [PATCH 15/30] [Core] add direct_kick_no_calibration in command

---
 Client/CMakeLists.txt                   |   5 +
 Client/src/actionmodule.cpp             | 741 ++++++++++++------------
 Client/src/actionmodule.h               | 142 ++---
 Client/src/actionmodule_nan.cpp         |  20 -
 Client/src/communicator.cpp             |   2 +-
 Client/src/interaction.cpp              |  13 +-
 Client/src/viewerinterface.h            |   2 +-
 Core/src/Simulator/CommandInterface.cpp |   4 +-
 Core/src/Simulator/CommandInterface.h   |  22 +-
 share/proto/zss_cmd.proto               |   4 +
 10 files changed, 490 insertions(+), 465 deletions(-)
 delete mode 100644 Client/src/actionmodule_nan.cpp

diff --git a/Client/CMakeLists.txt b/Client/CMakeLists.txt
index 192cf25..5952b54 100644
--- a/Client/CMakeLists.txt
+++ b/Client/CMakeLists.txt
@@ -43,6 +43,11 @@ else()
     find_package(ZLIB)
 endif()
 
+## fmt
+find_package(fmt REQUIRED)
+include_directories(${fmt_INCLUDE_DIRS})
+list(APPEND libs fmt::fmt)
+
 include_directories(${ZLIB_INCLUDE_DIRS})
 list(APPEND libs ${ZLIB_LIBRARIES})
 ## find Qt
diff --git a/Client/src/actionmodule.cpp b/Client/src/actionmodule.cpp
index 3db37ef..9fbe3d0 100644
--- a/Client/src/actionmodule.cpp
+++ b/Client/src/actionmodule.cpp
@@ -4,10 +4,10 @@
 #include "dealrobot.h"
 #include "crc.h"
 #include "messageinfo.h"
-#include <QtDebug>
 #include <chrono>
 #include <thread>
 #include <cmath>
+#include <fmt/core.h>
 #include <QtSerialPort/QSerialPortInfo>
 #include "staticparams.h"
 #include "networkinterfaces.h"
@@ -23,8 +23,8 @@ const double LOW_BATTERY = 196.0;
 const double FULL_CAPACITANCE = 254.0;
 const double LOW_CAPACITANCE = 29.0;
 auto zpm = ZSS::ZParamManager::instance();
-void encodeLegacy(const ZSS::Protocol::Robot_Command&, QByteArray&, int);
-quint8 kickStandardization(quint8, bool, quint16);
+// void encodeLegacy(const ZSS::Protocol::Robot_Command&, QByteArray&, int);
+quint8 linearKickMapping(quint8 id, bool kick_mode, float power);
 const QStringList radioSendAddress2choose=  {"10.12.225.240", "10.12.225.241","10.12.225.109","10.12.225.78"};
 const QStringList radioReceiveAddress2choose =  {"10.12.225.240", "10.12.225.241","10.12.225.110","10.12.225.79"};
 QString radioSendAddress[PARAM::TEAMS] = {radioSendAddress2choose[0],radioSendAddress2choose[1]};
@@ -46,282 +46,284 @@ struct NJ_Command{
     bool kick_mode;
     bool use_dir;
     bool need_report;
+    bool direct_kick_no_calibration;
+    float direct_kick_power;
 };
 void encodeNJLegacy(const NJ_Command&,QByteArray&,int);
 NJ_Command NJ_CMDS[PARAM::ROBOTMAXID];
 }
-Action_Serial24L01::Action_Serial24L01(const Config& config,const __callback_type& f,QObject *parent):QObject(parent),_cb(f),_config(config){}
-Action_Serial24L01::~Action_Serial24L01(){
-    disconnect();
-}
-bool Action_Serial24L01::start(){
-    return false;
-}
-bool Action_Serial24L01::stop(){
-    return true;
-}
-bool Action_Serial24L01::send(const char* ptr,const size_t size){
-
-}
-bool sendConfigPacket(const char* ptr,const size_t size){
-    return true;
-}
-void Action_Serial24L01::receiveData(){
-    static QByteArray datagram;
-//    while (_recvSocket.hasPendingDatagrams()) {
-//        datagram.resize(_recvSocket.pendingDatagramSize());
-//        _recvSocket.readDatagram(datagram.data(), datagram.size());
-//        if(_cb){
-//            std::invoke(_cb,datagram.data(),datagram.size());
-//        }
-//    }
-}
-
-Action_UDP24L01::Action_UDP24L01(const Config& config,const __callback_type& f,QObject *parent):QObject(parent),_cb(f),_config(config){}
-Action_UDP24L01::~Action_UDP24L01(){
-    disconnect();
-}
-bool Action_UDP24L01::start(){
-    if(_recvSocket.bind(QHostAddress::AnyIPv4, _config.recv_port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint)) {
-        QObject::connect(&_recvSocket, SIGNAL(readyRead()), this, SLOT(receiveData()), Qt::DirectConnection);
-        return true;
-    }
-    qDebug() << "Bind Error in Action_UDP24L01 !";
-    return false;
-}
-bool Action_UDP24L01::stop(){
-    QObject::disconnect(&_recvSocket);
-    _recvSocket.abort();
-    _recvSocket.disconnectFromHost();
-    return true;
-}
-void Action_UDP24L01::reConfig(const Config& config){
-    _config = config;
-    // stop();
-    // start();
-}
-bool Action_UDP24L01::sendConfigPacket(const char* ptr,const size_t size){
-    _sendSocket.writeDatagram(ptr,size,QHostAddress(_config.send_ip),_config.send_port);
-    _sendSocket.writeDatagram(ptr,size,QHostAddress(_config.recv_ip),_config.recv_port);
-}
-bool Action_UDP24L01::send(const char* ptr,const size_t size){
-    _sendSocket.writeDatagram(ptr,size,QHostAddress(_config.send_ip),_config.send_port);
-}
-void Action_UDP24L01::receiveData(){
-    static QByteArray datagram;
-    while (_recvSocket.hasPendingDatagrams()) {
-        datagram.resize(_recvSocket.pendingDatagramSize());
-        _recvSocket.readDatagram(datagram.data(), datagram.size());
-        if(_cb){
-            std::invoke(_cb,datagram.data(),datagram.size());
-        }
-    }
-}
-ActionModule::ActionModule(QObject *parent) : QObject(parent), team{-1, -1} {
-//    blue_sender_interface = blue_receiver_interface = yellow_sender_interface = yellow_receiver_interface = 0;
-    tx.resize(TRANSMIT_PACKET_SIZE);
-    tx[0] = 0x40;
-//    QObject::connect(&receiveSocket, SIGNAL(readyRead()), this, SLOT(readData()), Qt::DirectConnection);
-    bool newType;
-    for(int i=0;i<PARAM::ROBOTNUM;i++){
-        zpm->loadParam(newType,QString("RobotProtocol/%1New?").arg(i, 2, 10, QChar('0')),false);
-        _protocolType[i] = newType? ProtocolType::UDP_24L01 : ProtocolType::Serial_24L01;
-    }
-    if(receiveSocket.bind(QHostAddress::AnyIPv4, PORT_RECEIVE, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint)) {
-        qDebug() << "****** start receive ! ******";
-//        receiveThread = new std::thread([ = ] {readData();});
-//        receiveThread->detach();
-    } else {
-        qDebug() << "Bind Error in action module !";
-    }
-}
-
-ActionModule::~ActionModule() {
-    sendSocket.disconnectFromHost();
-    receiveSocket.disconnectFromHost();
-}
-
-bool ActionModule::connectRadio(int id, int frq) {
-    bool color;
-    if(id >= 0 && id < PARAM::TEAMS) {
-        zpm->loadParam(color, "ZAlert/IsYellow", false);
-        team[id] = color ? PARAM::YELLOW : PARAM::BLUE;
-        qDebug() << "connectRadio : " << id << (color ? "YELLOW" : "BLUE") << frq;
-        sendStartPacket(id, frq);
-        return true;
-    } else
-        qDebug() << "ERROR id in connectRadio function!";
-    return false;
-}
-
-bool ActionModule::disconnectRadio(int id) {
-    if(id >= 0 && id < PARAM::TEAMS) {
-        team[id] = -1;
-        sendSocket.disconnectFromHost();
-        return true;
-    } else
-        qDebug() << "ERROR id in disconnectRadio function!";
-    return false;
-}
-void ActionModule::setSimulation(bool simulation){
-    IS_SIMULATION = simulation;
-}
-void ActionModule::sendStartPacket(int t, int frequency) {
-    // this 't' is id
-    QByteArray startPacketSend(TRANSMIT_START_PACKET_SIZE, 0);
-    QByteArray startPacketReceive(TRANSMIT_START_PACKET_SIZE, 0);
-    int freq_encode = 0;
-    if(frequency < 8){
-        freq_encode = frequency*4;
-    }else{
-        freq_encode = frequency*4+58;
-    }
-    startPacketSend[0] = (char)0xf0;
-    startPacketSend[1] = (char)(freq_encode & 0xff);
-    startPacketSend[2] = (char)(freq_encode & 0xff);
-    startPacketSend[3] = (char)0x01;
-    startPacketSend[4] = (char)0x02;
-    int temp_value = 0;
-    for(int i=0;i<TRANSMIT_START_PACKET_SIZE-1;i++){
-        temp_value += startPacketSend[i];
-    }
-    startPacketSend[TRANSMIT_START_PACKET_SIZE-1] = (char)(temp_value & 0xff);
-
-    startPacketReceive[0] = (char)0xf0;
-    startPacketReceive[1] = (char)(freq_encode & 0xff);
-    startPacketReceive[2] = (char)(freq_encode & 0xff);
-    startPacketReceive[3] = (char)0x02;
-    startPacketReceive[4] = (char)0x02;
-    temp_value = 0;
-    for(int i=0;i<TRANSMIT_START_PACKET_SIZE-1;i++){
-        temp_value += startPacketReceive[i];
-    }
-    startPacketReceive[TRANSMIT_START_PACKET_SIZE-1] = (char)(temp_value & 0xff);
-    sendSocket.writeDatagram(startPacketSend, TRANSMIT_START_PACKET_SIZE, QHostAddress(radioSendAddress[t]), PORT_SEND);
-    receiveSocket.writeDatagram(startPacketReceive, TRANSMIT_START_PACKET_SIZE, QHostAddress(radioReceiveAddress[t]), PORT_SEND);
-    std::this_thread::sleep_for(std::chrono::milliseconds(4));
-    qDebug() << "Frequency:" << frequency << " Send IP:" << radioSendAddress[t] << " Receive IP:" << radioReceiveAddress[t];
-}
-
-void ActionModule::sendLegacy(int t, const ZSS::Protocol::Robots_Command& commands) {
-    // this 't' is color
-    auto& socket = sendSocket;
-    int id = -1;
-    if(t == team[0])
-        id = 0;
-    else if(t == team[1])
-        id = 1;
-    else
-        return;
-    int size = commands.command_size();
-    int count = 0;
-    tx.fill(0x00);
-    tx[0] = 0x40;
-    for(int i = 0; i < size; i++) {
-        if(count == 4) {
-            socket.writeDatagram(tx.data(), TRANSMIT_PACKET_SIZE, QHostAddress(radioSendAddress[id]), PORT_SEND);
-            std::this_thread::sleep_for(std::chrono::microseconds(3000));
-            tx.fill(0x00);
-            tx[0] = 0x40;
-            count = 0;
-        }
-
-        auto& command = commands.command(i);
-        if(_protocolType[command.robot_id()] == ProtocolType::Serial_24L01){
-            continue;
-        }
-        encodeLegacy(command, this->tx, count++);
-    }
-    //qDebug() << "sendLegacy : " << (t ? "Yellow" : "Blue") << id << "size:" << size;
-    socket.writeDatagram(tx.data(), TRANSMIT_PACKET_SIZE, QHostAddress(radioSendAddress[id]), PORT_SEND);
-}
-
-void ActionModule::readData() {
-    static QHostAddress address;
-    static int color;
-    static int count[PARAM::TEAMS][PARAM::ROBOTNUM];
-    for (int color = PARAM::BLUE; color < PARAM::TEAMS; color++) {
-        for (int j = 0; j < PARAM::ROBOTNUM; j++ ) {
-            count[color][j] = 0;
-        }
-    }
-    while(true) {
-        std::this_thread::sleep_for(std::chrono::microseconds(500));
-//        ZSS::ZParamManager::instance()->loadParam(isSimulation, "Alert/IsSimulation", false);
-        if(!IS_SIMULATION) {
-            for (int color = PARAM::BLUE; color < PARAM::TEAMS; color++) {
-                for (int j = 0; j < PARAM::ROBOTNUM; j++ ) {
-                    if (count[color][j]++ > 1000) {
-                        GlobalData::instance()->robotInfoMutex.lock();
-                        GlobalData::instance()->robotInformation[color][j].infrared = false;
-                        GlobalData::instance()->robotInformation[color][j].flat = false;
-                        GlobalData::instance()->robotInformation[color][j].chip = false;
-                        count[color][j] = 0;
-//                        qDebug() << "FUCK" << color << j;
-                        GlobalData::instance()->robotInfoMutex.unlock();
-                        emit receiveRobotInfo(color, j);
-                    }
-                }
-            }
-        }
-        while (receiveSocket.state() == QUdpSocket::BoundState && receiveSocket.hasPendingDatagrams()) {
-            qDebug() << "receive data !!!";
-            auto msgInfo = (MessageInfo*)(MessageInfo::instance());
-            char newInfo = msgInfo->info() | 0x02;
-            //        msgInfo->setInfo(newInfo);
-            rx.resize(receiveSocket.pendingDatagramSize());
-            receiveSocket.readDatagram(rx.data(), rx.size(), &address);
-            color = (address.toString() == radioReceiveAddress[0]) ? team[0] : team[1];
-            if (color == -1) {
-                qDebug() << "Receive Error Message from:" << address << "in actionmodule.cpp";
-                break;
-            }
-            auto& data = rx;
-            int id = 0;
-            bool infrared = false;
-            bool flat = false;
-            bool chip = false;
-            int battery = 0;
-            int capacitance = 0;
-            short wheelVel[4] = {0};
-
-            if(data[0] == (char)0xff && data[1] == (char)0x02) {
-                id       = (quint8)data[2] - 1;//real robot 1-12 -> 0-11
-                infrared = (quint8)data[3] & 0x40;
-                flat     = (quint8)data[3] & 0x20;
-                chip     = (quint8)data[3] & 0x10;
-                battery  = (quint8)data[4];
-                capacitance = (quint8)data[5];
-                wheelVel[0] = (quint16)(data[6] << 8) + data[7];
-                wheelVel[1] = 1 + (short)~(data[8] << 8) + data[9];
-                wheelVel[2] = 1 + (short)~(data[10] << 8) + data[11];
-                wheelVel[3] = (quint16)(data[12] << 8) + data[13];
-
-                GlobalData::instance()->robotInfoMutex.lock();
-                count[color][id] = 0;
-                GlobalData::instance()->robotInformation[color][id].infrared = infrared;
-                GlobalData::instance()->robotInformation[color][id].flat = flat;
-                GlobalData::instance()->robotInformation[color][id].chip = chip;
-                GlobalData::instance()->robotInformation[color][id].battery = std::min(std::max((battery - LOW_BATTERY) / (FULL_BATTERY - LOW_BATTERY), 0.0), 1.0);
-                GlobalData::instance()->robotInformation[color][id].capacitance = std::min(std::max((capacitance - LOW_CAPACITANCE) / (FULL_CAPACITANCE - LOW_CAPACITANCE), 0.0), 1.0);
-                GlobalData::instance()->robotInfoMutex.unlock();
-                emit receiveRobotInfo(color, id);
-            }
-//            qDebug() << rx.toHex();
-            qDebug() << color << id << infrared << flat << address;
-        }
-    }
-}
-void ActionModule::changeAddress(int team, int index){
-    radioSendAddress[team] = radioSendAddress2choose[index];
-    radioReceiveAddress[team] = radioReceiveAddress2choose[index];
-}
-QStringList ActionModule::getAllAddress(){
-    return radioSendAddress2choose;
-}
-QString ActionModule::getRealAddress(int index){
-    return radioSendAddress[index];
-}
+// Action_Serial24L01::Action_Serial24L01(const Config& config,const __callback_type& f,QObject *parent):QObject(parent),_cb(f),_config(config){}
+// Action_Serial24L01::~Action_Serial24L01(){
+//     disconnect();
+// }
+// bool Action_Serial24L01::start(){
+//     return false;
+// }
+// bool Action_Serial24L01::stop(){
+//     return true;
+// }
+// bool Action_Serial24L01::send(const char* ptr,const size_t size){
+
+// }
+// bool sendConfigPacket(const char* ptr,const size_t size){
+//     return true;
+// }
+// void Action_Serial24L01::receiveData(){
+//     static QByteArray datagram;
+// //    while (_recvSocket.hasPendingDatagrams()) {
+// //        datagram.resize(_recvSocket.pendingDatagramSize());
+// //        _recvSocket.readDatagram(datagram.data(), datagram.size());
+// //        if(_cb){
+// //            std::invoke(_cb,datagram.data(),datagram.size());
+// //        }
+// //    }
+// }
+
+// Action_UDP24L01::Action_UDP24L01(const Config& config,const __callback_type& f,QObject *parent):QObject(parent),_cb(f),_config(config){}
+// Action_UDP24L01::~Action_UDP24L01(){
+//     disconnect();
+// }
+// bool Action_UDP24L01::start(){
+//     if(_recvSocket.bind(QHostAddress::AnyIPv4, _config.recv_port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint)) {
+//         QObject::connect(&_recvSocket, SIGNAL(readyRead()), this, SLOT(receiveData()), Qt::DirectConnection);
+//         return true;
+//     }
+//     qDebug() << "Bind Error in Action_UDP24L01 !";
+//     return false;
+// }
+// bool Action_UDP24L01::stop(){
+//     QObject::disconnect(&_recvSocket);
+//     _recvSocket.abort();
+//     _recvSocket.disconnectFromHost();
+//     return true;
+// }
+// void Action_UDP24L01::reConfig(const Config& config){
+//     _config = config;
+//     // stop();
+//     // start();
+// }
+// bool Action_UDP24L01::sendConfigPacket(const char* ptr,const size_t size){
+//     _sendSocket.writeDatagram(ptr,size,QHostAddress(_config.send_ip),_config.send_port);
+//     _sendSocket.writeDatagram(ptr,size,QHostAddress(_config.recv_ip),_config.recv_port);
+// }
+// bool Action_UDP24L01::send(const char* ptr,const size_t size){
+//     _sendSocket.writeDatagram(ptr,size,QHostAddress(_config.send_ip),_config.send_port);
+// }
+// void Action_UDP24L01::receiveData(){
+//     static QByteArray datagram;
+//     while (_recvSocket.hasPendingDatagrams()) {
+//         datagram.resize(_recvSocket.pendingDatagramSize());
+//         _recvSocket.readDatagram(datagram.data(), datagram.size());
+//         if(_cb){
+//             std::invoke(_cb,datagram.data(),datagram.size());
+//         }
+//     }
+// }
+// ActionModule::ActionModule(QObject *parent) : QObject(parent), team{-1, -1} {
+// //    blue_sender_interface = blue_receiver_interface = yellow_sender_interface = yellow_receiver_interface = 0;
+//     tx.resize(TRANSMIT_PACKET_SIZE);
+//     tx[0] = 0x40;
+// //    QObject::connect(&receiveSocket, SIGNAL(readyRead()), this, SLOT(readData()), Qt::DirectConnection);
+//     bool newType;
+//     for(int i=0;i<PARAM::ROBOTNUM;i++){
+//         zpm->loadParam(newType,QString("RobotProtocol/%1New?").arg(i, 2, 10, QChar('0')),false);
+//         _protocolType[i] = newType? ProtocolType::UDP_24L01 : ProtocolType::Serial_24L01;
+//     }
+//     if(receiveSocket.bind(QHostAddress::AnyIPv4, PORT_RECEIVE, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint)) {
+//         qDebug() << "****** start receive ! ******";
+// //        receiveThread = new std::thread([ = ] {readData();});
+// //        receiveThread->detach();
+//     } else {
+//         qDebug() << "Bind Error in action module !";
+//     }
+// }
+
+// ActionModule::~ActionModule() {
+//     sendSocket.disconnectFromHost();
+//     receiveSocket.disconnectFromHost();
+// }
+
+// bool ActionModule::connectRadio(int id, int frq) {
+//     bool color;
+//     if(id >= 0 && id < PARAM::TEAMS) {
+//         zpm->loadParam(color, "ZAlert/IsYellow", false);
+//         team[id] = color ? PARAM::YELLOW : PARAM::BLUE;
+//         qDebug() << "connectRadio : " << id << (color ? "YELLOW" : "BLUE") << frq;
+//         sendStartPacket(id, frq);
+//         return true;
+//     } else
+//         qDebug() << "ERROR id in connectRadio function!";
+//     return false;
+// }
+
+// bool ActionModule::disconnectRadio(int id) {
+//     if(id >= 0 && id < PARAM::TEAMS) {
+//         team[id] = -1;
+//         sendSocket.disconnectFromHost();
+//         return true;
+//     } else
+//         qDebug() << "ERROR id in disconnectRadio function!";
+//     return false;
+// }
+// void ActionModule::setSimulation(bool simulation){
+//     IS_SIMULATION = simulation;
+// }
+// void ActionModule::sendStartPacket(int t, int frequency) {
+//     // this 't' is id
+//     QByteArray startPacketSend(TRANSMIT_START_PACKET_SIZE, 0);
+//     QByteArray startPacketReceive(TRANSMIT_START_PACKET_SIZE, 0);
+//     int freq_encode = 0;
+//     if(frequency < 8){
+//         freq_encode = frequency*4;
+//     }else{
+//         freq_encode = frequency*4+58;
+//     }
+//     startPacketSend[0] = (char)0xf0;
+//     startPacketSend[1] = (char)(freq_encode & 0xff);
+//     startPacketSend[2] = (char)(freq_encode & 0xff);
+//     startPacketSend[3] = (char)0x01;
+//     startPacketSend[4] = (char)0x02;
+//     int temp_value = 0;
+//     for(int i=0;i<TRANSMIT_START_PACKET_SIZE-1;i++){
+//         temp_value += startPacketSend[i];
+//     }
+//     startPacketSend[TRANSMIT_START_PACKET_SIZE-1] = (char)(temp_value & 0xff);
+
+//     startPacketReceive[0] = (char)0xf0;
+//     startPacketReceive[1] = (char)(freq_encode & 0xff);
+//     startPacketReceive[2] = (char)(freq_encode & 0xff);
+//     startPacketReceive[3] = (char)0x02;
+//     startPacketReceive[4] = (char)0x02;
+//     temp_value = 0;
+//     for(int i=0;i<TRANSMIT_START_PACKET_SIZE-1;i++){
+//         temp_value += startPacketReceive[i];
+//     }
+//     startPacketReceive[TRANSMIT_START_PACKET_SIZE-1] = (char)(temp_value & 0xff);
+//     sendSocket.writeDatagram(startPacketSend, TRANSMIT_START_PACKET_SIZE, QHostAddress(radioSendAddress[t]), PORT_SEND);
+//     receiveSocket.writeDatagram(startPacketReceive, TRANSMIT_START_PACKET_SIZE, QHostAddress(radioReceiveAddress[t]), PORT_SEND);
+//     std::this_thread::sleep_for(std::chrono::milliseconds(4));
+//     qDebug() << "Frequency:" << frequency << " Send IP:" << radioSendAddress[t] << " Receive IP:" << radioReceiveAddress[t];
+// }
+
+// void ActionModule::sendLegacy(int t, const ZSS::Protocol::Robots_Command& commands) {
+//     // this 't' is color
+//     auto& socket = sendSocket;
+//     int id = -1;
+//     if(t == team[0])
+//         id = 0;
+//     else if(t == team[1])
+//         id = 1;
+//     else
+//         return;
+//     int size = commands.command_size();
+//     int count = 0;
+//     tx.fill(0x00);
+//     tx[0] = 0x40;
+//     for(int i = 0; i < size; i++) {
+//         if(count == 4) {
+//             socket.writeDatagram(tx.data(), TRANSMIT_PACKET_SIZE, QHostAddress(radioSendAddress[id]), PORT_SEND);
+//             std::this_thread::sleep_for(std::chrono::microseconds(3000));
+//             tx.fill(0x00);
+//             tx[0] = 0x40;
+//             count = 0;
+//         }
+
+//         auto& command = commands.command(i);
+//         if(_protocolType[command.robot_id()] == ProtocolType::Serial_24L01){
+//             continue;
+//         }
+//         encodeLegacy(command, this->tx, count++);
+//     }
+//     //qDebug() << "sendLegacy : " << (t ? "Yellow" : "Blue") << id << "size:" << size;
+//     socket.writeDatagram(tx.data(), TRANSMIT_PACKET_SIZE, QHostAddress(radioSendAddress[id]), PORT_SEND);
+// }
+
+// void ActionModule::readData() {
+//     static QHostAddress address;
+//     static int color;
+//     static int count[PARAM::TEAMS][PARAM::ROBOTNUM];
+//     for (int color = PARAM::BLUE; color < PARAM::TEAMS; color++) {
+//         for (int j = 0; j < PARAM::ROBOTNUM; j++ ) {
+//             count[color][j] = 0;
+//         }
+//     }
+//     while(true) {
+//         std::this_thread::sleep_for(std::chrono::microseconds(500));
+// //        ZSS::ZParamManager::instance()->loadParam(isSimulation, "Alert/IsSimulation", false);
+//         if(!IS_SIMULATION) {
+//             for (int color = PARAM::BLUE; color < PARAM::TEAMS; color++) {
+//                 for (int j = 0; j < PARAM::ROBOTNUM; j++ ) {
+//                     if (count[color][j]++ > 1000) {
+//                         GlobalData::instance()->robotInfoMutex.lock();
+//                         GlobalData::instance()->robotInformation[color][j].infrared = false;
+//                         GlobalData::instance()->robotInformation[color][j].flat = false;
+//                         GlobalData::instance()->robotInformation[color][j].chip = false;
+//                         count[color][j] = 0;
+// //                        qDebug() << "FUCK" << color << j;
+//                         GlobalData::instance()->robotInfoMutex.unlock();
+//                         emit receiveRobotInfo(color, j);
+//                     }
+//                 }
+//             }
+//         }
+//         while (receiveSocket.state() == QUdpSocket::BoundState && receiveSocket.hasPendingDatagrams()) {
+//             qDebug() << "receive data !!!";
+//             auto msgInfo = (MessageInfo*)(MessageInfo::instance());
+//             char newInfo = msgInfo->info() | 0x02;
+//             //        msgInfo->setInfo(newInfo);
+//             rx.resize(receiveSocket.pendingDatagramSize());
+//             receiveSocket.readDatagram(rx.data(), rx.size(), &address);
+//             color = (address.toString() == radioReceiveAddress[0]) ? team[0] : team[1];
+//             if (color == -1) {
+//                 qDebug() << "Receive Error Message from:" << address << "in actionmodule.cpp";
+//                 break;
+//             }
+//             auto& data = rx;
+//             int id = 0;
+//             bool infrared = false;
+//             bool flat = false;
+//             bool chip = false;
+//             int battery = 0;
+//             int capacitance = 0;
+//             short wheelVel[4] = {0};
+
+//             if(data[0] == (char)0xff && data[1] == (char)0x02) {
+//                 id       = (quint8)data[2] - 1;//real robot 1-12 -> 0-11
+//                 infrared = (quint8)data[3] & 0x40;
+//                 flat     = (quint8)data[3] & 0x20;
+//                 chip     = (quint8)data[3] & 0x10;
+//                 battery  = (quint8)data[4];
+//                 capacitance = (quint8)data[5];
+//                 wheelVel[0] = (quint16)(data[6] << 8) + data[7];
+//                 wheelVel[1] = 1 + (short)~(data[8] << 8) + data[9];
+//                 wheelVel[2] = 1 + (short)~(data[10] << 8) + data[11];
+//                 wheelVel[3] = (quint16)(data[12] << 8) + data[13];
+
+//                 GlobalData::instance()->robotInfoMutex.lock();
+//                 count[color][id] = 0;
+//                 GlobalData::instance()->robotInformation[color][id].infrared = infrared;
+//                 GlobalData::instance()->robotInformation[color][id].flat = flat;
+//                 GlobalData::instance()->robotInformation[color][id].chip = chip;
+//                 GlobalData::instance()->robotInformation[color][id].battery = std::min(std::max((battery - LOW_BATTERY) / (FULL_BATTERY - LOW_BATTERY), 0.0), 1.0);
+//                 GlobalData::instance()->robotInformation[color][id].capacitance = std::min(std::max((capacitance - LOW_CAPACITANCE) / (FULL_CAPACITANCE - LOW_CAPACITANCE), 0.0), 1.0);
+//                 GlobalData::instance()->robotInfoMutex.unlock();
+//                 emit receiveRobotInfo(color, id);
+//             }
+// //            qDebug() << rx.toHex();
+//             qDebug() << color << id << infrared << flat << address;
+//         }
+//     }
+// }
+// void ActionModule::changeAddress(int team, int index){
+//     radioSendAddress[team] = radioSendAddress2choose[index];
+//     radioReceiveAddress[team] = radioReceiveAddress2choose[index];
+// }
+// QStringList ActionModule::getAllAddress(){
+//     return radioSendAddress2choose;
+// }
+// QString ActionModule::getRealAddress(int index){
+//     return radioSendAddress[index];
+// }
 
 ActionModuleSerialVersion::ActionModuleSerialVersion(QObject *parent) : QObject(parent) {
     bool newType;
@@ -430,6 +432,10 @@ void ActionModuleSerialVersion::sendLegacy(const ZSS::Protocol::Robots_Command&
             NJ_CMDS[id].dribble = command.dribbler_spin();
             NJ_CMDS[id].power = command.power()/10.0;
             NJ_CMDS[id].kick_mode = command.kick();
+            if (command.has_direct_kick_no_calibration()){
+                NJ_CMDS[id].direct_kick_no_calibration = command.direct_kick_no_calibration();
+                NJ_CMDS[id].direct_kick_power = command.direct_kick_power();
+            }
         }
     }
 
@@ -520,77 +526,77 @@ void ActionModuleSerialVersion::readData(){
 }
 
 namespace {
-void encodeLegacy(const ZSS::Protocol::Robot_Command& command, QByteArray& tx, int num) {
-    // send back to vision module
-    // num 0 ~ 3
-    // id  0 ~ 15
-    quint8 id = (quint8)command.robot_id();
-    double origin_vx = command.velocity_x();
-    double origin_vy = command.velocity_y();
-    double origin_vr = command.velocity_r();
-    double dt = 1. / Athena::FRAME_RATE;
-    double theta = - origin_vr * dt;
-    CVector v(origin_vx, origin_vy);
-    v = v.rotate(theta);
-    if (fabs(theta) > 0.00001) {
-        //            if (i==0) cout << theta << " " <<vx << " "<< vy << " ";
-        v = v * theta / (2 * sin(theta / 2));
-        origin_vx = v.x();
-        origin_vy = v.y();
-        //            if (i==0) cout << vx << " "<< vy << " " << endl;
-    }
-
-
-    qint16 vx = (qint16)(origin_vx/10.0);
-    qint16 vy = (qint16)(origin_vy/10.0);
-    qint16 vr = (qint16)(origin_vr * 160);
-    qint16 abs_vx = std::abs(vx);
-    qint16 abs_vy = std::abs(vy);
-    qint16 abs_vr = std::abs(vr);
-    //if(abs(vx) > 0.1) qDebug() << "id: " << id<< "  "<<num*4 + 1<< "  "<<vx;
-    // flat&chip m/s -> cm/s
-    // kick   1 : chip   0 : flat`
-    bool kick = command.kick();
-    quint16 speed = command.power();
-    quint8 power = 0;
-    if(speed > 0.5) {
-        power = kickStandardization(id, kick, (quint16)(command.power()));
-    }
-//    qDebug() << "id: " << id << "power: " << power << "speed: " << command.power();
-    // dribble -1 ~ +1 -> -3 ~ +3
-    qint8 dribble = command.dribbler_spin() > 0.5 ? 3 : 0;
-    tx[0] = (tx[0]) | (1 << (3 - num));
-    tx[num * 4 + 1] = ((quint8)kick << 6) | dribble << 4 | id;
-    tx[num * 4 + 2] = (vx >> 8 & 0x80) | (abs_vx & 0x7f);
-    tx[num * 4 + 3] = (vy >> 8 & 0x80) | (abs_vy & 0x7f);
-    tx[num * 4 + 4] = (vr >> 8 & 0x80) | (abs_vr & 0x7f);
-    tx[num  + 17] = (abs_vx >> 1 & 0xc0) | (abs_vy >> 3 & 0x30) | (abs_vr >> 7 & 0x0f);
-    tx[num  + 21] = power;
-}
-
-quint8 kickStandardization(quint8 id, bool mode, quint16 power) {
-    //if(power > 1) qDebug() << "id: "<<id<<" power: "<<power;
-    double new_power = 0;
-    double a, b, c;
-    double min_power, max_power;
-    QString key = "";
-
-    float ratio = mode ? 1 : 0.00001;
-    key = QString("Robot%1/%2_A").arg(id).arg(mode ? "CHIP" : "FLAT");
-    KParamManager::instance()->loadParam(a, key, 0);
-    key = QString("Robot%1/%2_B").arg(id).arg(mode ? "CHIP" : "FLAT");
-    KParamManager::instance()->loadParam(b, key, 1);
-    key = QString("Robot%1/%2_C").arg(id).arg(mode ? "CHIP" : "FLAT");
-    KParamManager::instance()->loadParam(c, key, 0);
-    key = QString("Robot%1/%2_MIN").arg(id).arg(mode ? "CHIP" : "FLAT");
-    KParamManager::instance()->loadParam(min_power, key, 15);
-    key = QString("Robot%1/%2_MAX").arg(id).arg(mode ? "CHIP" : "FLAT");
-    KParamManager::instance()->loadParam(max_power, key, 70);
-    new_power = (int)( ratio * a * power * power + b * power + c);
-    new_power = std::max(min_power, std::min(new_power, max_power));
-    new_power = std::max(10.0, std::min(new_power, 127.0));
-    return (quint8)new_power;
-}
+// void encodeLegacy(const ZSS::Protocol::Robot_Command& command, QByteArray& tx, int num) {
+//     // send back to vision module
+//     // num 0 ~ 3
+//     // id  0 ~ 15
+//     quint8 id = (quint8)command.robot_id();
+//     double origin_vx = command.velocity_x();
+//     double origin_vy = command.velocity_y();
+//     double origin_vr = command.velocity_r();
+//     double dt = 1. / Athena::FRAME_RATE;
+//     double theta = - origin_vr * dt;
+//     CVector v(origin_vx, origin_vy);
+//     v = v.rotate(theta);
+//     if (fabs(theta) > 0.00001) {
+//         //            if (i==0) cout << theta << " " <<vx << " "<< vy << " ";
+//         v = v * theta / (2 * sin(theta / 2));
+//         origin_vx = v.x();
+//         origin_vy = v.y();
+//         //            if (i==0) cout << vx << " "<< vy << " " << endl;
+//     }
+
+
+//     qint16 vx = (qint16)(origin_vx/10.0);
+//     qint16 vy = (qint16)(origin_vy/10.0);
+//     qint16 vr = (qint16)(origin_vr * 160);
+//     qint16 abs_vx = std::abs(vx);
+//     qint16 abs_vy = std::abs(vy);
+//     qint16 abs_vr = std::abs(vr);
+//     //if(abs(vx) > 0.1) qDebug() << "id: " << id<< "  "<<num*4 + 1<< "  "<<vx;
+//     // flat&chip m/s -> cm/s
+//     // kick   1 : chip   0 : flat`
+//     bool kick = command.kick();
+//     quint16 speed = command.power();
+//     quint8 power = 0;
+//     if(speed > 0.5) {
+//         power = kickStandardization(id, kick, (quint16)(command.power()));
+//     }
+// //    qDebug() << "id: " << id << "power: " << power << "speed: " << command.power();
+//     // dribble -1 ~ +1 -> -3 ~ +3
+//     qint8 dribble = command.dribbler_spin() > 0.5 ? 3 : 0;
+//     tx[0] = (tx[0]) | (1 << (3 - num));
+//     tx[num * 4 + 1] = ((quint8)kick << 6) | dribble << 4 | id;
+//     tx[num * 4 + 2] = (vx >> 8 & 0x80) | (abs_vx & 0x7f);
+//     tx[num * 4 + 3] = (vy >> 8 & 0x80) | (abs_vy & 0x7f);
+//     tx[num * 4 + 4] = (vr >> 8 & 0x80) | (abs_vr & 0x7f);
+//     tx[num  + 17] = (abs_vx >> 1 & 0xc0) | (abs_vy >> 3 & 0x30) | (abs_vr >> 7 & 0x0f);
+//     tx[num  + 21] = power;
+// }
+
+// quint8 kickStandardization(quint8 id, bool mode, quint16 power) {
+//     //if(power > 1) qDebug() << "id: "<<id<<" power: "<<power;
+//     double new_power = 0;
+//     double a, b, c;
+//     double min_power, max_power;
+//     QString key = "";
+
+//     float ratio = mode ? 1 : 0.00001;
+//     key = QString("Robot%1/%2_A").arg(id).arg(mode ? "CHIP" : "FLAT");
+//     KParamManager::instance()->loadParam(a, key, 0);
+//     key = QString("Robot%1/%2_B").arg(id).arg(mode ? "CHIP" : "FLAT");
+//     KParamManager::instance()->loadParam(b, key, 1);
+//     key = QString("Robot%1/%2_C").arg(id).arg(mode ? "CHIP" : "FLAT");
+//     KParamManager::instance()->loadParam(c, key, 0);
+//     key = QString("Robot%1/%2_MIN").arg(id).arg(mode ? "CHIP" : "FLAT");
+//     KParamManager::instance()->loadParam(min_power, key, 15);
+//     key = QString("Robot%1/%2_MAX").arg(id).arg(mode ? "CHIP" : "FLAT");
+//     KParamManager::instance()->loadParam(max_power, key, 70);
+//     new_power = (int)( ratio * a * power * power + b * power + c);
+//     new_power = std::max(min_power, std::min(new_power, max_power));
+//     new_power = std::max(10.0, std::min(new_power, 127.0));
+//     return (quint8)new_power;
+// }
 double velRegulation(const int num,const double v){
     QString key = QString("Robot%1/vel").arg(num);
     double ratio = 1.0;
@@ -611,7 +617,6 @@ void encodeNJLegacy(const NJ_Command& command,QByteArray& tx,int num){
     vy = velRegulation(real_num,vy);
     vr = velRegulation(real_num,vr);
 
-    qint16 power = (qint16)(command.power);
     bool kick_mode = command.kick_mode;
     qint16 dribble = (qint16)(command.dribble*3+0.4);
     if(dribble > 3) dribble = 3;
@@ -643,17 +648,43 @@ void encodeNJLegacy(const NJ_Command& command,QByteArray& tx,int num){
     TXBuff[6*i + 6] = TXBuff[6*i + 6] | ((w_value_uint & 0x100) >> 8);
     TXBuff[6*i + 7] = TXBuff[6*i + 7] | (w_value_uint & 0x0ff);
 
-
     // shoot power
-    quint8 p = command.power > 0.01 ? std::max(10.0, std::min((double)power, 127.0)) : 0;
-    TXBuff[6*i + 8] = p;
+    quint8 p = 0;
+    if (command.direct_kick_no_calibration){
+        p = command.direct_kick_power;
+    }else{
+        p = command.power > 0.01 ? linearKickMapping(real_num,kick_mode,command.power) : 0;
+    }
+    TXBuff[6*i + 8] = p & 0x0ff;
 
 }
-quint8 kickStandardization(quint8 id,bool mode,float power){
-    quint8 res = 0;
-    if(power > 0.5)
-        res = (quint8)std::max(10.0, std::min((double)power, 127.0));
-    return res;
+// id - 0 ~ 15
+// kick_mode - 1 : chip 0 : flat
+// power FLAT : cm/s
+// power CHIP : cm of the first 
+quint8 linearKickMapping(quint8 id,bool mode,float power){
+    if(power < 0.5)
+        return 0;
+
+    double mapped_power = 0;
+    double B, C;
+    double MIN_POWER, MAX_POWER;
+    QString MODE = mode ? "CHIP" : "FLAT";
+    QString key = "";
+    key = QString("Robot%1/%2_B").arg(id).arg(MODE);
+    KParamManager::instance()->loadParam(B, key, 1);
+    key = QString("Robot%1/%2_C").arg(id).arg(MODE);
+    KParamManager::instance()->loadParam(C, key, 0);
+    key = QString("Robot%1/%2_MIN").arg(id).arg(MODE);
+    KParamManager::instance()->loadParam(MIN_POWER, key, 5);
+    key = QString("Robot%1/%2_MAX").arg(id).arg(MODE);
+    KParamManager::instance()->loadParam(MAX_POWER, key, 126);
+    mapped_power = std::max(MIN_POWER, std::min(B * power + C, MAX_POWER));
+
+    qDebug() << "mapping kp : " << QString::fromStdString(fmt::format("ID:{},MODE:{},POWER:{:.0f},MAPPED:{}",id,MODE.toStdString(),power,mapped_power));
+
+    return (quint8)std::max(0.0, std::min(mapped_power, 127.0));
+
 }
 } // namespace ZSS::anonymous
 }
diff --git a/Client/src/actionmodule.h b/Client/src/actionmodule.h
index c482728..7a46a7d 100644
--- a/Client/src/actionmodule.h
+++ b/Client/src/actionmodule.h
@@ -13,77 +13,77 @@ enum struct ProtocolType{
     UDP_24L01,
     Serial_24L01,
 };
-class Action_Serial24L01 : public QObject {
-    Q_OBJECT
-public:
-    struct Config{
-        QString portName;
-    };
-    Action_Serial24L01(const Config& config,const __callback_type& f={},QObject *parent=nullptr);
-    ~Action_Serial24L01();
-    bool start();
-    bool stop();
-    void reConfig(const Config& config);
-    bool sendConfigPacket(const char*,const size_t);
-    bool send(const char*,const size_t);
-private slots:
-    void receiveData();
-private:
-    Config _config;
-    QSerialPort serial;
-    const __callback_type _cb;
-};
-class Action_UDP24L01 : public QObject {
-    Q_OBJECT
-public:
-    struct Config{
-        QString send_ip;
-        int send_port;
-        QString recv_ip;
-        int recv_port;
-    };
-    Action_UDP24L01(const Config& config,const __callback_type& f={},QObject *parent=nullptr);
-    ~Action_UDP24L01();
-    bool start();
-    bool stop();
-    void reConfig(const Config& config);
-    bool sendConfigPacket(const char*,const size_t);
-    bool send(const char*,const size_t);
-private slots:
-    void receiveData();
-private:
-    Config _config;
-    QUdpSocket _sendSocket;
-    QUdpSocket _recvSocket;
-    const __callback_type _cb;
-};
-class ActionModule : public QObject {
-    Q_OBJECT
-  public:
-    ActionModule(QObject *parent = 0);
-    ~ActionModule();
-    void sendLegacy(int t, const ZSS::Protocol::Robots_Command&);
-    bool connectRadio(int, int);
-    bool disconnectRadio(int);
-    void setSimulation(bool);
-    int team[PARAM::TEAMS];
-    void changeAddress(int team,int index);
-    QStringList getAllAddress();
-    QString getRealAddress(int team);
-  private slots:
-    void readData();
-  private:
-    void sendStartPacket(int, int);
-  private:
-    QByteArray tx;
-    QByteArray rx;
-    QUdpSocket sendSocket;
-    QUdpSocket receiveSocket;
-    ProtocolType _protocolType[PARAM::ROBOTNUM];
-  signals:
-    void receiveRobotInfo(int, int);
-};
-typedef Singleton<ActionModule> ZActionModule;
+// class Action_Serial24L01 : public QObject {
+//     Q_OBJECT
+// public:
+//     struct Config{
+//         QString portName;
+//     };
+//     Action_Serial24L01(const Config& config,const __callback_type& f={},QObject *parent=nullptr);
+//     ~Action_Serial24L01();
+//     bool start();
+//     bool stop();
+//     void reConfig(const Config& config);
+//     bool sendConfigPacket(const char*,const size_t);
+//     bool send(const char*,const size_t);
+// private slots:
+//     void receiveData();
+// private:
+//     Config _config;
+//     QSerialPort serial;
+//     const __callback_type _cb;
+// };
+// class Action_UDP24L01 : public QObject {
+//     Q_OBJECT
+// public:
+//     struct Config{
+//         QString send_ip;
+//         int send_port;
+//         QString recv_ip;
+//         int recv_port;
+//     };
+//     Action_UDP24L01(const Config& config,const __callback_type& f={},QObject *parent=nullptr);
+//     ~Action_UDP24L01();
+//     bool start();
+//     bool stop();
+//     void reConfig(const Config& config);
+//     bool sendConfigPacket(const char*,const size_t);
+//     bool send(const char*,const size_t);
+// private slots:
+//     void receiveData();
+// private:
+//     Config _config;
+//     QUdpSocket _sendSocket;
+//     QUdpSocket _recvSocket;
+//     const __callback_type _cb;
+// };
+// class ActionModule : public QObject {
+//     Q_OBJECT
+//   public:
+//     ActionModule(QObject *parent = 0);
+//     ~ActionModule();
+//     void sendLegacy(int t, const ZSS::Protocol::Robots_Command&);
+//     bool connectRadio(int, int);
+//     bool disconnectRadio(int);
+//     void setSimulation(bool);
+//     int team[PARAM::TEAMS];
+//     void changeAddress(int team,int index);
+//     QStringList getAllAddress();
+//     QString getRealAddress(int team);
+//   private slots:
+//     void readData();
+//   private:
+//     void sendStartPacket(int, int);
+//   private:
+//     QByteArray tx;
+//     QByteArray rx;
+//     QUdpSocket sendSocket;
+//     QUdpSocket receiveSocket;
+//     ProtocolType _protocolType[PARAM::ROBOTNUM];
+//   signals:
+//     void receiveRobotInfo(int, int);
+// };
+// typedef Singleton<ActionModule> ZActionModule;
 
 class ActionModuleSerialVersion : public QObject
 {
diff --git a/Client/src/actionmodule_nan.cpp b/Client/src/actionmodule_nan.cpp
deleted file mode 100644
index e753105..0000000
--- a/Client/src/actionmodule_nan.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-#include "actionmodule.h"
-#include "actionmodule.h"
-#include "parammanager.h"
-#include "globaldata.h"
-
-#include "crc.h"
-#include <thread>
-#include <chrono>
-#include <QtDebug>
-namespace ZSS {
-namespace {
-auto zpm = ZSS::ZParamManager::instance();
-
-}
-
-namespace{
-
-} // namespace ZSS::anonymous
-}
-
diff --git a/Client/src/communicator.cpp b/Client/src/communicator.cpp
index a0f91fd..c448ede 100644
--- a/Client/src/communicator.cpp
+++ b/Client/src/communicator.cpp
@@ -98,7 +98,7 @@ void Communicator::receiveCommand(int t) {
                     ZSS::ZRemoteSimModule::instance()->sendSim(t, commands);
             } else {
 //                qDebug() << "realreal!";
-                ZSS::ZActionModule::instance()->sendLegacy(t, commands);
+                // ZSS::ZActionModule::instance()->sendLegacy(t, commands);
                 ZSS::NActionModule::instance()->sendLegacy(commands);
             }
         }
diff --git a/Client/src/interaction.cpp b/Client/src/interaction.cpp
index cef284e..6d464cf 100644
--- a/Client/src/interaction.cpp
+++ b/Client/src/interaction.cpp
@@ -91,7 +91,7 @@ void Interaction::showIfEdgeTest() {
     std::cout << VisionModule::instance()->showIfEdgeTest() << std::endl;
 }
 void Interaction::setVision(bool needStart, bool real) {
-    ZSS::ZActionModule::instance()->setSimulation(!real);
+    // ZSS::ZActionModule::instance()->setSimulation(!real);
     if (needStart) {
         ZSS::ZParamManager::instance()->changeParam("Alert/IsSimulation", !real);
         ZCommunicator::instance()->reloadSimulation();
@@ -116,11 +116,12 @@ bool Interaction::connectRadio(bool sw, int id, int frq) {
     if(sw) {
         ZCommunicator::instance()->disconnectMedusa(id);
         ZCommunicator::instance()->connectMedusa(id);
-        ZSS::ZActionModule::instance()->disconnectRadio(id);
-        return ZSS::ZActionModule::instance()->connectRadio(id, frq);
+        // ZSS::ZActionModule::instance()->disconnectRadio(id);
+        // return ZSS::ZActionModule::instance()->connectRadio(id, frq);
     } else {
 //        return ZSS::ZActionModule::instance()->disconnectRadio(id);
     }
+    return false;
 }
 bool Interaction::connectSim(bool sw, int id, bool color) {
     if(sw) {
@@ -383,13 +384,13 @@ int Interaction::getFrequency(){
     return ZSS::NActionModule::instance()->getFrequency();
 }
 void Interaction::changeAddress(int team, int index){
-    ZSS::ZActionModule::instance()->changeAddress(team,index);
+    // ZSS::ZActionModule::instance()->changeAddress(team,index);
 }
 QStringList Interaction::getAllAddress(){
-    return ZSS::ZActionModule::instance()->getAllAddress();
+    // return ZSS::ZActionModule::instance()->getAllAddress();
 }
 QString Interaction::getRealAddress(int index){
-    return ZSS::ZActionModule::instance()->getRealAddress(index);
+    // return ZSS::ZActionModule::instance()->getRealAddress(index);
 };
 
 void Interaction::updateTestScriptList(){
diff --git a/Client/src/viewerinterface.h b/Client/src/viewerinterface.h
index e449985..f204551 100644
--- a/Client/src/viewerinterface.h
+++ b/Client/src/viewerinterface.h
@@ -15,7 +15,7 @@ public slots:
     }
 public:
     explicit ViewerInterface(QObject *parent = Q_NULLPTR){
-        QObject::connect(ZSS::ZActionModule::instance(),SIGNAL(receiveRobotInfo(int,int)),this,SLOT(changeRobotInfo(int,int)));
+        // QObject::connect(ZSS::ZActionModule::instance(),SIGNAL(receiveRobotInfo(int,int)),this,SLOT(changeRobotInfo(int,int)));
         QObject::connect(ZSS::ZSimModule::instance(),SIGNAL(receiveSimInfo(int,int)),this,SLOT(changeRobotInfo(int,int)));
     }
     enum Roles {
diff --git a/Core/src/Simulator/CommandInterface.cpp b/Core/src/Simulator/CommandInterface.cpp
index cde72e6..6e36cae 100644
--- a/Core/src/Simulator/CommandInterface.cpp
+++ b/Core/src/Simulator/CommandInterface.cpp
@@ -55,7 +55,7 @@ void CCommandInterface::setSpeed(int num, double dribble, double vx, double vy,
     commands[number].velocity_r = vr;
 
 }
-void CCommandInterface::setKick(int num, double kp, double cp) {
+void CCommandInterface::setKick(int num, double kp, double cp, bool direct_kick_no_calibration, double direct_kick_power) {
     int number = num;
     if (number < 0 || number > PARAM::Field::MAX_PLAYER - 1) {
         //std::cout << "Robot Number Error in Simulator setKick" << std::endl;
@@ -63,6 +63,8 @@ void CCommandInterface::setKick(int num, double kp, double cp) {
     }
     commands[number].flat_kick = kp;
     commands[number].chip_kick = cp;
+    commands[number].direct_kick_no_calibration = direct_kick_no_calibration;
+    commands[number].direct_kick_power = direct_kick_power;
 }
 
 void CCommandInterface::setNeedReport(int num, bool needReport){
diff --git a/Core/src/Simulator/CommandInterface.h b/Core/src/Simulator/CommandInterface.h
index e0100e7..2437b7a 100644
--- a/Core/src/Simulator/CommandInterface.h
+++ b/Core/src/Simulator/CommandInterface.h
@@ -15,15 +15,17 @@
 class QUdpSocket;
 
 struct RobotCommand{
-    double velocity_x;
-    double velocity_y;
-    double velocity_r;
-    double flat_kick;
-    double chip_kick;
-    double dribble_spin;
-    bool use_dir;
-    bool need_report;
-    RobotCommand():velocity_x(0),velocity_y(0),velocity_r(0),flat_kick(0),chip_kick(0),dribble_spin(0),use_dir(false),need_report(false) {}
+    double velocity_x = 0;
+    double velocity_y = 0;
+    double velocity_r = 0;
+    double flat_kick = 0;
+    double chip_kick = 0;
+    double dribble_spin = 0;
+    bool use_dir = false;
+    bool need_report = false;
+    bool direct_kick_no_calibration=false;
+    double direct_kick_power=0;
+    RobotCommand() = default;
 };
 
 class CCommandInterface : public QObject
@@ -36,7 +38,7 @@ class CCommandInterface : public QObject
     static CCommandInterface* instance(const COptionModule *pOption=nullptr);
     static void destruct();
     void setSpeed(int num, double dribble, double vx, double vy, double vr);
-    void setKick(int num, double kp, double cp);
+    void setKick(int num, double kp, double cp, bool direct_kick_no_calibration=false, double direct_kick_power=0);
     void setNeedReport(int num, bool needReport);
     void sendCommands();
 private slots:
diff --git a/share/proto/zss_cmd.proto b/share/proto/zss_cmd.proto
index 4e6b6d0..d171a21 100644
--- a/share/proto/zss_cmd.proto
+++ b/share/proto/zss_cmd.proto
@@ -52,4 +52,8 @@ message Robot_Command {
 
   required bool use_dir = 10;
   required bool need_report = 11;
+
+  // use for power calibration
+  optional bool  direct_kick_no_calibration = 12;
+  optional float direct_kick_power = 13;
 }

From ef6ee87797f40cc05a2f9d95ca29f5bf5e8f8107 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Sun, 14 Apr 2024 17:05:44 +0800
Subject: [PATCH 16/30] [Core] add nocalibration kick to task params

---
 Core/src/LuaModule/kickstatus.pkg     |  3 --
 Core/src/Main/ActionModule.cpp        | 20 +++-------
 Core/src/Utils/misc_types.h           |  2 +
 Core/src/Wireless/PlayerCommandV2.cpp | 20 ----------
 Core/src/Wireless/PlayerCommandV2.h   | 14 -------
 Core/src/WorldModel/KickStatus.h      | 53 ++++++++++-----------------
 6 files changed, 27 insertions(+), 85 deletions(-)

diff --git a/Core/src/LuaModule/kickstatus.pkg b/Core/src/LuaModule/kickstatus.pkg
index 5e45802..83e2bb2 100644
--- a/Core/src/LuaModule/kickstatus.pkg
+++ b/Core/src/LuaModule/kickstatus.pkg
@@ -6,12 +6,9 @@ class CKickStatus {
 	CKickStatus();
 	void setKick(int num, double power);
 	void setChipKick(int num, double power);
-	void setBothKick(int num, double kick, double chip);
-	void setAllKick(int num, double kick, double chip, double pass);
 	bool needKick(int num) const;
 	double getKickPower(int num) const;
 	double getChipKickDist(int num) const;
-	double getPassDist(int num) const;
 	int getKiker() const;
 	void clearAll();
 	void resetKick2ForceClose(bool forceClose = false, int forceCloseCycle = 0);
diff --git a/Core/src/Main/ActionModule.cpp b/Core/src/Main/ActionModule.cpp
index 6f02407..fc51577 100644
--- a/Core/src/Main/ActionModule.cpp
+++ b/Core/src/Main/ActionModule.cpp
@@ -62,23 +62,15 @@ bool CActionModule::sendAction() {
         }
 
         // 踢：有效的踢控指令
-        double kickPower = 0.0;
-        double chipkickDist = 0.0;
-        double passdist = 0.0;
         if (KickStatus::Instance()->needKick(vecNum)) {
-            // 更新踢相关参数
-            kickPower = KickStatus::Instance()->getKickPower(vecNum);
-            chipkickDist = KickStatus::Instance()->getChipKickDist(vecNum);
-            passdist = KickStatus::Instance()->getPassDist(vecNum);
-            // 涉及到平/挑射分档，这里只关系相关参数，实际分档请关注 CommandSender
-            CPlayerKickV2 kickCmd(vecNum, kickPower, chipkickDist, passdist, dribble);
-            // 机构动作 <kick dribble>
-            kickCmd.execute(vecNum);
+            double kickPower = KickStatus::Instance()->getKickPower(vecNum);
+            double chipkickDist = KickStatus::Instance()->getChipKickDist(vecNum);
+            bool direct_kick_no_calibration = KickStatus::Instance()->isDirectKickNoCalibration(vecNum);
+            double direct_kick_power = KickStatus::Instance()->getDirectKickPower(vecNum);
+            CCommandInterface::instance()->setKick(vecNum, kickPower, chipkickDist, direct_kick_no_calibration, direct_kick_power);
         } else {
-            CPlayerKickV2 kickCmd(vecNum, 0, 0, 0, dribble);
-            kickCmd.execute(vecNum);
+            CCommandInterface::instance()->setKick(vecNum, 0, 0);
         }
-
     }
 
     /************************************************************************/
diff --git a/Core/src/Utils/misc_types.h b/Core/src/Utils/misc_types.h
index 038f6ac..280a165 100644
--- a/Core/src/Utils/misc_types.h
+++ b/Core/src/Utils/misc_types.h
@@ -54,6 +54,8 @@ struct PlayerStatus{
 	bool needkick;													// 踢球动作执行开关
 	bool ispass;													// 是否进行传球
 	bool ischipkick;												// 挑球还是平射
+	bool isDirectNoCalibrationKick;									// 是否是直接下发踢球时间参数给硬件（一般用于标定）
+	double directNoCalibrationKickPower;							// 直接下发踢球时间参数给硬件的时间
 	double kickprecision;											// 踢球朝向精度
 	double kickpower;												// 踢球力度
 	double chipkickpower;											// 挑球力度	
diff --git a/Core/src/Wireless/PlayerCommandV2.cpp b/Core/src/Wireless/PlayerCommandV2.cpp
index 6e6ffd2..32c1c9f 100644
--- a/Core/src/Wireless/PlayerCommandV2.cpp
+++ b/Core/src/Wireless/PlayerCommandV2.cpp
@@ -65,26 +65,6 @@ double CPlayerSpeedV2::getAffectedRotateSpeed() const
 	return rspeed();
 }
 
-/************************************************************************/
-/*                          Kick                                        */
-/************************************************************************/
-void CPlayerKickV2::execute(int realNum){
-     CCommandInterface::instance()->setKick(realNum, _normal, _chip);
-}
-void CPlayerKickV2::toStream(std::ostream& os) const
-{
-	os << number();
-	if( _normal ){
-		if( _pass ){
-			os << " pass " << _pass;
-		}else{
-			os << " kick " << _normal;
-		}
-	}else{
-		os << " chip kick " << _chip;
-	}
-}
-
 /************************************************************************/
 /*                         Gyro(陀螺仪)                                 */
 /************************************************************************/
diff --git a/Core/src/Wireless/PlayerCommandV2.h b/Core/src/Wireless/PlayerCommandV2.h
index 0c955a0..e0ca568 100644
--- a/Core/src/Wireless/PlayerCommandV2.h
+++ b/Core/src/Wireless/PlayerCommandV2.h
@@ -22,20 +22,6 @@ class CPlayerSpeedV2 : public CPlayerCommand{
 	double _rspeed;
 };
 
-/************************************************************************/
-/*                         Kick                                         */
-/************************************************************************/
-class CPlayerKickV2 : public CPlayerCommand{
-public:
-	CPlayerKickV2(int number, double normal, double chip, double pass, unsigned char dribble)
-		: CPlayerCommand(number, dribble), _normal(normal), _chip(chip), _pass(pass){ }
-    virtual void execute(int);
-	virtual void toStream(std::ostream& os) const;
-private:
-	double _normal; // 普通击球的力度
-	double _chip; // 挑球的距离
-	double _pass; // 传球的距离
-};
 /************************************************************************/
 /*                         Gyro(陀螺仪)                                 */
 /************************************************************************/
diff --git a/Core/src/WorldModel/KickStatus.h b/Core/src/WorldModel/KickStatus.h
index 728e4e0..74eb76e 100644
--- a/Core/src/WorldModel/KickStatus.h
+++ b/Core/src/WorldModel/KickStatus.h
@@ -38,7 +38,7 @@ class CKickStatus {
 	/// <param name="num">  	Specified vehicle. </param>
 	/// <param name="power">	Specified kick power. </param>
 
-	void setKick(int num, double power) { _normal[num] = power; _chip[num] = _pass[num] = 0; _kicker = num; _needKick[num] = true; }
+	void setKick(int num, double power) { _normal[num] = power; _chip[num] = 0; _kicker = num; _needKick[num] = true; }
 
 	/// <summary>	Sets chip kick. </summary>
 	///
@@ -48,32 +48,24 @@ class CKickStatus {
 	/// <param name="power">	Specified kick power. </param>
 
 	void setChipKick(int num, double power) { 
-		_normal[num] = _pass[num] = 0;
+		_normal[num] = 0;
 		_chip[num] = power;
 		_kicker = num; 
 		_needKick[num] = true; 
 	}
 
-	/// <summary>	Sets both kick. </summary>
-	///
-	/// <remarks>	cliffyin, 2011/7/25. </remarks>
-	///
-	/// <param name="num"> 	Specified vehicle. </param>
-	/// <param name="kick">	The kick distance. </param>
-	/// <param name="chip">	The chip distance. </param>
-
-	void setBothKick(int num, double kick, double chip) { _normal[num] = kick; _chip[num] = chip; _pass[num] = 0; _kicker = num; _needKick[num] = true; }
-
-	/// <summary>	Sets all kick. </summary>
-	///
-	/// <remarks>	cliffyin, 2011/7/25. </remarks>
-	///
-	/// <param name="num"> 	Specified vehicle. </param>
-	/// <param name="kick">	The kick power. </param>
-	/// <param name="chip">	The chip distance. </param>
-	/// <param name="pass">	The pass distance. </param>
-
-	void setAllKick(int num, double kick, double chip, double pass) { _normal[num] = kick; _chip[num] = chip; _pass[num] = pass; _kicker = num; _needKick[num] = true; }
+	// kick_mode: false for flat kick, true for chip kick
+	void setDirectKick(int num, bool kick_mode, double power, bool no_calibration = false, double direct_kick_power = 0){
+		_normal[num] = kick_mode ? 0 : power;
+		_chip[num] = kick_mode ? power : 0;
+		_kicker = num;
+		_needKick[num] = true;
+		// use for kick collect calibration data
+		_direct_kick_no_calibration[num] = no_calibration;
+		_direct_kick_power[num] = direct_kick_power;
+	}
+	bool isDirectKickNoCalibration(int num) const { return _direct_kick_no_calibration[num]; }
+	double getDirectKickPower(int num) const { return _direct_kick_power[num]; }
 
 	/// <summary>	Check If Need kick. </summary>
 	///
@@ -101,14 +93,6 @@ class CKickStatus {
 
 	double getChipKickDist(int num) const { return _chip[num]; }
 
-	/// <summary>	Gets the pass distance. </summary>
-	///
-	/// <remarks>	cliffyin, 2011/7/25. </remarks>
-	///
-	/// <returns>	The pass distance. </returns>
-
-	double getPassDist(int num) const { return _pass[num]; }
-
 	/// <summary>	Gets the kiker. </summary>
 	///
 	/// <remarks>	cliffyin, 2011/7/25. </remarks>
@@ -127,7 +111,8 @@ class CKickStatus {
 			_needKick[vecNum] = false;
 			_normal[vecNum] = 0;
 			_chip[vecNum] = 0;
-			_pass[vecNum] = 0;
+			_direct_kick_no_calibration[vecNum] = false;
+			_direct_kick_power[vecNum] = 0;
 		}
 		_forceClose = false;
 		_forceCloseCycle = 0;
@@ -159,12 +144,12 @@ class CKickStatus {
 	/// <summary> The chip kick distance </summary>
 	double _chip[PARAM::Field::MAX_PLAYER];		// 挑球的距离
 
-	/// <summary> The pass kick distance</summary>
-	double _pass[PARAM::Field::MAX_PLAYER];		// 传球的距离
-
 	/// <summary> Need kick flag </summary>
 	bool _needKick[PARAM::Field::MAX_PLAYER];
 
+	bool _direct_kick_no_calibration[PARAM::Field::MAX_PLAYER];
+	double _direct_kick_power[PARAM::Field::MAX_PLAYER];
+
 	bool _forceClose;
 
 	int _forceCloseCycle;

From 93eaa98e057e5c04738fe3664f2528c9e8c293a6 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Mon, 15 Apr 2024 12:23:21 +0800
Subject: [PATCH 17/30] [Core] add gui_debug_x size

---
 Core/src/Strategy/skill/GotoPosition.cpp | 9 ++++++---
 Core/src/Utils/GDebugEngine.cpp          | 4 ++--
 Core/src/Utils/GDebugEngine.h            | 2 +-
 3 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/Core/src/Strategy/skill/GotoPosition.cpp b/Core/src/Strategy/skill/GotoPosition.cpp
index 6f9dfe5..f6666d0 100644
--- a/Core/src/Strategy/skill/GotoPosition.cpp
+++ b/Core/src/Strategy/skill/GotoPosition.cpp
@@ -151,9 +151,6 @@ CPlayerCommand* CGotoPositionV2::execute(const CVisionModule* pVision)
     bool ignoreNotStop = false;
     target = avoidPenaltyArea(pVision, vecPos, target, avoidLength, vecNumber);
 
-    // 记录当前的规划执行目标点
-    GDebugEngine::Instance()->gui_debug_x(target, TASK_TARGET_COLOR);
-    GDebugEngine::Instance()->gui_debug_line(self.Pos(), target, TASK_TARGET_COLOR);
     if(task().player.vel.mod() > 1e-8) {
         GDebugEngine::Instance()->gui_debug_line(target, target + task().player.vel / 10, COLOR_WHITE);
     }
@@ -185,6 +182,12 @@ CPlayerCommand* CGotoPositionV2::execute(const CVisionModule* pVision)
     final.SetDir((playerFlag & (PlayerStatus::TURN_AROUND_FRONT)) ? self.Dir() : task().player.angle);
     final.SetVel(task().player.vel);
     final.SetRotVel(task().player.rotvel);
+
+    // 记录当前的规划执行目标点
+    GDebugEngine::Instance()->gui_debug_x(final.Pos(), TASK_TARGET_COLOR, 0, 20);
+    GDebugEngine::Instance()->gui_debug_line(target, target+Utils::Polar2Vector(100,final.Dir()), TASK_TARGET_COLOR);
+    GDebugEngine::Instance()->gui_debug_line(self.Pos(), target, TASK_TARGET_COLOR);
+
     /// 调用控制方法
     CControlModel control;
     float usedtime = target.dist(self.Pos()) / capability.maxSpeed / 1.414;	// 单位：秒
diff --git a/Core/src/Utils/GDebugEngine.cpp b/Core/src/Utils/GDebugEngine.cpp
index a6daf65..4f60fad 100644
--- a/Core/src/Utils/GDebugEngine.cpp
+++ b/Core/src/Utils/GDebugEngine.cpp
@@ -16,9 +16,9 @@ CGDebugEngine::CGDebugEngine(){
 CGDebugEngine::~CGDebugEngine(){
 }
 
-void CGDebugEngine::gui_debug_x(const CGeoPoint& p, int debug_color,int RGB_value){
+void CGDebugEngine::gui_debug_x(const CGeoPoint& p, int debug_color,int RGB_value, const int size){
 	CGeoPoint basePos = p;
-    static const int x_line_length = 30;
+    const int x_line_length = size/2;
     const CGeoPoint p1 = basePos + CVector( x_line_length, x_line_length);
     const CGeoPoint p2 = basePos + CVector(-x_line_length,-x_line_length);
     gui_debug_line(p1, p2, debug_color,RGB_value);
diff --git a/Core/src/Utils/GDebugEngine.h b/Core/src/Utils/GDebugEngine.h
index 5b4bedf..306dd26 100644
--- a/Core/src/Utils/GDebugEngine.h
+++ b/Core/src/Utils/GDebugEngine.h
@@ -31,7 +31,7 @@ class CGDebugEngine{
 public:
     CGDebugEngine();
     ~CGDebugEngine();
-    void gui_debug_x(const CGeoPoint& p, int debug_color = 1,int RGB_value=0);
+    void gui_debug_x(const CGeoPoint& p, int debug_color = 1,int RGB_value=0, const int size = 60 /*mm*/);
     void gui_debug_points(const std::vector<CGeoPoint> points, int debug_color = 1,int RGB_value=0);
     void gui_debug_line(const CGeoPoint& p1, const CGeoPoint& p2, int debug_color = 1,int RGB_value=0);
     void gui_debug_arc(const CGeoPoint& p, double r, double start_angle, double span_angle, int debug_color = 1, int RGB_value=0);

From 1571e08f6582e9454216ac1cbe9fb8a9391658a2 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Mon, 15 Apr 2024 15:30:30 +0800
Subject: [PATCH 18/30] [Core] add Debug/DeviceCmd for sent cmd display

---
 Core/src/Simulator/CommandInterface.cpp | 38 +++++++++++++++++++++++++
 Core/src/Wireless/RobotSensor.cpp       |  6 ++--
 2 files changed, 41 insertions(+), 3 deletions(-)

diff --git a/Core/src/Simulator/CommandInterface.cpp b/Core/src/Simulator/CommandInterface.cpp
index 6e36cae..f3fcacb 100644
--- a/Core/src/Simulator/CommandInterface.cpp
+++ b/Core/src/Simulator/CommandInterface.cpp
@@ -19,6 +19,7 @@ int SIM_PORT = 0;
 int SELF_PORT = 0;
 int CHIP_ANGLE = 1;
 int TEAM;
+bool DEBUG_CMD = false;
 ZSS::Protocol::Robots_Command robots_command;
 std::thread *_thread = nullptr;
 }
@@ -31,6 +32,8 @@ CCommandInterface::CCommandInterface(const COptionModule *pOption, QObject *pare
     ZSS::ZParamManager::instance()->loadParam(SELF_PORT, "Ports/SimSelfPort", 30015);
     ZSS::ZParamManager::instance()->loadParam(CHIP_ANGLE, "Simulator/ChipAngle", 45);
     ZSS::ZParamManager::instance()->loadParam(isYellow, "ZAlert/IsYellow", false);
+
+    ZSS::ZParamManager::instance()->loadParam(DEBUG_CMD,"Debug/DeviceCmd",false);
     TEAM = isYellow ? PARAM::YELLOW : PARAM::BLUE;
     command_socket = new QUdpSocket();
     receiveSocket = new QUdpSocket();
@@ -102,6 +105,41 @@ void CCommandInterface::sendCommands() {
             robot_command->set_power(commands[i].chip_kick);
         }
     }
+    if(DEBUG_CMD){
+        static const int DATA_SIZE = 7;
+        static const std::array<std::string, DATA_SIZE> titles{"BotID","Vel_X","Vel_Y","Vel_R","Dribb","KickM","Power"};
+        static const int FONT_SIZE = 50;
+        static const double DEBUG_TEXT_LENGTH = FONT_SIZE*3;
+        static const double DEBUG_X = PARAM::Field::PITCH_LENGTH / 2 - 3 * FONT_SIZE * (PARAM::Field::MAX_PLAYER) - DEBUG_TEXT_LENGTH;
+        static const double DEBUG_X_STEP = PARAM::Field::PITCH_LENGTH / 30;
+        static const double DEBUG_Y = PARAM::Field::PITCH_WIDTH / 2 - 5*FONT_SIZE;
+        static const double DEBUG_Y_STEP = -FONT_SIZE*2;
+        std::string title_txt = "";
+        for(auto title_str : titles){
+            title_txt += fmt::format(" {:>6s}",title_str);
+        }
+        GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y),title_txt,COLOR_PURPLE,0,FONT_SIZE);
+        for(int i=0;i<robots_command.command_size();i++){
+            auto&& cmd = robots_command.command(i);
+            std::array<std::string,DATA_SIZE> info_str{
+                fmt::format("{:>7d}",cmd.robot_id()),
+                fmt::format("{:>7.0f}",cmd.velocity_x()),
+                fmt::format("{:>7.0f}",cmd.velocity_y()),
+                fmt::format("{:>7.2f}",cmd.velocity_r()),
+                fmt::format("{:>7.2f}",cmd.dribbler_spin()),
+                fmt::format("{:>7s}",cmd.kick() ? "✓" : "✕"),
+                fmt::format("{:>7.0f}",cmd.power())
+            };
+            std::array<bool,DATA_SIZE> info_check{1, std::abs(cmd.velocity_x())>1, std::abs(cmd.velocity_y())>1, std::abs(cmd.velocity_r())>0.01, cmd.dribbler_spin()>0.1, cmd.kick(), cmd.power()>100};
+            std::string msg,msg_invalid;
+            for(int j=0;j<info_str.size();j++){
+                msg += fmt::format("{:>7s}",info_check[j] ? info_str[j] : "");
+                msg_invalid += fmt::format("{:>7s}",info_check[j] ? "" : "--");
+            }
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * (i+1)),msg,COLOR_PURPLE,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * (i+1)),msg_invalid,COLOR_GRAY,0,FONT_SIZE);
+        }
+    }
     int size = ::robots_command.ByteSizeLong();
     QByteArray data(size, 0);
     ::robots_command.SerializeToArray(data.data(), size);
diff --git a/Core/src/Wireless/RobotSensor.cpp b/Core/src/Wireless/RobotSensor.cpp
index f1ac5e7..702a2ca 100644
--- a/Core/src/Wireless/RobotSensor.cpp
+++ b/Core/src/Wireless/RobotSensor.cpp
@@ -27,9 +27,9 @@ void CRobotSensor::Update(const CVisionModule* pVision){
             robotMsg[i]._mutex.unlock();
         }
         for(int i=0;i<DATA_SIZE;i++){
-            GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * i),fmt::format("{:6s}: ",text[i]),COLOR_GRAY,0,FONT_SIZE);
-            GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs[i],COLOR_GRAY,0,FONT_SIZE);
-            GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs2[i],COLOR_GREEN,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * i),fmt::format("{:6s}: ",text[i]),COLOR_GRAY,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs[i],COLOR_GRAY,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs2[i],COLOR_GREEN,0,FONT_SIZE);
         }
     }
     for(int i=0;i<PARAM::Field::MAX_PLAYER;i++){

From 06edee05d5aefd4ebd04c737f6fca2f5ed8df1f3 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Mon, 15 Apr 2024 18:14:17 +0800
Subject: [PATCH 19/30] [Core] fix bug of subplay getRoleNum error

---
 ZBin/lua_scripts/SubPlay.lua | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/ZBin/lua_scripts/SubPlay.lua b/ZBin/lua_scripts/SubPlay.lua
index 4398b40..1ff8763 100644
--- a/ZBin/lua_scripts/SubPlay.lua
+++ b/ZBin/lua_scripts/SubPlay.lua
@@ -130,11 +130,11 @@ function gSubPlay.getRoleAndNum(roleName)
         else
             if gSubPlay.playTable[scope] == nil then
                 warning("subPlay not exist - " .. scope .. " " .. role .. pathOutput(findPath))
-                return -1
+                return "",-1
             end
             if gSubPlay.playTable[scope].roleMapping[role] == nil then
                 warning("role not exist - " .. scope .. " " .. role .. pathOutput(findPath))
-                return -1
+                return "",-1
             end
             local tRoleMap = gSubPlay.playTable[scope].roleMapping[role]
             scope, role = tRoleMap[1], tRoleMap[2]
@@ -144,7 +144,7 @@ function gSubPlay.getRoleAndNum(roleName)
         iterCount = iterCount + 1
         if iterCount > 10 then
             warning("getRoleNum failed - " .. pathOutput(findPath))
-            return -1
+            return "",-1
         end
     end
 end
\ No newline at end of file

From da7be9a34e769f804e08d8343af14f7665fae552 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Mon, 15 Apr 2024 19:26:50 +0800
Subject: [PATCH 20/30] [Core] fix subPlay task function closure error

---
 ZBin/lua_scripts/SubPlay.lua | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/ZBin/lua_scripts/SubPlay.lua b/ZBin/lua_scripts/SubPlay.lua
index 1ff8763..525b3c2 100644
--- a/ZBin/lua_scripts/SubPlay.lua
+++ b/ZBin/lua_scripts/SubPlay.lua
@@ -82,6 +82,10 @@ function gSubPlay.roleTask(name, role)
                 gSubPlay.register(name, role, subTask.args)
                 return subTask.task()
             end
+            local _subRoleTask = _subPlayState[role]
+            if type(_subRoleTask) == "function" then
+                return _subRoleTask()
+            end
             return _subPlayState[role]
         end,
         args = {

From 6f5bb028393a2165b72ee31c8285e46786cc435f Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Mon, 15 Apr 2024 22:57:57 +0800
Subject: [PATCH 21/30] [Client] remove useless display in log

---
 Client/src/rec_recorder.cpp | 48 -------------------------------------
 1 file changed, 48 deletions(-)

diff --git a/Client/src/rec_recorder.cpp b/Client/src/rec_recorder.cpp
index 2323434..5179ca7 100644
--- a/Client/src/rec_recorder.cpp
+++ b/Client/src/rec_recorder.cpp
@@ -13,7 +13,6 @@ namespace  {
 auto GS = GlobalSettings::instance();
 //RecMsg recMsg;
 bool isRun = true;
-bool DRAW_VELOCITY = true;
 QFile recordFile;
 QIODevice* recIO;
 QString filename;
@@ -21,7 +20,6 @@ QString filename;
 //QTime timer;
 }
 RecRecorder::RecRecorder() {
-    ZSS::ZParamManager::instance()->loadParam(DRAW_VELOCITY,"Lesson/DrawVelocity",true);
 }
 void RecRecorder::init() {
     isRun = true;
@@ -96,55 +94,9 @@ void RecRecorder::store() {
             debugMsgs = recMsg.add_debugmsgs();
             if (team == PARAM::BLUE) {
                 debugMsgs->ParseFromArray(GlobalData::instance()->debugBlueMessages.data(), GlobalData::instance()->debugBlueMessages.size());
-//                qDebug() << "before : " << recMsg.ByteSizeLong();
-                if(DRAW_VELOCITY){
-                    auto vision = maintainMsg;
-                    auto robot = vision.robot[PARAM::BLUE][0];
-                    if(vision.robotSize[PARAM::BLUE] > 0){
-                        auto data = GlobalData::instance()->robotCommand[PARAM::BLUE];
-                        auto vel = data[0].robotSpeed[0];
-                        auto lVel = data[-1].robotSpeed[0];
-                        // vel x
-                        {
-                            auto msg = debugMsgs->add_msgs();
-                            msg->set_color(Debug_Msg_Color_CYAN);
-                            msg->set_type(Debug_Msg_Debug_Type_TEXT);
-                            auto text = msg->mutable_text();
-                            auto pos = text->mutable_pos();
-                            pos->set_x(robot.pos.x()/10+15);
-                            pos->set_y(robot.pos.y()/10+18);
-                            text->set_text(QString("vx:%1,ax:%2").arg(vel.vx/100,0,'f',4).arg(fabs(lVel.vx - vel.vx)/100,0,'f',4).toLatin1());
-                        }
-                        // vel y
-                        {
-                            auto msg = debugMsgs->add_msgs();
-                            msg->set_color(Debug_Msg_Color_CYAN);
-                            msg->set_type(Debug_Msg_Debug_Type_TEXT);
-                            auto text = msg->mutable_text();
-                            auto pos = text->mutable_pos();
-                            pos->set_x(robot.pos.x()/10+15);
-                            pos->set_y(robot.pos.y()/10+5);
-                            text->set_text(QString("vy:%1").arg(vel.vy/100,0,'f',2).toLatin1());//m
-                        }
-                        // vel w
-                        {
-                            auto msg = debugMsgs->add_msgs();
-                            msg->set_color(Debug_Msg_Color_CYAN);
-                            msg->set_type(Debug_Msg_Debug_Type_TEXT);
-                            auto text = msg->mutable_text();
-                            auto pos = text->mutable_pos();
-                            pos->set_x(robot.pos.x()/10+15);
-                            pos->set_y(robot.pos.y()/10-8);
-                            text->set_text(QString("vw:%1,aw:%2").arg(vel.vr,0,'f',4).arg(fabs(lVel.vr - vel.vr),0,'f',4).toLatin1());
-                        }
-                    }
-                }
-//                qDebug() << "after : " << recMsg.ByteSizeLong();
             } else {
                 debugMsgs->ParseFromArray(GlobalData::instance()->debugYellowMessages.data(), GlobalData::instance()->debugYellowMessages.size());
             }
-            //        qDebug() << "FUCK DEBUG MESSAGE SIZE" <<  debugMsgs->ByteSizeLong();
-            //        qDebug() << "FUCK DEBUG MESSAGE SIZE" <<  recMsg.debugmsgs(0).ByteSizeLong();
         }
 
         GlobalData::instance()->debugMutex.unlock();

From 61650a72867fb0a328c47fd81135e037a150a943 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Mon, 15 Apr 2024 23:36:27 +0800
Subject: [PATCH 22/30] [Core] tidy cmmotion to origin

---
 Core/src/MotionControl/CMmotion.cpp      | 386 ++++++++++-------------
 Core/src/MotionControl/CMmotion.h        |  21 +-
 Core/src/MotionControl/ControlModel.cpp  |   4 +-
 Core/src/MotionControl/ControlModel.h    |   2 +-
 Core/src/Strategy/skill/GotoPosition.cpp |   3 +-
 5 files changed, 165 insertions(+), 251 deletions(-)

diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
index a294d76..73e4c53 100644
--- a/Core/src/MotionControl/CMmotion.cpp
+++ b/Core/src/MotionControl/CMmotion.cpp
@@ -6,6 +6,7 @@
 #include <fstream>
 #include "GDebugEngine.h"
 #include "parammanager.h"
+#include <fmt/core.h>
 
 
 using namespace std;
@@ -30,8 +31,6 @@ bool DISPLAY_ROTATION_LIMIT = ZSS::ZParamManager::instance()->value("Debug/Rotat
 bool DEBUG_TIME = ZSS::ZParamManager::instance()->value("Debug/TimePredict", QVariant(false)).toBool();
 bool addComp = true;
 int timeDebugColor = COLOR_GREEN;
-int timeItor = 0;
-int isX = 1;
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -52,157 +51,134 @@ int isX = 1;
 /// @param	traj_accel		计算得加速度
 /// @param	traj_time		计算得加速时间
 ////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-
-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) {
-    if (x0 == 0. && v0 == v1) {
-        traj_accel = 0;
-        if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-380*10 * isX, -270*10 + (timeItor++) * 20*10), QString("R1").toLatin1(), timeDebugColor);
-        return;
-    }
-
-    if(!finite(x0) || !finite(v0) || !finite(v1)) {
-        traj_accel = 0;
-        if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-380*10 * isX, -270*10 + (timeItor++) * 20*10), QString("R2").toLatin1(), timeDebugColor);
-        return;
-    }
-    if(pT == MOVE_X && fabs(v1) > 1e-8
-            && fabs(v0) < fabs(v1) && v0 * v1 > 0) {
-        traj_accel = copysign(a_max, v1);
-        return;
-    }
-
-
-    double decFactor = DEC_FACTOR;
-    a_max /= a_factor;
-    d_max /= (1.0 * a_factor);
-
-    double accel_time_to_v1 = fabs(v1 - v0) / a_max;                                                  // 最大加速度加速到末速度的时间
-    double accel_dist_to_v1 = fabs((v1 + v0) / 2.0) * accel_time_to_v1;                               // 单一加速到末速度时的位移
-    double decel_time_to_v1 = fabs(v0 - v1) / d_max;                                                  // 最大减速度减速到末速度的时间
-    double decel_dist_to_v1 = fabs((v0 + v1) / 2.0) * decel_time_to_v1;                               // 单一减速到末速度时的位移
-
-    // 这个时间很关键，设得较大则定位精度将大大降低 by qxz
-    double period = 1 / 40.0; // 一段很小的时间，处理运动到目标点附近时加速度，稳定到点，防止超调
-    if(pT == MOVE_X)
-        period = 1 / 40.0;
-    else if(pT == MOVE_Y)
-        period = 1.0 / 75.0;
-    else
-        period = 1 / 40.0;
-
-    if(a_max > 600*10 && pT != MOVE_Y) {
-        if(fabs(v0) > 150*10)
-            period = 1 / 15.0;
-        else if(fabs(v0) > 50*10)
-            period = 1 / 20.0;
-    }
-    else if(a_max > 480*10 && pT != MOVE_Y) {
-        if(fabs(v0) > 150*10)
-            period = 1 / 25.0;
-        else if(fabs(v0) > 50*10)
-            period = 1 / 30.0;
-    }
-
-    // 计算时间部分
-    // 需要先后退再加速非零速到点
-    if (fabs(v1) > 1e-8 && v1 * x0 > 0 && mode == ACCURATE) {
-        double time_to_accel = fabs(v1) / a_max;
-        double x_to_accel = (v1 * v1) / (2.0 * a_max);
-        compute_motion_1d(x0 + copysign(x_to_accel, v1), v0, 0, a_max * a_factor, d_max *  a_factor,
-                          v_max, a_factor, vel_factor, traj_accel, traj_time, traj_time_acc, traj_time_dec, traj_time_flat, pT, mode);
-        traj_time += time_to_accel;
-        if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-380*10 * isX, -270*10 + (timeItor++) * 20*10), QString("R3").toLatin1(), timeDebugColor);
-
-        return;
-    }
-    // 从x0运动到零点
-    // 初速和目标点反向 或 初速大于目标速来不及减速到目标速
-    // 全程减速
-    else if (v0 * x0 > 0 || (fabs(v0) > fabs(v1) && decel_dist_to_v1 > fabs(x0))) {
-        // 停下后到达的时间 + 停下所用时间
-        double time_to_stop = fabs(v0) / (d_max);                                                       // 停下时间
-        double x_to_stop = v0 * v0 / (2.0 * d_max);                                                   // 停止时运动距离
-        double time_to_accel = fabs(v1) / a_max;
-        double x_to_accel = (v1 * v1) / (2.0 * a_max);
-
-        if (fabs(v1) > 1e-8 && mode == ACCURATE) {
-            compute_motion_1d(x0 + copysign(x_to_accel, v1), v0, 0, a_max * a_factor, d_max * a_factor,
-                              v_max, a_factor, vel_factor, traj_accel, traj_time, traj_time_acc, traj_time_dec, traj_time_flat, pT, mode);
-            traj_time += time_to_accel;
-            if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-380*10 * isX, -270*10 + (timeItor++) * 20*10), QString("R4").toLatin1(), timeDebugColor);
-            return;
-        }
-        else {
-            compute_motion_1d(x0 + copysign(x_to_stop, v0), 0, v1, a_max * a_factor, d_max * a_factor,
-                              v_max, a_factor, vel_factor, traj_accel, traj_time, traj_time_acc, traj_time_dec, traj_time_flat, pT, mode);    // 递归运算直到跳出这一条件
-            traj_time += time_to_stop;                                                                    // 加上路径规划时间
-            traj_time_dec += time_to_stop;
-            // 减速
-            if (time_to_stop < period) {
-                if(pT == MOVE_Y)
-                    traj_accel = compute_stop(v0, a_max);
-                else
-                    traj_accel = time_to_stop / period * (- copysign(d_max * a_factor, v0)) + (1.0 - time_to_stop / period) * traj_accel;
-            }
-            else {
-                traj_accel = - copysign(decFactor * d_max * a_factor, v0);
-            }
-            if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-380*10 * isX, -270*10 + (timeItor++) * 20*10), QString("R5").toLatin1(), timeDebugColor);
-            return;
-        }
+void origin_compute_motion_1d(double x0, double v0, double v1,
+                              double a_max, double v_max, double a_factor,
+                              double &traj_accel, double &traj_time)
+{
+  // First check to see if nothing needs to be done...
+  if (x0 == 0 && v0 == v1) { traj_accel = 0; traj_time = 0;  return; }
+
+  if(!finite(x0) || !finite(v0) || !finite(v1)){
+    printf("Robot::compute_motion_1d: NANs!\n");
+    traj_accel = 0; traj_time = 0;  return;
+  }
+
+  // Need to do some motion.
+  a_max /= a_factor;
+
+  double time_to_v1 = fabs(v0 - v1) / a_max;
+  double x_to_v1 = fabs((v0 + v1) / 2.0) * time_to_v1;
+
+  double period = 2.0 * FRAME_PERIOD;
+
+  v1 = copysign(v1, -x0);
+
+  if (v0 * x0 > 0 || (fabs(v0) > fabs(v1) && x_to_v1 > fabs(x0))) {
+    // Time to reach goal after stopping + Time to stop.
+    double time_to_stop = fabs(v0) / a_max;
+    double x_to_stop = v0 * v0 / (2 * a_max);
+
+    origin_compute_motion_1d(x0 + copysign(x_to_stop, v0), 0, v1, a_max * a_factor,
+                      v_max, a_factor, traj_accel, traj_time);
+    traj_time += time_to_stop;
+ 
+    // Decelerate
+    if (traj_time < period) 
+      traj_accel = compute_stop(v0, a_max * a_factor);
+    else if (time_to_stop < period) 
+      traj_accel = time_to_stop / period * - copysign(a_max * a_factor, v0) +
+	(1.0 - time_to_stop / period) * traj_accel;
+    else traj_accel = -copysign(a_max * a_factor, v0);
+
+    return;
+  }
+
+
+  // At this point we have two options.  We can maximally accelerate
+  // and then maximally decelerate to hit the target.  Or we could
+  // find a single acceleration that would reach the target with zero
+  // velocity.  The later is useful when we are close to the target
+  // where the former is less stable.
+
+  // OPTION 1
+  // This computes the amount of time to accelerate before decelerating.
+  double t_a, t_accel, t_decel;
+
+  if (fabs(v0) > fabs(v1)) {
+    //    t_a = (sqrt((3*v1*v1 + v0*v0) / 2.0 - fabs(v0 * v1) + fabs(x0) * a_max) 
+    //	   - fabs(v0)) / a_max;
+    t_a = (sqrt((v0 * v0 + v1 * v1) / 2.0 + fabs(x0) * a_max) 
+	   - fabs(v0)) / a_max;
+
+    if (t_a < 0.0) t_a = 0;
+    t_accel = t_a;
+    t_decel = t_a + time_to_v1;
+  } else if (x_to_v1 > fabs(x0)) {
+    t_a = (sqrt(v0 * v0 + 2 * a_max * fabs(x0)) - fabs(v0)) / a_max;
+    t_accel = t_a;
+    t_decel = 0.0;
+  } else {
+    //    t_a = (sqrt((3*v0*v0 + v1*v1) / 2.0 - fabs(v0 * v1) + fabs(x0) * a_max) 
+    //  - fabs(v1)) / a_max;
+
+    t_a = (sqrt((v0 * v0 + v1 * v1) / 2.0 + fabs(x0) * a_max) 
+	   - fabs(v1)) / a_max;
+
+    if (t_a < 0.0) t_a = 0;
+    t_accel = t_a + time_to_v1;
+    t_decel = t_a;
+  }
+
+  // OPTION 2
+  double a_to_v1_at_x0 = (v0 * v0 - v1 * v1) / (2 * fabs(x0));
+  double t_to_v1_at_x0 = 
+    (-fabs(v0) + sqrt(v0 * v0 + 2 * fabs(a_to_v1_at_x0) * fabs(x0))) / 
+    fabs(a_to_v1_at_x0);
+
+  // We follow OPTION 2 if t_a is less than a FRAME_PERIOD making it
+  // difficult to transition to decelerating and stopping exactly.
+  if (0 && a_to_v1_at_x0 < a_max && a_to_v1_at_x0 > 0.0 &&
+      t_to_v1_at_x0 < 2.0 * FRAME_PERIOD && 0) {
+
+    // OPTION 2
+    // Use option 1 time, even though we're not following it.
+    traj_time = t_accel + t_decel;;
+
+    // Target acceleration to stop at x0.
+    traj_accel = -copysign(a_to_v1_at_x0, v0);
+
+    return;
+  } else {
+
+    // OPTION 1
+    // Time to accelerate and decelerate.
+    traj_time = t_accel + t_decel;
+
+    // If the acceleration time would get the speed above v_max, then
+    //  we need to add time to account for cruising at max speed.
+    if (t_accel * a_max + fabs(v0) > v_max) {
+      traj_time += 
+	pow(v_max - (a_max * t_accel + fabs(v0)), 2.0) / a_max / v_max;
     }
 
-    if (accel_dist_to_v1 > fabs(x0) && fabs(v0) < fabs(v1)) {
-        traj_time_acc = (sqrt(2 * a_max * fabs(x0) + v0 * v0) - fabs(v0)) / a_max;
-        traj_time_flat = 0;
-        traj_time_dec = 0;
-    } else if (decel_dist_to_v1 > fabs(x0) && fabs(v0) > fabs(v1)) {
-        traj_time_acc = 0;
-        traj_time_flat = 0;
-        traj_time_dec = (fabs(v0) - sqrt(v0 * v0 - 2 * d_max * fabs(x0))) / d_max;
+    // Accelerate (unless t_accel is less than FRAME_PERIOD, then set
+    // acceleration to average acceleration over the period.)
+    if (t_accel < period && t_decel == 0.0) {
+      traj_accel = copysign(a_max * a_factor, -x0);
+    } else if (t_accel < period && t_decel > 0.0) {
+      traj_accel = compute_stop(v0, a_max * a_factor);
+    } else if (t_accel < period) {
+      traj_accel = copysign((2.0 * t_accel / (period) - 1) * a_max * a_factor, v0);
     } else {
-        double v_max_dist = (v_max * v_max - v0 * v0) / (2 * a_max) + (v_max * v_max - v1 * v1) / (2 * d_max);
-        if (v_max_dist > fabs(x0)) {
-            double v_m = sqrt((2 * a_max * d_max * fabs(x0) + d_max * v0 * v0 + a_max * v1 * v1) / (a_max + d_max));
-            traj_time_acc = (v_m - fabs(v0)) / a_max;
-            traj_time_flat = 0;
-            traj_time_dec = (v_m - fabs(v1)) / d_max;
-        } else {
-            traj_time_acc = (v_max - fabs(v0)) / a_max;
-            traj_time_flat = (fabs(x0) - v_max_dist) / v_max;
-            traj_time_dec = (v_max - fabs(v1)) / d_max;
-        }
-    }
-    // 分配加速度部分
-
-    double a_to_v1_at_x0 = fabs(v0 * v0 - v1 * v1) / (2 * fabs(x0));
-    double t_to_v1_at_x0 = (-fabs(v0) + sqrt(v0 * v0 + 2 * fabs(a_to_v1_at_x0) * fabs(x0))) / fabs(a_to_v1_at_x0);
-    if (t_to_v1_at_x0 < period && a_to_v1_at_x0 < a_max) {
-        traj_accel = - copysign(a_to_v1_at_x0, v0);
-        traj_time += t_to_v1_at_x0;
-        if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-380*10 * isX, -270*10 + (timeItor++) * 20*10), QString("R6").toLatin1(), timeDebugColor);
-        return;
-    }
-
-    if (FRAME_PERIOD * a_max + fabs(v0) > v_max && traj_time_flat > period ) {                           // 匀速运动阶段
-        traj_time += traj_time_acc + traj_time_flat + traj_time_dec;
-        traj_accel = 0;
-    }
-    else if (traj_time_acc < vel_factor * period && traj_time_flat <  period && traj_time_dec > 0.0) {                                         // 加速接近结束且需减速
-        traj_time += traj_accel + traj_time_flat +  traj_time_dec;
-        traj_accel =  copysign(d_max * a_factor, -v0);
+      traj_accel = copysign(a_max * a_factor, -x0);
     }
-    else {
-        traj_time += traj_time_acc + traj_time_flat + traj_time_dec;
-        traj_accel =  copysign(a_max * a_factor, -x0);
-    }
-
-    if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-380*10 * isX, -270*10 + (timeItor++) * 20*10), QString("R7").toLatin1(), timeDebugColor);
+  }
 }
+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){
+                        origin_compute_motion_1d(x0, v0, v1, a_max, v_max, a_factor, traj_accel, traj_time);
+                       }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 /// @fn	void compute_motion_2d(CVector x0, CVector v0, CVector v1, double a_max, double v_max,
@@ -225,42 +201,24 @@ void compute_motion_1d(double x0, double v0, double v1,
 
 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) {
+                       double a_factor, CVector &traj_accel, double &time) {
     double time_x = 0, time_x_acc = 0, time_x_dec = 0, time_x_flat = 0;
     double time_y = 0, time_y_acc = 0, time_y_dec = 0, time_y_flat = 0;
     double rotangle = 0;
     double traj_accel_x = 0;
     double traj_accel_y = 0;
-//     if(v0 * x0 > 0) {           //如果发现正在反方向走，则不再零速到点，防止车冲出去
-// //        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1.0, 0.0), QString("Limit v1").toLatin1());
-//         v1 = CVector(0.0, 0.0);
-//     }
-    // if (v1.mod() == 0 || mode == FAST) {
-        rotangle = x0.dir();
-    // }
-    // else {
-        // rotangle = v1.dir();
-    // }
+    
+    rotangle = x0.dir();
     x0 = x0.rotate(-rotangle);
     v0 = v0.rotate(-rotangle);
     v1 = v1.rotate(-rotangle); //坐标系转换，转换到末速度方向为x轴的坐标系中
 
     double velFactorX = 1.0, velFactorY = 1.0;
-    // velFactorX = (fabs(v1.x()) > 1e-8 ? 2.8 : 1.0);
-    // velFactorY = (fabs(v1.y()) > 1e-8 ? 2.8 : 1.0);
-//     if(v1.mod() > 0 && mode == FAST) {
-//         v1.setVector(copysign(v1.mod(), v1.x()), 0);
-// //        v_max = v1.mod();
-//     }
-
-    timeItor = 0;
-    isX = 1;
+
     compute_motion_1d(x0.x(), v0.x(), v1.x(), a_max, d_max, v_max, a_factor, velFactorX,
-                      traj_accel_x, time_x, time_x_acc, time_x_dec, time_x_flat, MOVE_X, mode);
-    timeItor = 0;
-    isX = -1;
+                      traj_accel_x, time_x);
     compute_motion_1d(x0.y(), v0.y(), v1.y(), a_max, d_max, v_max, a_factor, velFactorY,
-                      traj_accel_y, time_y, time_y_acc, time_y_dec, time_y_flat, MOVE_Y, mode);//两轴同样的最大速度、加速度独立考虑求两轴运动时间
+                      traj_accel_y, time_y);//两轴同样的最大速度、加速度独立考虑求两轴运动时间
 
     // if(v1.mod() > 1e-8 && mode == ACCURATE) {
     //     if (time_x - time_y > FRAME_PERIOD) {
@@ -272,36 +230,35 @@ void compute_motion_2d(CVector x0, CVector v0, CVector v1,
     //     }
     // }
     if(v1.mod() > 0 && DEBUG_NO_ZERO_VEL) {
+        x0 = x0.rotate(rotangle);
+        v0 = v0.rotate(rotangle);
+        v1 = v1.rotate(rotangle); //坐标系转换，转换到末速度方向为x轴的坐标系中
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 0.0), QString("Vel: %1 %2").arg(v0.x()).arg(v0.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 20.0*10), QString("xVelFinal: %1").arg(v0.x() + traj_accel_x * FRAME_PERIOD).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 40.0*10), QString("targetVel:  %1 %2 %3").arg(v1.mod()).arg(v1.x()).arg(v1.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 60.0*10), QString("yVel: %1").arg(v0.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 80.0*10), QString("yVelFinal: %1").arg(v0.y() + traj_accel_y * FRAME_PERIOD).toLatin1());
+        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 100.0*10), fmt::format("v0:{:.1f},{:.1f}",v0.x(),v0.y()));
         // GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 100.0*10), QString("v_max: %1").arg(v_max).toLatin1());
         auto S = CGeoPoint(-1000,-1000);
-        GDebugEngine::Instance()->gui_debug_line(S,S+v0.rotate(rotangle),COLOR_BLUE);
-        GDebugEngine::Instance()->gui_debug_msg(S + v0.rotate(rotangle), "v0", COLOR_BLUE);
-        GDebugEngine::Instance()->gui_debug_line(S, S + v1.rotate(rotangle), COLOR_BLUE);
-        GDebugEngine::Instance()->gui_debug_msg(S + v1.rotate(rotangle), "v1", COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_line(S,S+v0,COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_msg(S+v0, "v0", COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_line(S, S+v1, COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_msg(S+v1, "v1", COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_line(S, S+x0,COLOR_PURPLE);
+        GDebugEngine::Instance()->gui_debug_msg(S+x0, "x0", COLOR_PURPLE);
+        auto traj_acc = CVector(traj_accel_x, traj_accel_y).rotate(rotangle);
+        GDebugEngine::Instance()->gui_debug_line(S, S+traj_acc,COLOR_RED);
+        GDebugEngine::Instance()->gui_debug_msg(S+traj_acc, "Acc", COLOR_RED);
+        GDebugEngine::Instance()->gui_debug_line(S,S+Utils::Polar2Vector(500,rotangle), COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_msg(S+Utils::Polar2Vector(500,rotangle), fmt::format("PlanDir: {:.1f},{:.1f}",traj_acc.x(),traj_acc.y()), COLOR_BLUE);
     }
 
     traj_accel = CVector(traj_accel_x, traj_accel_y);
     traj_accel = traj_accel.rotate(rotangle);
     if(time_x < 1e-5 || time_x > 50) time_x = 0;
     if(time_y < 1e-5 || time_y > 50) time_y = 0;
-    if(time_x < time_y) {
-        if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(320*10 , -270*10), QString("this").toLatin1());
-        time = time_y;
-        time_acc = time_y_acc;
-        time_dec = time_y_dec;
-        time_flat = time_y_flat;
-    } else {
-        if(DEBUG_TIME) GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-320*10 , -270*10), QString("this").toLatin1());
-        time = time_x;
-        time_acc = time_x_acc;
-        time_dec = time_x_dec;
-        time_flat = time_x_flat;
-    }
+    time = std::max(time_x, time_y);
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -346,8 +303,7 @@ void goto_point_omni( const PlayerPoseT& start,
                       const PlayerCapabilityT& capability,
                       const double& accel_factor,
                       const double& angle_accel_factor,
-                      PlayerPoseT& nextStep,
-                      nonZeroMode mode) {
+                      PlayerPoseT& nextStep) {
     CGeoPoint target_pos = final.Pos();
     CVector x = start.Pos() - target_pos;
     CVector v = start.Vel();
@@ -365,7 +321,7 @@ 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, time_acc, time_dec, time_flat, mode);
+    compute_motion_2d(x, v, target_vel, max_accel, max_decel, max_speed, accel_factor, a, time);
     // a = Utils::Polar2Vector(std::min(1.0,a.mod()),a.dir());
     factor_a = 1;
 
@@ -393,7 +349,7 @@ void goto_point_omni( const PlayerPoseT& start,
             max_angle_speed = min_max_angle_speed + (capability.maxAngularSpeed * rotateScaleFactor - min_max_angle_speed) * rotateFactor;
     }
 
-    compute_motion_1d(ang, ang_v, 0.0, max_angle_accel, max_angle_decel, max_angle_speed, angle_accel_factor, 1.0, ang_a, time_a, time_a_acc, time_a_dec, time_a_flat, ROTATE, mode);
+    compute_motion_1d(ang, ang_v, 0.0, max_angle_accel, max_angle_decel, max_angle_speed, angle_accel_factor, 1.0, ang_a, time_a);
 
     if(DISPLAY_ROTATION_LIMIT){
         GDebugEngine::Instance()->gui_debug_msg(target_pos+CVector(0,-40*10), QString("maxRotateAcc:   %1").arg(max_angle_accel).toLatin1());
@@ -423,27 +379,6 @@ void goto_point_omni( const PlayerPoseT& start,
     nextStep.SetVel(v);
 
     nextStep.SetRotVel(ang_v);
-
-    if(DEBUG_TIME) {
-        CVector acc;
-        compute_motion_2d(x, v, target_vel, OUR_MAX_ACC, OUR_MAX_DEC, OUR_MAX_SPEED, accel_factor, acc, time, time_acc, time_dec, time_flat);
-
-        static double lastTime = time;
-        static double lastTimeAcc = time_acc;
-        static double lastTimeDec = time_dec;
-        static double lastTimeFlat = time_flat;
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-100*10, -400*10), QString("initialVel: %1").arg(start.Vel().mod()).toLatin1(), timeDebugColor);
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(100*10, -400*10), QString("finalVel: %1").arg((start.Vel() + a / PARAM::Vision::FRAME_RATE).mod()).toLatin1(), timeDebugColor);
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 0.0), QString("time: %1").arg((lastTime - time) * 1000.0).toLatin1(), timeDebugColor);
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 20*10), QString("timeAcc: %1").arg(time_acc * 1000.0).toLatin1(), timeDebugColor);
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 40*10), QString("timeDec: %1").arg(time_dec * 1000.0).toLatin1(), timeDebugColor);
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 60*10), QString("timeFlat: %1").arg((lastTimeFlat - time_flat) * 1000.0).toLatin1(), timeDebugColor);
-
-        lastTime = time;
-        lastTimeAcc = time_acc;
-        lastTimeDec = time_dec;
-        lastTimeFlat = time_flat;
-    }
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -471,13 +406,12 @@ double expectedCMPathTime(const PlayerPoseT& start, const CGeoPoint& final, doub
     CVector target_vel = CVector(0, 0);
     CVector a;
     double time;
-    double time_acc, time_dec, time_flat;
     compute_motion_2d(x, v, target_vel,
                       maxAccel,
                       maxAccel,
                       maxVelocity,
                       accel_factor,
-                      a, time, time_acc, time_dec, time_flat);
+                      a, time);
     return time;
 }
 
@@ -486,7 +420,6 @@ double predictedTime(const PlayerVisionT& start, const CGeoPoint & Target, const
     CVector v = (start.Vel().mod() < 2.5*10) ? CVector(0, 0) : start.Vel();
     double time;
     CVector a;
-    double time_acc, time_dec, time_flat;
     double accel_factor = 1.5;
     if(IS_SIMULATION) {
         accel_factor = 1.0;
@@ -496,7 +429,7 @@ double predictedTime(const PlayerVisionT& start, const CGeoPoint & Target, const
                       OUR_MAX_DEC,
                       OUR_MAX_SPEED,
                       accel_factor,
-                      a, time, time_acc, time_dec, time_flat);
+                      a, time);
 
     return time;
 }
@@ -507,7 +440,6 @@ double predictedTimeWithRawVel(const PlayerVisionT& start, const CGeoPoint & Tar
 //    GDebugEngine::Instance()->gui_debug_msg(start.Pos(), QString("vel: (%1, %2)").arg(v.x()).arg(v.y()).toLatin1());
     double time;
     CVector a;
-    double time_acc, time_dec, time_flat;
     double accel_factor = 1.5;
     if(IS_SIMULATION) {
         accel_factor = 1.0;
@@ -517,7 +449,7 @@ double predictedTimeWithRawVel(const PlayerVisionT& start, const CGeoPoint & Tar
                       OUR_MAX_DEC,
                       OUR_MAX_SPEED,
                       accel_factor,
-                      a, time, time_acc, time_dec, time_flat);
+                      a, time);
 
     return time;
 }
@@ -530,13 +462,12 @@ double predictedTheirTime(const PlayerVisionT& start, const CGeoPoint & Target,
     CVector v = start.Vel();
     double time;
     CVector a;
-    double time_acc, time_dec, time_flat;
     compute_motion_2d(x, v, targetVel,
                       max_acc,
                       max_acc,
                       max_speed,
                       1.5,
-                      a, time, time_acc, time_dec, time_flat);
+                      a, time);
 
     return time;
 }
@@ -545,13 +476,12 @@ double predictedTime2d(const PlayerVisionT& start, const CGeoPoint& final, doubl
     CVector x0 = start.Pos() - final;
     CVector trajAcc;
     double trajTime;
-    double time_acc, time_dec, time_flat;
     compute_motion_2d(x0, start.Vel(), CVector(0, 0),
                       maxAccel,
                       maxAccel,
                       maxVelocity,
                       accel_factor,
-                      trajAcc, trajTime, time_acc, time_dec, time_flat);
+                      trajAcc, trajTime);
     return trajTime;
 }
 
@@ -562,7 +492,7 @@ double predictedTime1d(const double &start, const double &end, const double &sta
                       maxAccel,
                       maxVelocity,
                       1.5,
-                      1.0, trajAcc, trajTime, accTime, flatTime, decTime, MOVE_X);
+                      1.0, trajAcc, trajTime);
     return trajTime;
 }
 
@@ -572,7 +502,7 @@ bool predictRushSpeed(const PlayerVisionT& start, const CGeoPoint& final, const
     double zeroTime;
     CVector a;
     double time_acc, time_dec, time_flat;
-    compute_motion_2d(x, v, CVector(0, 0), capability.maxAccel, capability.maxDec, capability.maxSpeed, 1.5, a, zeroTime, time_acc, time_dec, time_flat);
+    compute_motion_2d(x, v, CVector(0, 0), capability.maxAccel, capability.maxDec, capability.maxSpeed, 1.5, a, zeroTime);
     if (zeroTime < time) {
         targetVel = CVector(0, 0);
         return true;
@@ -628,9 +558,9 @@ void openSpeedCircle(const PlayerPoseT& start, const double dist2Center, const i
     double rotAcc, rotTime, rotAccTime, rotDecTime, rotFlatTime;
 
     if (rotateMethod == 1 || rotateMethod == 4) {
-        compute_motion_1d(posDirDiff, start.RotVel(), 0, 15, 5, 5, 1.5, 1.0, rotAcc, rotTime, rotAccTime, rotDecTime, rotFlatTime, ROTATE);
+        compute_motion_1d(posDirDiff, start.RotVel(), 0, 15, 5, 5, 1.5, 1.0, rotAcc, rotTime);
     } else {
-        compute_motion_1d(-posDirDiff, start.RotVel(), 0, 15, 5, 5, 1.5, 1.0, rotAcc, rotTime, rotAccTime, rotDecTime, rotFlatTime, ROTATE);
+        compute_motion_1d(-posDirDiff, start.RotVel(), 0, 15, 5, 5, 1.5, 1.0, rotAcc, rotTime);
     }
 
     double rotVel = startRotVel + rotAcc / PARAM::Vision::FRAME_RATE;
diff --git a/Core/src/MotionControl/CMmotion.h b/Core/src/MotionControl/CMmotion.h
index ec0a6f5..35ea047 100644
--- a/Core/src/MotionControl/CMmotion.h
+++ b/Core/src/MotionControl/CMmotion.h
@@ -1,38 +1,23 @@
 #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);
 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
index 4da6480..187e928 100644
--- a/Core/src/MotionControl/ControlModel.cpp
+++ b/Core/src/MotionControl/ControlModel.cpp
@@ -64,7 +64,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;
@@ -77,7 +77,7 @@ 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);
 }
 
 /// Trapezoidal control from ZJU : zero final velocity trajectory
diff --git a/Core/src/MotionControl/ControlModel.h b/Core/src/MotionControl/ControlModel.h
index 7ba3d40..5ae52d4 100644
--- a/Core/src/MotionControl/ControlModel.h
+++ b/Core/src/MotionControl/ControlModel.h
@@ -51,7 +51,7 @@ 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);
 
 	/// Trapezoidal control from ZJU : zero final velocity trajectory
 	void makeTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
diff --git a/Core/src/Strategy/skill/GotoPosition.cpp b/Core/src/Strategy/skill/GotoPosition.cpp
index f6666d0..e1960a7 100644
--- 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;
 

From 9d83b7a1f22bbf40cff9b11b7ad583bd5b467a41 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Tue, 16 Apr 2024 00:21:40 +0800
Subject: [PATCH 23/30] [Core] update touch

---
 Core/src/MotionControl/CMmotion.cpp |  1 +
 Core/tactics/skill/Touch.cpp        | 17 +++++++++--------
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
index 73e4c53..3def1b6 100644
--- a/Core/src/MotionControl/CMmotion.cpp
+++ b/Core/src/MotionControl/CMmotion.cpp
@@ -256,6 +256,7 @@ void compute_motion_2d(CVector x0, CVector v0, CVector v1,
 
     traj_accel = CVector(traj_accel_x, traj_accel_y);
     traj_accel = traj_accel.rotate(rotangle);
+    traj_accel = Utils::Polar2Vector(std::min(traj_accel.mod(),a_max), traj_accel.dir());
     if(time_x < 1e-5 || time_x > 50) time_x = 0;
     if(time_y < 1e-5 || time_y > 50) time_y = 0;
     time = std::max(time_x, time_y);
diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
index b7a35c1..9811f1d 100644
--- a/Core/tactics/skill/Touch.cpp
+++ b/Core/tactics/skill/Touch.cpp
@@ -44,12 +44,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 = predictedTimeWithRawVel(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;
@@ -65,7 +67,7 @@ void CTouch::plan(const CVisionModule* pVision){
     const double ballVel_ball2Target_ad = ballVelMod > 300 ? 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);
@@ -82,7 +84,7 @@ void CTouch::plan(const CVisionModule* pVision){
 
     const auto diff4avoid_ball = std::abs(angleDiff(me2target.dir(), (targetMousePos - ballPos).dir()));
     const auto needAvoidDynamic = !BALL_STATIC && me2TargetSeg.IsPointOnLineOnSegment(ballPos) && diff4avoid_ball < 90 / 180.0 * PARAM::Math::PI;
-    const auto avoidDistDynamic = needAvoidDynamic ? 3*clip(90-diff4avoid_ball *180.0 / PARAM::Math::PI,0.0,90.0) : 0;
+    const auto avoidDistDynamic = needAvoidDynamic ? 0.5*clip(90-diff4avoid_ball *180.0 / PARAM::Math::PI,0.0,90.0)+20 : 0;
 
     double avoid_dist = 0;
     if (toBallDist > 120 && (needAvoidStatic || needAvoidDynamic)){
@@ -93,15 +95,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 = 300;
+    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));
     }
 

From 587f557a81f97df53742e0f7168ff5157a8023f3 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Tue, 16 Apr 2024 14:01:09 +0800
Subject: [PATCH 24/30] [Core] change gotoposition control with CMU2002 origin
 code

---
 Core/src/MotionControl/CMmotion.cpp      | 80 ++++++++++++++++++++++--
 Core/src/MotionControl/CMmotion.h        |  6 ++
 Core/src/MotionControl/ControlModel.cpp  | 12 ++++
 Core/src/MotionControl/ControlModel.h    |  1 +
 Core/src/Strategy/skill/GotoPosition.cpp |  2 +-
 5 files changed, 95 insertions(+), 6 deletions(-)

diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
index 3def1b6..c05d3a0 100644
--- a/Core/src/MotionControl/CMmotion.cpp
+++ b/Core/src/MotionControl/CMmotion.cpp
@@ -51,10 +51,9 @@ int timeDebugColor = COLOR_GREEN;
 /// @param	traj_accel		计算得加速度
 /// @param	traj_time		计算得加速时间
 ////////////////////////////////////////////////////////////////////////////////////////////////////
-void origin_compute_motion_1d(double x0, double v0, double v1,
+void __new_compute_motion_1d(double x0, double v0, double v1,
                               double a_max, double v_max, double a_factor,
-                              double &traj_accel, double &traj_time)
-{
+                              double &traj_accel, double &traj_time){
   // First check to see if nothing needs to be done...
   if (x0 == 0 && v0 == v1) { traj_accel = 0; traj_time = 0;  return; }
 
@@ -78,7 +77,7 @@ void origin_compute_motion_1d(double x0, double v0, double v1,
     double time_to_stop = fabs(v0) / a_max;
     double x_to_stop = v0 * v0 / (2 * a_max);
 
-    origin_compute_motion_1d(x0 + copysign(x_to_stop, v0), 0, v1, a_max * a_factor,
+    __new_compute_motion_1d(x0 + copysign(x_to_stop, v0), 0, v1, a_max * a_factor,
                       v_max, a_factor, traj_accel, traj_time);
     traj_time += time_to_stop;
  
@@ -174,10 +173,81 @@ void origin_compute_motion_1d(double x0, double v0, double v1,
     }
   }
 }
+
+void __new_compute_motion_2d(CVector x0, CVector v0, CVector v1,
+                              double a_max, double v_max, double a_factor,
+                              CVector &traj_accel, double &time){
+  double time_x, time_y;
+  double rotangle = x0.dir();
+
+  x0 = x0.rotate(-rotangle);
+  v0 = v0.rotate(-rotangle);
+  v1 = v1.rotate(-rotangle);
+  
+  double traj_accel_x, traj_accel_y;
+
+  __new_compute_motion_1d(x0.x(), v0.x(), v1.x(), a_max, v_max, a_factor,
+		    traj_accel_x, time_x);
+  __new_compute_motion_1d(x0.y(), v0.y(), v1.y(), a_max, v_max, a_factor,
+		    traj_accel_y, time_y);
+
+  traj_accel = CVector(traj_accel_x, traj_accel_y).rotate(rotangle);
+  traj_accel = Utils::Polar2Vector(std::min(traj_accel.mod(),a_max), traj_accel.dir());
+
+  time = std::max(time_x, time_y);
+}
+
+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){
+    CGeoPoint target_pos = final.Pos();
+    CVector x = start.Pos() - target_pos;
+    CVector v = start.Vel();
+    CVector target_vel = final.Vel();
+
+    CVector a;
+    double time;
+    __new_compute_motion_2d(x, v, target_vel, CAPABILITY.maxAccel, CAPABILITY.maxSpeed, ACC_FACTOR, a, time);
+
+    double w0 = Utils::Normalize(start.Dir() - final.Dir());
+    double wv = start.RotVel();
+    double wv1 = final.RotVel();
+    double aw;
+    double time_w;
+    __new_compute_motion_1d(w0, wv, wv1, CAPABILITY.maxAngularAccel, CAPABILITY.maxAngularSpeed, ANGLE_ACC_FACTOR, aw, time_w);
+
+    if (DISPLAY_ROTATION_LIMIT){
+        auto P = start.Pos() + CVector(200,200);
+        auto C = COLOR_PURPLE;
+        GDebugEngine::Instance()->gui_debug_msg(P, "TT", C);
+        GDebugEngine::Instance()->gui_debug_line(P,P+v*0.1,C);
+        GDebugEngine::Instance()->gui_debug_msg(P+v*0.2, fmt::format("V,wv:{:.1f},l-({:.1f},{:.1f})",wv, (wv + aw * FRAME_PERIOD), std::min(1000.0,CAPABILITY.maxAccel/(v.mod()))), C);
+    }
+
+    v = v + a * FRAME_PERIOD;
+
+    // option 1 : old method - bang-bang control to vw
+    // wv = wv + aw * FRAME_PERIOD;
+
+    // 思路 ： 根据移动和旋转的预计时间进行限制的分配
+    // option2 : consider conbine constraint of vw and v
+    double target_wv = wv + aw * FRAME_PERIOD;
+    wv = copysign(std::min(fabs(target_wv), CAPABILITY.maxAccel/(v.mod())), target_wv);
+
+    nextStep.SetValid(true);
+    nextStep.SetVel(v);
+    nextStep.SetPos(start.Pos() + v*FRAME_PERIOD);
+    nextStep.SetRotVel(wv);
+    nextStep.SetDir(Utils::Normalize(start.Dir() + wv * FRAME_PERIOD));
+}
+
 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){
-                        origin_compute_motion_1d(x0, v0, v1, a_max, v_max, a_factor, traj_accel, traj_time);
+                        __new_compute_motion_1d(x0, v0, v1, a_max, v_max, a_factor, traj_accel, traj_time);
                        }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/Core/src/MotionControl/CMmotion.h b/Core/src/MotionControl/CMmotion.h
index 35ea047..6b8a925 100644
--- a/Core/src/MotionControl/CMmotion.h
+++ b/Core/src/MotionControl/CMmotion.h
@@ -18,6 +18,12 @@ void goto_point_omni( const PlayerPoseT& start,
 					 const double& accel_factor,
 					 const double& angle_accel_factor,
                      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
index 187e928..3b86756 100644
--- a/Core/src/MotionControl/ControlModel.cpp
+++ b/Core/src/MotionControl/ControlModel.cpp
@@ -80,6 +80,18 @@ void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT
     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);
+}
+
 /// Trapezoidal control from ZJU : zero final velocity trajectory
 void CControlModel::makeTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability)
 {
diff --git a/Core/src/MotionControl/ControlModel.h b/Core/src/MotionControl/ControlModel.h
index 5ae52d4..ccf94e4 100644
--- a/Core/src/MotionControl/ControlModel.h
+++ b/Core/src/MotionControl/ControlModel.h
@@ -52,6 +52,7 @@ class CControlModel {
 	
 	/// Trapezoidal control from CMU : none-zero final velocity trajectory
     void makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
+    void makeNewCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
 
 	/// Trapezoidal control from ZJU : zero final velocity trajectory
 	void makeTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
diff --git a/Core/src/Strategy/skill/GotoPosition.cpp b/Core/src/Strategy/skill/GotoPosition.cpp
index e1960a7..af135c4 100644
--- a/Core/src/Strategy/skill/GotoPosition.cpp
+++ b/Core/src/Strategy/skill/GotoPosition.cpp
@@ -192,7 +192,7 @@ CPlayerCommand* CGotoPositionV2::execute(const CVisionModule* pVision)
     float usedtime = target.dist(self.Pos()) / capability.maxSpeed / 1.414;	// 单位：秒
 
     /// 进行轨迹生成并记录理想执行时间
-    control.makeCmTrajectory(self, final, capability);					// CMU 非零速到点
+    control.makeNewCmTrajectory(self, final, capability);					// CMU 非零速到点
 
     const double time_factor = 1.5;
 

From 7e5267672562c1cf791f74e2eeb6e0839a122613 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 17 Apr 2024 10:07:57 +0800
Subject: [PATCH 25/30] [Core] delete unused traj plan

---
 Core/src/MotionControl/ControlModel.cpp       |  120 --
 Core/src/MotionControl/ControlModel.h         |    9 -
 .../TrapezoidalVelTrajectory.cpp              |  983 ----------------
 .../MotionControl/TrapezoidalVelTrajectory.h  |   61 -
 .../noneTrapzodalVelTrajectory.cpp            | 1016 -----------------
 .../noneTrapzodalVelTrajectory.h              |   89 --
 6 files changed, 2278 deletions(-)
 delete mode 100644 Core/src/MotionControl/TrapezoidalVelTrajectory.cpp
 delete mode 100644 Core/src/MotionControl/TrapezoidalVelTrajectory.h
 delete mode 100644 Core/src/MotionControl/noneTrapzodalVelTrajectory.cpp
 delete mode 100644 Core/src/MotionControl/noneTrapzodalVelTrajectory.h

diff --git a/Core/src/MotionControl/ControlModel.cpp b/Core/src/MotionControl/ControlModel.cpp
index 4da6480..17eccdd 100644
--- a/Core/src/MotionControl/ControlModel.cpp
+++ b/Core/src/MotionControl/ControlModel.cpp
@@ -21,8 +21,6 @@
 #include <cornell/Trajectory.h>				// for Cornell BangBang
 #include <cornell/TrajectorySupport.h>
 
-#include <TrapezoidalVelTrajectory.h>
-#include <noneTrapzodalVelTrajectory.h>
 #include <CMmotion.h>
 #include <parammanager.h>
 
@@ -80,124 +78,6 @@ void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT
     goto_point_omni(start,final,capability,accel_factor,angle_accel_factor,_nextStep, mode);
 }
 
-/// Trapezoidal control from ZJU : zero final velocity trajectory
-void CControlModel::makeTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability)
-{
-    _pathList.clear();
-    trapezoidalVelocityPath(start,final,PARAM::Vision::FRAME_RATE,capability,_nextStep,_pathList);
-}
-
-/// Trapezoidal control from ZJU : none-zero final velocity trajectory
-void CControlModel::makeNoneTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability)
-{
-    _pathList.clear();
-    nonetrapezoidalVelocityPath(start,final,PARAM::Vision::FRAME_RATE,capability,_nextStep,_pathList);
-}
-
-/// Parameterized control from ZJU : diff-omni zero final velocity trajectory
-//void CControlModel::makePTGTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, PTG_CTRL_MODE mode)
-//{
-//	// Initialize
-//	CONTROL_MODULE->Initialize();
-//	CONTROL_MODULE->ResetMCState();
-//
-//	// Two boundary
-//	P_State initial_state;
-//	P_State final_state;
-//
-//	static double cmd_v = 0.0;
-//	static double cmd_w = 0.0;
-//
-//	if (DIFF == mode) {
-//		// start
-//		initial_state.x = start.X();
-//		initial_state.y = start.Y();
-//		initial_state.theta = start.Dir();			// 规划的初始朝向为当前车的朝向
-//		initial_state.v = start.Vel().mod();
-//		initial_state.w = start.RotVel();
-//		// final
-//		final_state.x = final.X();
-//		final_state.y = final.Y();
-//		final_state.theta = final.Dir();
-//		final_state.v = final.Vel().mod();
-//		final_state.w = final.RotVel();
-//
-//		CONTROL_MODULE->SetTransitionSpeed(initial_state.v);
-//		CONTROL_MODULE->SetRotationalSpeed(initial_state.w);
-//		CONTROL_MODULE->GenerateMoveCmd(initial_state,final_state);
-//
-//		// get result
-//		bool isFailed = CONTROL_MODULE->IS_Generate_PTG_Failed();
-//		P_PathList _ptg_pathlist = CONTROL_MODULE->GetPathList();
-//
-//		_pathList.clear();
-//		for (size_t i = 0 ; i < _ptg_pathlist.size(); i ++) {
-//			vector<double>  storeData;
-//			storeData.push_back(_ptg_pathlist[i].x);
-//			storeData.push_back(_ptg_pathlist[i].y);
-//			storeData.push_back(_ptg_pathlist[i].theta);
-//			storeData.push_back(_ptg_pathlist[i].v);
-//			storeData.push_back(_ptg_pathlist[i].w);
-//			storeData.push_back(_ptg_pathlist[i].a);
-//			_pathList.push_back(storeData);
-//		}
-//
-//		cmd_v = CONTROL_MODULE->GetTransitionSpeed();
-//		cmd_w = CONTROL_MODULE->GetRotationalSpeed();
-//
-//		_nextStep.SetVel(Utils::Polar2Vector(cmd_v,start.Dir()));
-//		_nextStep.SetRotVel(cmd_w);
-//
-//	} else if (OMNI == mode) {
-//		initial_state.x = start.X();
-//		initial_state.y = start.Y();
-//		initial_state.theta = start.Vel().dir();		// 规划的初始朝向为当前车速度的方向
-//		initial_state.v = start.Vel().mod();
-//		initial_state.w = start.RotVel();
-//
-//		if (fabs(initial_state.v) < 10) {
-//			initial_state.theta = (final.Pos() - start.Pos()).dir();
-//			initial_state.v = 10;
-//		}
-//
-//		final_state.x = final.X();
-//		final_state.y = final.Y();
-//		final_state.theta = final.Dir();
-//		final_state.v = final.Vel().mod();
-//		final_state.w = final.RotVel();
-//
-//		CONTROL_MODULE->SetTransitionSpeed(initial_state.v);
-//		CONTROL_MODULE->SetRotationalSpeed(initial_state.w);
-//		CONTROL_MODULE->GenerateMoveCmd(initial_state,final_state);
-//
-//		// get result
-//		bool isFailed = CONTROL_MODULE->IS_Generate_PTG_Failed();
-//		P_PathList _ptg_pathlist = CONTROL_MODULE->GetPathList();
-//
-//		_pathList.clear();
-//		for (size_t i = 0 ; i < _ptg_pathlist.size(); i ++) {
-//			vector<double>  storeData;
-//			storeData.push_back(_ptg_pathlist[i].x);
-//			storeData.push_back(_ptg_pathlist[i].y);
-//			storeData.push_back(_ptg_pathlist[i].theta);
-//			storeData.push_back(_ptg_pathlist[i].v);
-//			storeData.push_back(_ptg_pathlist[i].w);
-//			storeData.push_back(_ptg_pathlist[i].a);
-//			_pathList.push_back(storeData);
-//		}
-//
-//
-//		cmd_v = CONTROL_MODULE->GetTransitionSpeed();
-//		cmd_w = CONTROL_MODULE->GetRotationalSpeed();
-//
-//		_nextStep.SetVel(Utils::Polar2Vector(cmd_v,Utils::Normalize(initial_state.theta+cmd_w*1.0/60)));
-//		_nextStep.SetRotVel(1.0*cmd_w);
-//
-//	}
-//
-//	return ;
-//}
-
 //////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////
 
diff --git a/Core/src/MotionControl/ControlModel.h b/Core/src/MotionControl/ControlModel.h
index 7ba3d40..4133e06 100644
--- a/Core/src/MotionControl/ControlModel.h
+++ b/Core/src/MotionControl/ControlModel.h
@@ -53,15 +53,6 @@ class CControlModel {
 	/// Trapezoidal control from CMU : none-zero final velocity trajectory
     void makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, nonZeroMode mode = FAST);
 
-	/// Trapezoidal control from ZJU : zero final velocity trajectory
-	void makeTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
-	
-	/// Trapezoidal control from ZJU : none-zero final velocity trajectory
-	void makeNoneTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability);
-	
-	/// Parameterized control from ZJU : diff-omni zero final velocity trajectory
-	// void makePTGTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, PTG_CTRL_MODE mode = DIFF);
-	
 	/// Get the real-time next step
 	const PlayerPoseT& getNextStep() const { return _nextStep; }
 
diff --git a/Core/src/MotionControl/TrapezoidalVelTrajectory.cpp b/Core/src/MotionControl/TrapezoidalVelTrajectory.cpp
deleted file mode 100644
index 952ec73..0000000
--- a/Core/src/MotionControl/TrapezoidalVelTrajectory.cpp
+++ /dev/null
@@ -1,983 +0,0 @@
-#include <iostream>
-#include <math.h>
-#include <utils.h>
-#include "staticparams.h"
-#include <fstream>
-#include <istream>
-#include <sstream>
-#include <string>
-#include "staticparams.h"
-#include "TrapezoidalVelTrajectory.h"
-
-
-//#define _PRINT_DEBUG_MSG
-#ifdef _PRINT_DEBUG_MSG
-#define PRINT_DEBUG(x) std::cout << #x << " : " << (x) << std::endl;
-#define PRINT_MSG(x) std::cout << x << std::endl;
-#else 
-#define PRINT_DEBUG(x)
-#define PRINT_MSG(x)
-#endif
-
-//static CVector VEL_LOG[2] = {CVector(-9999,-9999), CVector(-9999, -9999)};
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	vector<vector<double>> tg_recurs(double t0, double z0, double zf, double vz0, int rev,
-/// 	int iter, double uza, double uzd, double vzMax)
-///
-/// @brief 零速到点规划。基本算法
-///       
-///        根据 t0 时刻的机器人状态，计算得到从起点到终点之间的整个规划过程。
-///     该方法假设机器人在加速和减速过程中，机器人的加速度是恒定不变的。
-///     在这个假设的基础上，根据机器人的初始状态, 将机器人的运动分为三
-///     大类：
-///        1: 初速度与位移方向不一致，先将减速到 0
-///        2: 初速度与位移方向一致
-///           2.1: 初速度小于最大速度 Vmax, 且加速至最大速度时位移不会
-///         超过目标点，则先加速至最大速度。
-///           2.2: 保持最大速度运动
-///           2.3: 以最大减速度减速，也会超过目标点，则以最大减速减速
-///        3: 初速大于最大速度，先减速到 Vmax
-/// 
-///     
-/// 
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	t0		initial time = absolute time when this stage begins
-/// @param	z0		initial position 
-/// @param	zf		final position 
-/// @param	vz0		initial velocity. 
-/// @param	rev		The reverse. 
-/// @param	iter	The iterator. 
-/// @param	uza		the acceleration. 
-/// @param	uzd		The decelerate. 
-/// @param	vzMax	The vz maximum. 
-///
-/// @return	返回规划得到的路径，路径由多段组成，每段包括以下参数：
-/// 
-///         z0U: 初始位移
-///        vz0U: 下一阶段的初速度
-///          az: 这一阶段的加速度
-///         rev: 是否改变参考方向
-///           t: 这一阶段所用时间
-///         t0U: 已用时间（不包括这一阶段）
-///
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-vector<vector<double>> tg_recurs(double t0, 
-								 double z0, 
-								 double zf, 
-								 double vz0, 
-								 int rev, 
-								 int iter, 
-								 double uza,
-								 double uzd,
-								 double vzMax)
-{
-	//std::cout<<"TrapezoidalVelTrajectory"<<std::endl;
-	double tol = 0.01;   // tolerance for machine precision
-	double zfU = zf;
-	double z0U = z0;
-	double vz0U = vz0;
-	double az;
-	double t=0;
-	double t0U = 0;
-	vector<vector<double> > DataVector;
-	iter = iter + 1;
-	if (abs(z0)!=0)
-	{
-		// Adjust offset for z0
-		zfU = zf-z0;
-		z0U = 0;
-		vz0U = vz0;
-		az = 0;
-		t = 0;
-	} 
-	while ( (abs(zfU)>=tol) || (abs(vz0U)>=tol) )
-	{
-		//std::cout<<"zfU:"<<zfU<<"vz0U:"<<vz0U<<std::endl;
-		vz0 = vz0U;
-		if (zfU<0){
-			//Destination in wrong direction, need to flip the sign
-			PRINT_MSG("Flip direction");
-			z0U = 0;
-			zfU = -zfU;
-			vz0U = -vz0U;
-			az = 0;
-			t = 0;
-			if (rev==1)
-				rev = -1;
-			else
-				rev = 1;
-		}
-		else if ( vz0U > vzMax ){
-			//Case 3: Need to decelerate until vz = vzMax
-			PRINT_MSG("Case 3");
-			t = (vz0U-vzMax)/uzd;
-			z0U = 0;
-			zfU = zfU - (vz0U*vz0U-vzMax*vzMax)/2.0/uzd;
-			vz0U = vzMax;
-			az = -uzd;
-		}
-		else if (vz0U<0){
-			// 需要反向，先把原来的速度减到零
-			//Case 1: Moving in wrong direction, need to decelerate first
-			PRINT_MSG("Case 1");
-			t = abs(vz0U/uzd);
-			double zDelta = vz0U*t + uzd*t*t/2.0;
-			z0U = 0;
-			zfU = zfU-zDelta;
-			vz0U = 0; // vz0 + uzd*t; // "correct" solution isn't great, rounding errors
-			az = uzd;
-		}    
-		else if (zfU<=(vz0U*vz0U/2.0/uzd + tol)){
-			// 从现在起以最大减速度减速，也会超过目标点
-			// Case 2.3: Need to decelerate
-			PRINT_MSG("Case 2.3");
-			t = vz0U/uzd;
-			double zDelta = vz0U*vz0U/2.0/uzd;
-			z0U = 0;
-			zfU = zfU-zDelta;
-			vz0U = 0;
-			az = -uzd;
-		}
-		else if ( fabs(vz0U - vzMax) < tol ){
-			// 计算保持最大速度的时间，这段时间运动的距离，及到减速时剩下的路程
-			//Case 2.2: Cruising at vzMax until you have to decelerate
-			PRINT_MSG("Case 2.2");
-			z0U = 0;
-			double zDelta = vzMax*vzMax/2.0/uzd;		
-			vz0U = vzMax;
-			az = 0;
-			double zCruise = zfU - zDelta;
-			t = zCruise / vzMax;
-			zfU = vzMax*vzMax/2.0/uzd;
-			PRINT_DEBUG(t);
-		}    
-		else{
-			// Case 2.1: Accelerate
-			PRINT_MSG("Case 2.1");
-			// Accelerate to vzMax
-			double t1 = (vzMax - vz0U)/uza;
-
-			// Accelerate without reaching vzMax
-			double zDelta = vz0U*vz0U/2.0/uza;
-			float temp = (float)( (zfU + zDelta)/(1.0/2/uza + 1.0/2/uzd) );
-			double vz2 = Utils::SquareRootFloat( temp );
-			double t2 = (vz2-vz0U)/uza;
-			if (t1<t2){
-				t = t1;
-				z0U = 0;
-				zfU = zfU - vz0U*t1 - uza/2.0*t1*t1;
-				vz0U = vzMax;
-				az = uza;
-			}
-			else{
-				t = t2;
-				z0U = 0;
-				zfU = zfU- vz0U*t2 - uza/2.0*t2*t2;
-				vz0U = vz2;
-				az = uza;
-			}
-		}
-		// zfU: 剩余路程
-		// vz0: 初速度
-		// vz0U: 下一阶段的初速度
-		// az: 这一阶段的加速度
-		// rev: 是否改变参考方向
-		// t: 这一阶段所用时间
-		// t0U: 已用时间（不包括这一阶段）
-		double data[6] = {z0U, vz0, az, rev, t, t0U};
-		vector<double> storeData(data,data+6);
-		DataVector.push_back(storeData);
-		t0U = t + t0U;   //Initial time for the next stage
-
-		if ( vz0U == vzMax && vzMax < 0 )
-			break;
-	}
-	double data[6] = {0, 0, 0, rev, 0, t0U};
-	vector<double> storeData(data,data+6);
-	DataVector.push_back(storeData);
-	return DataVector;
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	vector< vector<double> > tg_recurs(double t0, double z0, double zf, double vz0,
-/// 	double vzf, int rev, int iter, double uza, double uzd, double vzMax)
-///
-/// @brief	非零速到点规划。
-///  
-///         ???? 对目标的设定超过机器人的运动学约束的情况，考虑还不是很细致，有可能会出问题。
-///        根据 t0 时刻的机器人状态，计算得到从起点到终点之间的整个规划过程。
-///     该方法假设机器人在加速和减速过程中，机器人的加速度是恒定不变的。
-///     在这个假设的基础上，根据机器人的初始状态, 将机器人的运动分为三
-///     大类：
-///        1: 初速度与位移方向不一致，先将减速到 0
-///        2: 初速度与位移方向一致
-///           2.1: 初速度小于最大速度 Vmax, 且加速至最大速度时位移不会
-///         超过目标点，则先加速至最大速度。
-///           2.2: 保持最大速度运动
-///           2.3: 以最大减速度减速，也会超过目标点，则以最大减速减速
-///        3: 初速大于最大速度，先减速到 Vmax
-///
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	t0		initial time = absolute time when this stage begins
-/// @param	z0		initial position 
-/// @param	zf		final position 
-/// @param	vz0		initial velocity.
-/// @param  vzf     final velocity. 
-/// @param	rev		The reverse. 
-/// @param	iter	The iterator. 
-/// @param	uza		the acceleration. 
-/// @param	uzd		The decelerate. 
-/// @param	vzMax	The vz maximum. 
-///
-/// @return	返回规划得到的路径，路径由多段组成，每段包括以下参数：
-/// 
-///         z0U: 初始位移
-///        vz0U: 下一阶段的初速度
-///          az: 这一阶段的加速度
-///         rev: 是否改变参考方向
-///           t: 这一阶段所用时间
-///         t0U: 已用时间（不包括这一阶段）. 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-vector< vector<double> > tg_recurs(double t0,
-								   double z0,
-								   double zf,
-								   double vz0,
-								   double vzf,
-								   int rev,
-								   int iter,
-								   double uza,
-								   double uzd,
-								   double vzMax)
-{
-	// zfU: 剩余路程
-	// vz0: 初速度
-	// vz0U: 下一阶段的初速度
-	// az: 这一阶段的加速度
-	// rev: 是否改变参考方向
-	// t: 这一阶段所用时间
-	// t0U: 已用时间（不包括这一阶段）
-	const double tol = 0.01;   // tolerance for machine precision
-	const double pos_tol = vzMax/1000;
-	const double vel_tol = vzMax/1000;
-	double zfU = zf;
-	double z0U = z0;
-	double vz0U = vz0;
-	double az;
-	double t=0;
-	double t0U = 0;
-	vector<vector<double> > DataVector;
-	PRINT_DEBUG(z0);
-	PRINT_DEBUG(zf);
-	PRINT_DEBUG(vz0);
-	PRINT_DEBUG(vzf);
-	iter = iter + 1;
-	int numOfStep = 0;  // 
-//	bool canReach = true;
-
-	if (fabs(z0)!=0)
-	{
-		// Adjust offset for z0
-		zfU = zf-z0;
-		z0U = 0;
-		vz0U = vz0;
-		az = 0;
-		t = 0;
-	}
-	/// check if the robot can reach the target
-
-	while ( ((fabs(zfU)>= pos_tol) || (fabs(vz0U-vzf)>= vel_tol)) && (numOfStep < 10) )
-	{
-		//std::cout<<"zfU:"<<zfU<<"vz0U:"<<vz0U<<std::endl;
-		vz0 = vz0U;
-		if (zfU<0 && 0 == numOfStep){
-			// Destination in wrong direction, need to flip the sign
-			PRINT_MSG("Flip direction");
-			z0U = 0;
-			zfU = -zfU;
-			vz0U = -vz0U; 
-			vzf = -vzf;
-			az = 0;
-			t = 0;
-			if (rev==1)
-				rev = -1;
-			else
-				rev = 1;
-		}
-
-		else if (vz0U<0){
-			// 需要反向，先把原来的速度减到零
-			//Case 1: Moving in wrong direction, need to decelerate first
-			PRINT_MSG("Case 1");
-			t = fabs(vz0U/uzd);
-			double zDelta = vz0U*t + uzd*t*t/2.0;
-			z0U = 0;
-			zfU = zfU-zDelta;
-			vz0U = 0; // vz0 + uzd*t; // "correct" solution isn't great, rounding errors
-			az = uzd;
-			PRINT_DEBUG(t);
-			PRINT_DEBUG(zfU);
-			PRINT_DEBUG(vz0U);
-		}    
-		else if (zfU<=((vz0U*vz0U - vzf*vzf)/2.0/uzd + pos_tol)){
-			// 从现在起以最大减速度减速，也会超过目标点
-			// Case 2.3: Need to decelerate
-			PRINT_MSG("Case 2.3");
-
-			double zDelta = (vz0U*vz0U - vzf*vzf)/2.0/uzd;
-
-			t = (vz0U-vzf)/uzd;
-			zfU = zfU-zDelta;
-			vz0U = vzf;
-			//	az = -uzd;
-			PRINT_DEBUG(t);
-			PRINT_DEBUG(zfU);
-			PRINT_DEBUG(vz0U);
-
-			z0U = 0;
-			az = -uzd;
-		}
-		else if ( fabs(vz0U - vzMax) < vel_tol ){
-			// 计算保持最大速度的时间，这段时间运动的距离，及到减速时剩下的路程
-			//Case 2.2: Cruising at vzMax until you have to decelerate
-			PRINT_MSG("Case 2.2");
-			z0U = 0;
-			double zDelta = (vzMax*vzMax - vzf*vzf)/2.0/uzd;		
-			vz0U = vzMax;
-			az = 0;
-			double zCruise = zfU - zDelta;
-			t = zCruise / vzMax;
-			zfU = (vzMax*vzMax - vzf*vzf)/2.0/uzd;
-			PRINT_DEBUG(t);
-			PRINT_DEBUG(zfU);
-			PRINT_DEBUG(vz0U);
-		}    
-		else{
-			// Case 2.1: Accelerate
-			PRINT_MSG("Case 2.1");
-			// Accelerate to vzMax
-			double zDelta = ( vzf*vzf-vz0U*vz0U)/2/uza;
-
-			if( vz0U < vzf && zfU < zDelta)
-			{
-				z0U = 0;
-				double vz2 = sqrt( vz0U*vz0U+2*uza*zfU);
-				t = (vz2-vz0U)/uza;
-				vz0U = vz2;
-				zfU = 0;
-				vzf = vz0U;
-				az = uza;
-			}
-			else{
-				double t1 = (vzMax - vz0U)/uza;
-
-				// Accelerate without reaching vzMax
-				double zDelta = vz0U*vz0U/2.0/uza + vzf*vzf/2.0/uzd;
-				float temp = (float)( (zfU + zDelta)/(1.0/2/uza + 1.0/2/uzd) );
-				double vz2 = Utils::SquareRootFloat( temp );
-				double t2 = (vz2 - vz0U)/uza;
-				if (t1<t2){
-					t = t1;
-					z0U = 0;
-					zfU = zfU - vz0U*t1 - uza/2.0*t1*t1;
-					vz0U = vzMax;
-					az = uza;
-					PRINT_DEBUG(t1);
-					PRINT_DEBUG(zfU);
-					PRINT_DEBUG(vz0U);
-				}
-				else{
-					t = t2;
-					z0U = 0;
-					zfU = zfU- vz0U*t2 - uza/2.0*t2*t2;
-					vz0U = vz2;
-					az = uza;
-					PRINT_DEBUG(t2);
-					PRINT_DEBUG(zfU);
-					PRINT_DEBUG(vz0U);
-				}
-			}
-		}
-		numOfStep++;
-		PRINT_DEBUG( numOfStep );
-		double data[6] = {z0U, vz0, az, rev, t, t0U};
-		vector<double> storeData(data,data+6);
-		DataVector.push_back(storeData);
-		t0U = t + t0U;   //Initial time for the next stage
-
-		if ( vz0U == vzMax && vzMax < 0 )
-			break;
-	}
-	double data[6] = {0, 0, 0, rev, 0, t0U};
-	vector<double> storeData(data,data+6);
-	DataVector.push_back(storeData);
-	return DataVector;
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	void buildPath(const vector<vector<double>>& storeData,double& z, double& vz,
-/// 	double& azList)
-///
-/// @brief	根据函数 <tg_recurs> 的结果返回下一周期的位移 z，速度 vz 和加速度 azList。
-///           
-///         1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2];
-///         2. 速度 v = v0 + a*dt
-///         3. 位移 z= z0 + v*dt
-/// 
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	storeData		Information describing the pathe. 
-/// @param [in,out]	z		the next postion. 
-/// @param [in,out]	vz		the next velocity. 
-/// @param [in,out]	azList	the next acceration. 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void buildPath(const vector<vector<double>>& storeData,double& z, double& vz, double& azList)
-{
-	double num_stages = storeData.size();
-	vector<double> data = storeData[num_stages-1];
-	double Duration = data[5]; // 运动控制总时间
-	int stage = 0;
-	z = storeData[0][0];
-	int tfint = Duration*PARAM::Vision::FRAME_RATE;
-	int	maxStepNumber=(std::min)(tfint, 1);
-	for (int index = 0;index<=maxStepNumber;index++){
-		if (index>=Duration*PARAM::Vision::FRAME_RATE)
-			continue;
-		while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE)
-			stage = stage + 1;		
-		double t0 = storeData[stage-1][5] ;
-		double vz0 = storeData[stage-1][1];
-		double az  = storeData[stage-1][2];
-		double rev = storeData[stage-1][3];
-		azList = rev*az;	
-		vz = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0);
-		if (index!=0)
-			z+=vz0/PARAM::Vision::FRAME_RATE;	
-	}
-	// This is to prevent errors when duration = 0
-	if (tfint == 0){
-		//std::cout<<"duration = 0!!!!!"<<endl;
-		z      = storeData[0][0];
-		vz     = storeData[0][1];
-		azList = storeData[0][2];
-		//std::cout<<"tfint = 0 "<<z<<" "<<vz<<" "<<azList<<endl;
-	}	
-	//std::cout<<"stage:"<<stage<<" "<<z<<" "<<vz<<" "<<vz/PARAM::Vision::FRAME_RATE<<endl;
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	void buildPath(const vector< vector<double> >& storeData, double& z, double& vz,
-/// 	double& azList, vector< vector<double> >& pathList)
-///
-///    根据函数 <tg_recurs> 的结果返回下一周期的位移 z，速度 vz 和加速度 azList 和整条路径 pathList。
-///           
-///         1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2];
-///         2. 速度 v(t) = v0 + a*dt
-///         3. 位移 z(t) = z0 + v(t)*dt
-///
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	storeData			Information describing the store. 
-/// @param [in,out]	z			the z coordinate. 
-/// @param [in,out]	vz			the vz. 
-/// @param [in,out]	azList		list of azs. 
-/// @param [in,out]	pathList	list of pathes. 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void buildPath(const vector< vector<double> >& storeData,
-			   double& z,
-			   double& vz,
-			   double& azList,
-			   vector< vector<double> >& pathList)
-{
-	double num_stages = storeData.size();
-	vector<double> data = storeData[num_stages-1];
-	double Duration = data[5]; // 运动控制总时间
-	int stage = 0;
-	double z_t, vz_t, azList_t;
-	z_t = storeData[0][0];
-	int tfint = Duration*PARAM::Vision::FRAME_RATE;
-    int	maxStepNumber = (std::min)( int(PARAM::Vision::FRAME_RATE), (std::max)(tfint, 1));
-	int indexStep = 6;
-	for (int index = -1;index<=maxStepNumber;){
-		if( index < 1 )
-			index ++;
-		else
-			index += indexStep;
-
-		if (index>=Duration*PARAM::Vision::FRAME_RATE)
-			continue;
-		while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE)
-			stage = stage + 1;		
-		double t0  = storeData[stage-1][5];
-		double vz0 = storeData[stage-1][1];
-		double az  = storeData[stage-1][2];
-		double rev = storeData[stage-1][3];
-		azList_t = rev*az;	
-		vz_t = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0);
-		if (index!=0 ){
-			if( index == 1 )
-				z_t+=vz_t/PARAM::Vision::FRAME_RATE;
-			else 
-				z_t+=indexStep*vz_t/PARAM::Vision::FRAME_RATE;
-		}
-
-        if ( 1 == index)
-        {
-             z = z_t;
-			 vz = vz_t;
-			 azList = azList_t;
-			 //std::cout<<"stage:"<<stage<<" index: "<<index<<" "<<z<<" "<<vz<<" "<<azList<<endl;
-        }
-        double data[3] = { z_t, vz_t, azList_t };
-		vector<double> storedata(data, data+3);
-		pathList.push_back(storedata);
-	//	std::cout<<"stage:"<<stage<<" index: "<<index<<" "<<z<<" "<<vz<<" "<<azList<<endl;
-
-		
-	// This is to prevent errors when duration = 0
-	if (tfint <= 1){
-		//std::cout<<"duration = 0!!!!!"<<endl;
-		z      = storeData[0][0];
-		vz     = storeData[0][1];
-		azList = storeData[0][2];
-		//std::cout<<"tfint = 0"<<z<<" "<<vz<<" "<<azList<<endl;
-		//std::cout<<z<<" "<<vz<<" "<<azList<<endl;
-		}
-
-	}
-	}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	double syncTG(const PlayerPoseT& start, CGeoPoint finalPos,
-/// 	const PlayerCapabilityT& capability)
-///
-/// @brief	采用二分法来分配 x 和 y  方向的最大速度和最大加速度，以达到 x, y 方向所用的时间
-///         相等. 二分法搜索的变量为 alpha, 其变化范围是 [0, pi/2] 
-///
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	start		The start. 
-/// @param	finalPos	The final position. 
-/// @param	capability	The capability. 
-///
-/// @return	期望的整条路径所用的时间. 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-double syncTG(const PlayerPoseT& start, CGeoPoint finalPos,const PlayerCapabilityT& capability)
-{
-	// Do a binary search to find the proper control effort, such that the times for the
-	// trajectories in x and y direction are the same.
-	// The variable to vary is "alpha", which runs from 0 to pi/2
-	//The return value is the expected path time.
-	const double factor = 1.2;
-
-	double x0      = start.X();
-	double y0      = start.Y();
-	double xf      = finalPos.x();
-	double yf      = finalPos.y();
-	double vx0     = start.VelX();
-	double vy0     = start.VelY();
-    
-	double maxSpeed = capability.maxSpeed;
-	double maxAccel = capability.maxAccel;
-//	double maxAngleSpeed = capability.maxAngularSpeed;
-//	double maxAngleAccel = capability.maxAngularAccel;
-
-	int numIter = 10;       //number of iterations performed. error in u is 2^(-numIter)
-	double u   =  1.568;    // approx. PI/2
-	double du  = -1.568;
-    double alpha,durX,durY;
-	for (int j=1;j<=numIter;j++)
-	{
-		du = du*0.5;
-		alpha = u+du;
-		double axMax = sin(alpha)*maxAccel;
-		double ayMax = cos(alpha)*maxAccel;
-		double vxMax = sin(alpha)*maxSpeed;
-		double vyMax = cos(alpha)*maxSpeed;
-
-		//Find duration for x-direction    
-		int iter = 0;
-		double t0 = 0;
-		double rev = 1;
-		vector<vector<double> > storeDataX = tg_recurs(t0, x0, xf, vx0, rev, iter, axMax, factor*axMax, vxMax);
-		double num_stagesX = storeDataX.size();
-		durX = storeDataX[num_stagesX-1][5];
-		//Find duration for y-direction    
-		iter = 0;
-		t0 = 0;
-		rev = 1;
-		vector<vector<double> > storeDataY = tg_recurs(t0, y0, yf, vy0, rev, iter, ayMax, factor*ayMax, vyMax);
-		double num_stagesY = storeDataY.size();
-		durY =  storeDataY[num_stagesY-1][5];
-		double tfDelta = durX-durY;
-		//std::cout<<"alpha:"<<alpha<<"durx:"<<durX<<"dury:"<<durY<<endl;
-		if (tfDelta <= 0.0)
-			u=alpha;
-	}
-	double duration = (durX+durY)/2;
-	return duration;
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	void trapezoidalVelocityPath( const PlayerPoseT& start, const PlayerPoseT& final,
-/// 	double frameRate, const PlayerCapabilityT& capability, PlayerPoseT& nextStep )
-///
-/// @brief	零速到点路径规划函数。算法流程如下：
-///           1. 采用二分法，同步 x 和 y 方向的位移
-///           2. 独立计算转动方向的所化时间 t_theta, 取 t_x, t_y, t_theta 中的最大值
-///              为最后所花的时间
-///         
-///           
-/// @author	Yonghai Wu
-/// @date	2010-3-24
-///
-/// @param	start				The start. 
-/// @param	final				The final. 
-/// @param	frameRate			The frame rate. 
-/// @param	capability			The capability. 
-/// @param [in,out]	nextStep	the next step. 返回下一周的位移和速度
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void trapezoidalVelocityPath( const PlayerPoseT& start, const PlayerPoseT& final, double frameRate, const PlayerCapabilityT& capability, PlayerPoseT& nextStep )
-{
-	double x0      = start.X();
-	double y0      = start.Y();
-	double xf      = final.X();
-	double yf      = final.Y();
-	double vx0     = start.VelX();
-	double vy0     = start.VelY();
-	double vtheta0 = start.RotVel();
-	double theta0  = start.Dir();
-	double thetaf  = final.Dir();
-	double x,vx,ax,y,vy,ay,theta,vtheta,atheta;
-	if (capability.maxAccel==0 || capability.maxAngularAccel==0 || capability.maxSpeed==0 || capability.maxAngularSpeed==0){
-		cout << "trapezoidalVelocityPath code error: invalid scale(s)" << endl;
-	}
-
-	double maxSpeed = capability.maxSpeed;
-	double maxAccel = capability.maxAccel;
-    double maxAngleSpeed = capability.maxAngularSpeed;
-	double maxAngleAccel = capability.maxAngularAccel;
-	//finds the shortest "rotational path" between theta0 and thetaf
-	double dtheta=thetaf-theta0;
-	if (dtheta < -PARAM::Math::PI)
-		dtheta = dtheta + 2*PARAM::Math::PI;
-	if (dtheta > PARAM::Math::PI)
-		dtheta = dtheta - 2*PARAM::Math::PI;
-	thetaf=theta0 + dtheta;
-
-    //double controlAlpha = syncTG(x0,y0,vx0,vy0,maxAccel,maxAccel,maxSpeed);
-	// Do a binary search to find the proper control effort, such that the times for the
-	// trajectories in x and y direction are the same.
-	// The variable to vary is "alpha", which runs from 0 to pi/2
-
-	const double factor = 1.5;
-
-	int numIter = 5;       //number of iterations performed. error in u is 2^(-numIter)
-	double u   =  1.568;    // approx. PI/2
-	double du  = -1.568;
-	double alpha,durX,durY;
-	vector<vector<double> > storeDataX,storeDataY;
-	int iter = 0;
-	double t0 = 0;
-	double rev = 1;
-	for (int j=1;j<=numIter;j++)
-	{
-		du = du*0.5;
-		alpha = u+du;
-		double axMax = sin(alpha)*maxAccel;
-		double ayMax = cos(alpha)*maxAccel;
-		double vxMax = sin(alpha)*maxSpeed;
-		double vyMax = cos(alpha)*maxSpeed;
-		//std::cout<<axMax<<" "<<ayMax<<endl;
-		//Find duration for x-direction    
-		int iter = 0;
-		double t0 = 0;
-		double rev = 1;
-		storeDataX = tg_recurs(t0, x0, xf, vx0, rev, iter, axMax, factor*axMax, vxMax);
-		double num_stagesX = storeDataX.size();
-		durX = storeDataX[num_stagesX-1][5];
-		//Find duration for y-direction    
-		iter = 0;
-		t0 = 0;
-		rev = 1;
-		storeDataY = tg_recurs(t0, y0, yf, vy0, rev, iter, ayMax, factor*ayMax, vyMax);
-		double num_stagesY = storeDataY.size();
-		durY =  storeDataY[num_stagesY-1][5];
-		double tfDelta = durX-durY;
-		//std::cout<<"alpha:"<<alpha<<"durx:"<<durX<<"dury:"<<durY<<endl;
-		if (tfDelta <= 0.0)
-			u=alpha;
-	}
-	vector<vector<double> >  storeDataTheta = tg_recurs(t0,theta0,thetaf,vtheta0,rev,iter,maxAngleAccel,factor*maxAngleAccel,maxAngleSpeed);
-	buildPath(storeDataTheta,theta,vtheta,atheta);
-	buildPath(storeDataX,x,vx,ax);
-	buildPath(storeDataY,y,vy,ay);
-//	double duration = durX > durY ? durX : durY; // 运动控制总时间
-	nextStep.SetValid(true);
-	nextStep.SetPos(x,y);
-	nextStep.SetVel(vx,vy);
-	nextStep.SetDir(theta);
-	nextStep.SetRotVel(vtheta);
-
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	void trapezoidalVelocityPath( const PlayerPoseT& start, const PlayerPoseT& final,
-/// 	double frameRate, const PlayerCapabilityT& capability, PlayerPoseT& nextStep,
-/// 	vector< vector<double> >& pathList)
-///
-/// @brief	非零速到点路径规划函数。算法流程如下：
-///           1. 采用二分法，同步 x 和 y 方向的位移
-///           2. 独立计算转动方向的所化时间 t_theta, 取 t_x, t_y, t_theta 中的最大值
-///              为最后所花的时间
-///
-/// @author	Yonghai Wu
-/// @date	2010-3-24
-///
-/// @param	start				The start. 
-/// @param	final				The final. 
-/// @param	frameRate			The frame rate. 
-/// @param	capability			The capability. 
-/// @param [in,out]	nextStep	the next step.  下一周期的位移和速度
-/// @param [in,out]	pathList	list of pathes. 整条路径
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void trapezoidalVelocityPath( const PlayerPoseT& start,
-							 const PlayerPoseT& final,
-							 double frameRate,
-							 const PlayerCapabilityT& capability,
-							 PlayerPoseT& nextStep,
-							 vector< vector<double> >& pathList)
-	{
-	
-
-	double x0      = start.X();
-	double y0      = start.Y();
-	double xf      = final.X();
-	double yf      = final.Y();
-	double vx0     = start.VelX();
-	double vy0     = start.VelY();
-	double vxf     = final.VelX();
-	double vyf     = final.VelY();
-	double vtheta0 = start.RotVel();
-	double vthetaf = final.RotVel();
-	double theta0  = start.Dir();
-	double thetaf  = final.Dir();
-
-	double x,vx,ax,y,vy,ay,theta,vtheta,atheta;
-	if (capability.maxAccel==0
-		|| capability.maxAngularAccel==0
-		|| capability.maxSpeed==0
-		|| capability.maxAngularSpeed==0){
-			cout << "trapezoidalVelocityPath code error: invalid scale(s)" << endl;
-		}
-	/*std::cout<<" maxacc: "<<capability.maxAccel<<" maxdec: "<<capability.maxDec<<" maxangacc: "<<capability.maxAngularAccel
-		<<" maxangdec: "<<capability.maxAngularDec<<" maxspeed: "<<capability.maxSpeed<<endl;
-	std::cout<<" x0: "<<x0<<" xf: "<<xf<<" vx0: "<<vx0<<" vxf: "<<vxf<<endl;*/
-
-	double maxSpeed = capability.maxSpeed;
-	double maxAccel = capability.maxAccel;
-	double maxAngleSpeed = capability.maxAngularSpeed;
-	double maxAngleAccel = capability.maxAngularAccel;
-	double maxDec = capability.maxDec;
-	double maxAngleDec = capability.maxAngularDec;
-
-	//finds the shortest "rotational path" between theta0 and thetaf
-	double dtheta=thetaf-theta0;
-	if (dtheta < -PARAM::Math::PI)
-		dtheta = dtheta + 2*PARAM::Math::PI;
-	if (dtheta > PARAM::Math::PI)
-		dtheta = dtheta - 2*PARAM::Math::PI;
-	thetaf=theta0 + dtheta;
-
-	//double controlAlpha =
-	// syncTG(x0,y0,vx0,vy0,maxAccel,maxAccel,maxSpeed); Do a binary
-	// search to find the proper control effort, such that the times
-	// for the trajectories in x and y direction are the same. The
-	// variable to vary is "alpha", which runs from 0 to pi/2
-
-	//  syncTG(start, final, capability);
-
-
-	int numIter = 5;       //number of iterations performed. error in u is 2^(-numIter)
-	double u   =  1.568;    // approx. PI/2
-	double du  = -1.568;
-	double alpha,durX,durY;
-	vector<vector<double> > storeDataX,storeDataY;
-	int iter = 0;
-	double t0 = 0;
-	double rev = 1;
-
-	 double tol_pos = 1;
-	 double tol_vel = 2;
-	 double tol_theta = 0.01;
-	 double tol_vtheta = 0.02;
-	for (int j=1;j<=numIter;j++)
-		{
-			
-			
-		du = du*0.5;
-		alpha = u+du;
-		//alpha = PI/2;
-		double axMax = sin(alpha)*maxAccel;
-		double ayMax = cos(alpha)*maxAccel;
-		double adxMax = sin(alpha)*maxDec;
-		double adyMax = cos(alpha)*maxDec;
-		double vxMax = sin(alpha)*maxSpeed;
-		double vyMax = cos(alpha)*maxSpeed;
-		//std::cout<<axMax<<" "<<ayMax<<endl;
-
-		//Find duration for x-direction    
-		int iter = 0;
-		double t0 = 0;
-		double rev = 1;
-		storeDataX = tg_recurs(t0,
-			x0,
-			xf,
-			vx0,
-			vxf,
-			rev,
-			iter,
-			axMax,
-			adxMax,
-			vxMax,
-			tol_pos,
-			tol_vel);
-		double num_stagesX = storeDataX.size();
-		durX = storeDataX[num_stagesX-1][5];
-		//Find duration for y-direction    
-		iter = 0;
-		t0 = 0;
-		rev = 1;
-		storeDataY = tg_recurs(t0,
-			y0,
-			yf,
-			vy0,
-			vyf,
-			rev,
-			iter,
-			ayMax,
-			adyMax,
-			vyMax,
-			tol_pos,
-			tol_vel);
-		double num_stagesY = storeDataY.size();
-		durY =  storeDataY[num_stagesY-1][5];
-		double tfDelta = durX-durY;
-		//std::cout<<"alpha:"<<alpha<<"durx:"<<durX<<"dury:"<<durY<<endl;
-		if (tfDelta <= 0.0001 )
-			{
-			//std::cout<<j<<" alpha: "<<alpha<<" durx: "<<durX<<" dury: "<<durY<<endl;
-			u=alpha;
-			//     break;
-			}
-		if( fabs(tfDelta) < 0.0001 )
-			break;
-
-		
-		}
-	PRINT_DEBUG(alpha);
-	PRINT_DEBUG(durX);
-	PRINT_DEBUG(durY);
-
-	vector<vector<double> >  storeDataTheta = tg_recurs(t0,
-		theta0,
-		thetaf,
-		vtheta0,
-		vthetaf,
-		rev,
-		iter,
-		maxAngleAccel,
-		maxAngleDec,
-		maxAngleSpeed,
-		tol_theta,
-		tol_vtheta);
-
-	
-
-	//cout<<"theta: "<<alpha<<endl;
-	//cout<<maxAngleAccel<<"::::"<<maxAngleDec<<"::::"<<maxAngleSpeed<<endl;
-	//cout<<theta0<<"::::"<<thetaf<<"::::"<<vtheta0<<"..."<<vthetaf<<endl;
-	vector< vector<double> > pathListR, pathListX, pathListY;
-	buildPath(storeDataTheta,theta,vtheta,atheta,pathListR);
-	buildPath(storeDataX,x,vx,ax, pathListX);
-	buildPath(storeDataY,y,vy,ay, pathListY);
-
-//	double duration = durX > durY ? durX : durY; // 运动控制总时间
-
-	int pathStepX = pathListX.size();
-	int pathStepY = pathListY.size();
-	int pathStepR = pathListR.size();
-	//std::cout<<" path len: r "<<pathStepR<<std::endl;
-	PRINT_DEBUG(pathStepX);
-	PRINT_DEBUG(pathStepY);
-	PRINT_DEBUG(pathStepR);
-   
-	nextStep.SetValid(true);
-	nextStep.SetPos(x,y);
-	nextStep.SetVel(vx,vy);
-	nextStep.SetDir(theta);
-	nextStep.SetRotVel(vtheta);
-	//std::cout<<"~~ vx0: "<<vx0<<" vxf: "<<vxf<<" x0: "<<x0<<" xf: "<<xf<<" ax: "<<ax<<" t= "<<pathStepX<<std::endl;
-	//std::cout<<"~~ vY0: "<<vy0<<" vyf: "<<vy0<<" y0: "<<y0<<" yf: "<<yf<<" yx: "<<ay<<" t= "<<pathStepY<<std::endl;
-//	std::cout<<"~~ vx: "<<vx<<" v: "<<vy<<" x: "<<x0<<" y: "<<y0<<" vx0: "<<vx0<<" vy0: "<<vy0<<" xf: "<<xf<<" yf: "<<yf<<std::endl;
-	//std::cout<<" num of X: "<<pathStepX<<" num of Y: "<<pathStepY<<" num of R: "<<pathStepR<<endl;
-	int maxPathStep = max( pathStepX, max(pathStepY, pathStepR) );
-	PRINT_DEBUG(maxPathStep);
-
-	
-	
-	/// save every step of the path
-	for (int i=0; i < maxPathStep; i++)
-		{
-		double data[7] = {0};
-		data[0] = i;
-
-		if ( i >= pathStepX)
-			{
-			data[1] = 0;
-			data[4] = 0;
-			}
-		else{
-			data[1] = pathListX[i][0];
-			data[4] = pathListX[i][1];
-			}
-		if ( i >= pathStepY)
-			{
-			data[2] = 0;
-			data[5] = 0;
-			}
-		else{
-			data[2] = pathListY[i][0];
-			data[5] = pathListY[i][1];
-			}
-
-		if ( i >= pathStepR)
-			{
-			data[3] = 0;
-			data[6] = 0;
-			}
-		else{
-			data[3] = pathListR[i][0];
-			data[6] = pathListR[i][1];
-			}
-		vector<double>  storeData(data, data+7);
-		pathList.push_back(storeData);
-		//  PRINT_DEBUG(pathList.size());
-		}
-
-	
-	
-	}
diff --git a/Core/src/MotionControl/TrapezoidalVelTrajectory.h b/Core/src/MotionControl/TrapezoidalVelTrajectory.h
deleted file mode 100644
index d0b124e..0000000
--- a/Core/src/MotionControl/TrapezoidalVelTrajectory.h
+++ /dev/null
@@ -1,61 +0,0 @@
-#ifndef TRAPEZOIDAL_VEL_TRAJECTORY_H
-#define TRAPEZOIDAL_VEL_TRAJECTORY_H
-#include <WorldDefine.h>
-#include <vector>
-
-
-
-using namespace std;   
-vector<vector<double>> tg_recurs(double t0, 
-								 double z0, 
-								 double zf,
-								 double vz0, 
-								 int rev, 
-								 int iter,
-								 double uza, 
-								 double uzd, 
-								 double vzMax);
-
-vector<vector<double>> tg_recurs(double t0, 
-								 double z0, 
-								 double zf,
-								 double vz0, 
-								 double vzf,
-								 int rev, 
-								 int iter,
-								 double uza, 
-								 double uzd, 
-								 double vzMax,
-								 double tol_pos,
-								 double tol_vel);
-
-void buildPath(const vector< vector<double> > & storeData,
-			   double& z,
-			   double& vz,
-			   double& azList,
-			   vector< vector<double> >& pathList);
-
-void buildPath(const vector<vector<double>> & storeData,
-			   double& z, 
-			   double& vz, 
-			   double& azList);
-
-double syncTG(const PlayerPoseT& start,
-			  CGeoPoint finalPos,
-			  const PlayerCapabilityT& capability);
-
-void trapezoidalVelocityPath( const PlayerPoseT& start,
-						   const PlayerPoseT& final,
-						   double frameRate,
-						   const PlayerCapabilityT& capability,
-						   PlayerPoseT& nextStep
-						   );
-
-void trapezoidalVelocityPath( const PlayerPoseT& start,
-							 const PlayerPoseT& final,
-							 double frameRate,
-							 const PlayerCapabilityT& capability,
-							 PlayerPoseT& nextStep,
-							 vector< vector<double> >& pathList
-							 );
-#endif
\ No newline at end of file
diff --git a/Core/src/MotionControl/noneTrapzodalVelTrajectory.cpp b/Core/src/MotionControl/noneTrapzodalVelTrajectory.cpp
deleted file mode 100644
index 3a37a8a..0000000
--- a/Core/src/MotionControl/noneTrapzodalVelTrajectory.cpp
+++ /dev/null
@@ -1,1016 +0,0 @@
-#include <iostream>
-#include <math.h>
-#include <utils.h>
-#include "staticparams.h"
-#include <fstream>
-#include <istream>
-#include <sstream>
-#include <string>
-#include "noneTrapzodalVelTrajectory.h"
-
-
-//#define _PRINT_DEBUG_MSG
-#ifdef _PRINT_DEBUG_MSG
-#define PRINT_DEBUG(x) std::cout << #x << " : " << (x) << std::endl;
-#define PRINT_MSG(x) std::cout << x << std::endl;
-#else 
-#define PRINT_DEBUG(x)
-#define PRINT_MSG(x)
-#endif
-namespace{
-	bool SHOW_TG_TIME = true;
-	
-}
-//static CVector VEL_LOG[2] = {CVector(-9999,-9999), CVector(-9999, -9999)};
-/////////////////////////////////////////////////////////////////////
-/// @fn	vector<vector<double>> genTrajZero
-///
-/// @brief 零速到点规划。基本算法
-///       
-///     
-/// 
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	t0		initial time = absolute time when this stage begins
-/// @param	z0		initial position 
-/// @param	zf		final position 
-/// @param	vz0		initial velocity. 
-/// @param	rev		The reverse. 
-/// @param	iter	The iterator. 
-/// @param	uza		the acceleration. 
-/// @param	uzd		The decelerate. 
-/// @param	vzMax	The vz maximum. 
-///
-/// @return	返回规划得到的路径，路径由多段组成，每段包括以下参数：
-/// 
-///         z0U: 初始位移
-///        vz0U: 下一阶段的初速度
-///          az: 这一阶段的加速度
-///         rev: 是否改变参考方向
-///           t: 这一阶段所用时间
-///         t0U: 已用时间（不包括这一阶段）
-///
-///////////////////////////////////////////////////////////////////////////////////////
-
-vector<vector<double>> genTrajZero(vector<vector<double>> &DataVector,  
-                                   double zfU, 
-                                   double vz0, 
-                                   int rev, 
-                                   double uza,
-                                   double uzd,
-                                   double vzMax,
-                                   double tol_pos,
-                                   double tol_vel)
-{
-	//std::cout<<"TrapezoidalVelTrajectory"<<std::endl;
-//	double tol = 0.01;   // tolerance for machine precision
-	double z0U = 0;
-	double vz0U = vz0;
-	double az;
-	double t=0;
-	double t0U = 0;
-    double pos_factor = 1.2;
-    double vel_factor = 1.2;
-    
-
-    
-	int numOfDataVector = DataVector.size() ;
-	if (  numOfDataVector > 0 )
-	{
-		t0U = DataVector[numOfDataVector-1][5];
-	}
-    double zDelta = 0;
-    
-	while ( (abs (zfU ) >= pos_factor*tol_pos) || ( abs( vz0U ) >= vel_factor*tol_vel ) )
-	{
-		//std::cout<<"zfU:"<<zfU<<"vz0U:"<<vz0U<<std::endl;
-      vz0 = vz0U;
-      if ( zfU < 0 ){
-        //Destination in wrong direction, need to flip the sign
-        PRINT_MSG("Flip direction");
-        z0U = 0;
-        zfU = -zfU;
-        vz0U = -vz0U;
-        az = 0;
-        t = 0;
-        zDelta = 0;
-            
-        if (rev==1)
-          rev = -1;
-        else
-          rev = 1;
-      }
-		else if ( vz0U > vzMax ){
-          //Case 3: Need to decelerate until vz = vzMax
-          PRINT_MSG("Case 3");
-          t = ( vz0U - vzMax ) / uzd;
-          z0U = 0;
-          zDelta = ( vz0U*vz0U - vzMax*vzMax ) / 2.0 / uzd;
-          zfU = zfU - zDelta;
-          vz0U = vzMax;
-          az = -uzd;
-		}
-		else if ( vz0U < 0 ){
-			// 需要反向，先把原来的速度减到零
-			//Case 1: Moving in wrong direction, need to decelerate first
-			PRINT_MSG("Case 1");
-			t = abs( vz0U / uzd );
-			zDelta = vz0U*t + uzd*t*t/2.0;
-			z0U = 0;
-			zfU = zfU-zDelta;
-			vz0U = 0; // vz0 + uzd*t; // "correct" solution isn't great, rounding errors
-			az = uzd;
-		}    
-		else if (zfU <= ( vz0U*vz0U / 2.0/ uzd + tol_pos )){
-			// 从现在起以最大减速度减速，也会超过目标点
-			// Case 2.3: Need to decelerate
-			PRINT_MSG("Case 2.3");
-			t = vz0U / uzd;
-			zDelta = vz0U*vz0U/2.0/uzd;
-			z0U = 0;
-			zfU = zfU-zDelta;
-			vz0U = 0;
-			az = -uzd;
-		}
-		else if ( fabs(vz0U - vzMax) < tol_vel ){
-			// 计算保持最大速度的时间，这段时间运动的距离，及到减速时剩下的路程
-			//Case 2.2: Cruising at vzMax until you have to decelerate
-			PRINT_MSG("Case 2.2");
-			z0U = 0;
-			zDelta = vzMax*vzMax/2.0/uzd;		
-			vz0U = vzMax;
-			az = 0;
-			double zCruise = zfU - zDelta;
-			t = zCruise / vzMax;
-			zfU = vzMax*vzMax/2.0/uzd;
-		}    
-		else{
-			// Case 2.1: Accelerate
-			PRINT_MSG("Case 2.1");
-			// Accelerate to vzMax
-			double t1 = (vzMax - vz0U)/uza;
-			
-			// Accelerate without reaching vzMax
-            zDelta = vz0U*vz0U/2.0/uza;
-			float temp = (float)( (zfU + zDelta)/(1.0/2/uza + 1.0/2/uzd) );
-			double vz2 = sqrt( temp );
-			double t2 = (vz2-vz0U)/uza;
-			if (t1<t2){
-				t = t1;
-				z0U = 0;
-				zfU = zfU - vz0U*t1 - uza/2.0*t1*t1;
-				vz0U = vzMax;
-				az = uza;
-			}
-			else{
-				t = t2;
-				z0U = 0;
-				zfU = zfU- vz0U*t2 - uza/2.0*t2*t2;
-				vz0U = vz2;
-				az = uza;
-			}
-		}
-		// zfU: 剩余路程
-		// vz0: 初速度
-		// vz0U: 下一阶段的初速度
-		// az: 这一阶段的加速度
-		// rev: 是否改变参考方向
-		// t: 这一阶段所用时间
-		// t0U: 已用时间（不包括这一阶段）
-		double data[6] = {z0U, vz0, az, rev, t, t0U};
-		vector<double> storeData(data,data+6);
-		DataVector.push_back(storeData);
-		t0U = t + t0U;   //Initial time for the next stage
-
-		if ( vz0U == vzMax && vzMax < 0 )
-			break;
-	}
-	
-	double data[6] = {0, 0, 0, rev, 0, t0U};
-	vector<double> storeData(data,data+6);
-	DataVector.push_back(storeData);
-	return DataVector;
-}
-
-///////////////////////////////////////////////////////////////////////////////////////
-/// @fn	
-///
-/// @brief	非零速到点规划。
-///
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	t0		initial time = absolute time when this stage begins
-/// @param	z0		initial position 
-/// @param	zf		final position 
-/// @param	vz0		initial velocity.
-/// @param  vzf     final velocity. 
-/// @param	rev		The reverse. 
-/// @param	iter	The iterator. 
-/// @param	uza		the acceleration. 
-/// @param	uzd		The decelerate. 
-/// @param	vzMax	The vz maximum. 
-///
-/// @return	返回规划得到的路径，路径由多段组成，每段包括以下参数：
-/// 
-////////////////////////////////////////////////////////////////////////////////
-vector< vector<double> > genTrajNone(vector<vector<double>> &DataVector,
-                                     double zfU,
-                                     double vz0,
-                                     double vzf,
-                                     int rev,
-                                     double uza,
-                                     double uzd,
-                                     double vzMax,
-                                     double tol_pos,
-                                     double tol_vel)
-{
-	// zfU: 剩余路程
-	// vz0: 初速度
-	// vz0U: 下一阶段的初速度
-	// az: 这一阶段的加速度
-	// rev: 是否改变参考方向
-	// t: 这一阶段所用时间
-	// t0U: 已用时间（不包括这一阶段）
-	const double tol = 0.01;   // tolerance for machine precision
-	double z0U = 0;
-	double vz0U = vz0;
-	double az;
-	double t=0;
-	double t0U = 0;
-	int numOfDataVector = DataVector.size() ;
-	if (  numOfDataVector > 0 )
-	{
-		t0U = DataVector[numOfDataVector-1][5];
-	}
-
-	int numOfStep = 0;  // 
-//	bool canReach = true;
-    double zDelta = 0;
-   
-	/// check if the robot can reach the target
-
-	while ( ((fabs(zfU)>= 1.2*tol_pos) || (fabs(vz0U-vzf)>= 1.2*tol_pos)) && (numOfStep < 5) )
-	{
-      vz0 = vz0U;
-      if ( numOfStep > 4 )
-        std::cout<<"~~~~~~~~~~~~~~~~~~~~~~~~~~zfU:"<<zfU<<"vz0U:"<<vz0U<<std::endl;
-      if ( zfU < 0 ){
-        PRINT_MSG("Flip direction");
-        z0U = 0;
-        zfU = -zfU;
-        vz0U = -vz0U; 
-        vzf = -vzf;
-        az = 0;
-        t = 0;
-        zDelta = 0;
-        rev = rev == 1 ? -1:1;
-      }
-      else if ( vz0U  > vzMax ){
-        // 需要反向，先把原来的速度减到零
-        PRINT_MSG("Case 3");
-        t = (vz0U-vzMax)/uzd;
-        z0U = 0;
-        zDelta = (vz0U*vz0U-vzMax*vzMax)/2.0/uzd;
-        zfU = zfU - zDelta;
-        vz0U = vzMax;
-        az = -uzd;
-      }
-      else if ( vz0U < 0 ){
-        // 需要反向，先把原来的速度减到零
-        PRINT_MSG("Case 1");
-        t = fabs(vz0U/uzd);
-        zDelta = vz0U*t + uzd*t*t/2.0;
-        z0U = 0;
-        zfU = zfU-zDelta;
-        vz0U = 0; // vz0 + uzd*t; // "correct" solution isn't great, rounding errors
-        az = uzd;
-      }    
-      else if ( vz0U > vzf && zfU <= ((vz0U*vz0U - vzf*vzf)/2.0/uzd + tol_pos) ){
-        // 从现在起以最大减速度减速，也会超过目标点
-        // Case 2.3: Need to decelerate
-        PRINT_MSG("Case 2.3");
-        zDelta = (vz0U*vz0U - vzf*vzf)/2.0/uzd;
-        t = (vz0U-vzf)/uzd;
-        zfU = zfU-zDelta;
-        vz0U = vzf;
-        z0U = 0;
-        az = -uzd;
-      }
-      else if ( fabs(vz0U - vzMax) < tol_vel ){
-        // 计算保持最大速度的时间，这段时间运动的距离，及到减速时剩下的路程
-        //Case 2.2: Cruising at vzMax until you have to decelerate
-        PRINT_MSG("Case 2.2");
-        z0U = 0;
-        zDelta = (vzMax*vzMax - vzf*vzf)/2.0/uzd;		
-        vz0U = vzMax;
-        az = 0;
-        double zCruise = zfU - zDelta;
-        t = zCruise / vzMax;
-        zfU = (vzMax*vzMax - vzf*vzf)/2.0/uzd;
-      }    
-      else{
-        // Case 2.1: Accelerate
-        PRINT_MSG("Case 2.1");
-        // Accelerate to vzMax
-        double t1 = (vzMax - vz0U)/uza;
-        
-        // Accelerate without reaching vzMax
-        zDelta = vz0U*vz0U/2.0/uza + vzf*vzf/2.0/uzd;
-		float temp = (float)( (zfU + zDelta)/(1.0/2/uza + 1.0/2/uzd) );
-		double vz2 = sqrt( temp );
-        double t2 = (vz2 - vz0U)/uza;
-        if (t1<t2){
-          t = t1;
-          z0U = 0;
-          zfU = zfU - vz0U*t1 - uza/2.0*t1*t1;
-          vz0U = vzMax;
-          az = uza;
-        }
-        else{
-          t = t2;
-          z0U = 0;
-          zfU = zfU- vz0U*t2 - uza/2.0*t2*t2;
-          vz0U = vz2;
-          az = uza;
-        }
-      }
-      
-      numOfStep++;
-      //PRINT_DEBUG( numOfStep );
-		
-      double data[6] = {z0U, vz0, az, rev, t, t0U};
-      vector<double> storeData(data,data+6);
-      DataVector.push_back(storeData);
-      t0U = t + t0U;   //Initial time for the next stage
-
-      if ( vz0U == vzMax && vzMax < 0 )
-        break;
-    }
-    //std::cout<<"~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~numOfStep "<<numOfStep<<endl;
-    double data[6] = {0, 0, 0, rev, 0, t0U};
-    vector<double> storeData(data,data+6);
-    DataVector.push_back(storeData);
-    return DataVector;
-}
-
-vector< vector<double> > tg_recurs(double t0,
-								   double z0,
-								   double zf,
-								   double vz0,
-								   double vzf,
-								   int rev,
-								   int iter,
-								   double uza,
-								   double uzd,
-								   double vzMax,
-								   double tol_pos,
-								   double tol_vel)
-{
-  	const double tol = 0.01;   // tolerance for machine precision
-	//const double tol_pos = 1;	//pos_error	
-	//const double tol_vel = 2;	//vel_error
-	double zfU = zf;
-	double z0U = 0;
-	double vz0U = vz0;
-	double az;
-	double t=0;
-	double t0U = 0;
-    double zfU2;
-    
-	vector<vector<double> >  DataVector;
-    DataVector.clear();
-	iter = iter + 1;
-//	int numOfStep = 0;  //
-//	bool canReach = true;
-//    double zDelta = 0;
-
-    bool isGotoBehindFirst = false;
-    
-	if (fabs(z0)!=0)
-	{
-		// Adjust offset for z0
-		zfU = zf-z0;
-		z0U = 0;
-		vz0U = vz0;
-		az = 0;
-		t = 0;
-	}
-    /// 以位移的方向为正
-    if (zfU<0 )
-    {
-      // Destination in wrong direction, need to flip the sign
-      // display('Flip direction');
-      z0U = 0;
-      zfU = -zfU;
-      vz0U = -vz0U;
-      vzf = -vzf;
-      az = 0;
-      t = 0;
-      rev = rev == 1 ? -1 : 1;
-        
-      double data[6] = { z0U, vz0, az, rev, t, t0U };
-      vector<double> storeData(data, data + 6 );
-      DataVector.push_back( storeData );
-      t0U = t + t0U;   //Initial time for the next stage  
-    }
-    
-    int revold = rev;
-    double vf = vzf;
-    double zMinDist1, zMinDist2;
-
-    /// 达到末速度的最短距离
-    zMinDist1 = (vf*vf)/2/uza;
-    /// 末速度大于 0
-    if ( vf > 0 ){
-      if ( vz0U > 0 )
-      { /// 初速度大于 0
-        double d =  (vf*vf-vz0U*vz0U);
-        /// 末速度与初速度的关系
-        /// 如果初速度比末速度小，则 zMinDist2 等于最短加速距离
-        /// 如果初速度比末速度大，则 zMinDist2 等于最短减速距离
-        zMinDist2 = vf > vz0U ? d/2/uza : -d/2/uzd;
-      }
-      else{
-        /// 初速度小于 0
-        zMinDist2 = (vf*vf)/2/uza - (vz0U*vz0U)/2/uzd;
-      }
-      /// 如果达到末速度的所需的最短加速距离小于 zfU
-      /// 机器人需先后退到 zfU - zMinDist1， 然后再加速
-      /// 
-      if ( zMinDist2 > zfU )
-      {
-        zfU2 = zfU - zMinDist1;
-		genTrajNone(DataVector,
-			zfU2,
-			vz0U,
-			0.0,
-			rev,
-			uza,
-			uzd,
-			vzMax,
-			tol_pos,
-			tol_vel);
-        zfU = zMinDist1;
-        vz0U = 0.0;   
-        rev = revold;
-        isGotoBehindFirst = true;
-	//	std::cout<<" backup first 1 ~~~~~~~~~~~~~~~"<<endl;
-      }
-    }
-    else{ /// 末速度小于零
-      if ( vz0U > 0 )
-      { /// 初速大于零
-        zMinDist2 = zfU-vz0U*vz0U/2/uzd;
-        if (zMinDist2 < -zMinDist1){
-          zfU2 = vz0U*vz0U/2/uzd;
-          zfU = zMinDist2; 
-        }
-        else{
-          zfU2 = zfU+zMinDist1;
-          zfU = -zMinDist1;
-        }
-      }
-      else{ ///初速度小于零
-        zMinDist2 = zfU+vz0U*vz0U/2/uzd;
-        zfU2 = zfU+zMinDist1;
-        zfU = -zMinDist1;
-      }
-      
-	  genTrajNone(DataVector,
-		  zfU2,
-		  vz0U,
-		  0.0,
-		  rev,
-		  uza,
-		  uzd,
-		  vzMax,
-		  tol_pos,
-		  tol_vel);
-        vz0U = 0.0;
-		
-        rev = revold;
-        isGotoBehindFirst = true;
-        //std::cout<<" backup first 2 ~~~~~~~~~~~~~~~"<<endl;
-    }
-	
-  
-			genTrajNone(DataVector,
-                              zfU,
-                              vz0U,
-                              vzf,
-                              rev,
-                              uza,
-                              uzd,
-                              vzMax,
-                              tol_pos,
-                              tol_vel);
-  
-  return DataVector;
-}
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	void buildPath(const vector<vector<double>>& storeData,double& z, double& vz,
-/// 	double& azList)
-///
-/// @brief	根据函数 <tg_recurs> 的结果返回下一周期的位移 z，速度 vz 和加速度 azList。
-///           
-///         1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2];
-///         2. 速度 v = v0 + a*dt
-///         3. 位移 z= z0 + v*dt
-/// 
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	storeData		Information describing the pathe. 
-/// @param [in,out]	z		the next postion. 
-/// @param [in,out]	vz		the next velocity. 
-/// @param [in,out]	azList	the next acceration. 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void buildPathNone(const vector<vector<double>>& storeData,double& z, double& vz, double& azList)
-{
-	double num_stages = storeData.size();
-	vector<double> data = storeData[num_stages-1];
-	double Duration = data[5]; // 运动控制总时间
-	int stage = 0;
-	z = storeData[0][0];
-	int tfint = Duration*PARAM::Vision::FRAME_RATE;
-	int	maxStepNumber=(std::min)(tfint, 1);
-	for (int index = 0;index<=maxStepNumber;index++){
-		if (index>=Duration*PARAM::Vision::FRAME_RATE)
-			continue;
-		while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE)
-			stage = stage + 1;		
-		double t0 = storeData[stage-1][5] ;
-		double vz0 = storeData[stage-1][1];
-		double az  = storeData[stage-1][2];
-		double rev = storeData[stage-1][3];
-		azList = rev*az;	
-		vz = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0);
-		if (index!=0)
-			z+=vz0/PARAM::Vision::FRAME_RATE;	
-	}
-	// This is to prevent errors when duration = 0
-	if (tfint == 0){
-		//std::cout<<"duration = 0!!!!!"<<endl;
-		z      = storeData[0][0];
-		vz     = storeData[0][1];
-		azList = storeData[0][2];
-		//std::cout<<"tfint = 0 "<<z<<" "<<vz<<" "<<azList<<endl;
-	}	
-	//std::cout<<"stage:"<<stage<<" "<<z<<" "<<vz<<" "<<vz/PARAM::Vision::FRAME_RATE<<endl;
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	void buildPath(const vector< vector<double> >& storeData, double& z, double& vz,
-/// 	double& azList, vector< vector<double> >& pathList)
-///
-///    根据函数 <tg_recurs> 的结果返回下一周期的位移 z，速度 vz 和加速度 azList 和整条路径 pathList。
-///           
-///         1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2];
-///         2. 速度 v(t) = v0 + a*dt
-///         3. 位移 z(t) = z0 + v(t)*dt
-///
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	storeData			Information describing the store. 
-/// @param [in,out]	z			the z coordinate. 
-/// @param [in,out]	vz			the vz. 
-/// @param [in,out]	azList		list of azs. 
-/// @param [in,out]	pathList	list of pathes. 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void buildPathNone(const vector< vector<double> >& storeData,
-			   double& z,
-			   double& vz,
-			   double& azList,
-			   vector< vector<double> >& pathList)
-{
-	double num_stages = storeData.size();
-	vector<double> data = storeData[num_stages-1];
-	double Duration = data[5]; // 运动控制总时间
-
-	if ( abs(Duration) < 0.00001 )
-	{
-		double data[3] = { z, vz, azList };
-		vector<double> storedata(data, data+3);
-		pathList.push_back(storedata);
-		return;
-	}
-	int stage = 0;
-	double z_t, vz_t, azList_t;
-	z_t = storeData[0][0];
-	int tfint = Duration*PARAM::Vision::FRAME_RATE;
-	int	maxStepNumber = /*(std::max)(tfint, 1) */(std::min)( 1000, (std::max)(tfint, 1));
-	int indexStep = 3;
-	for (int index = -1;index<=maxStepNumber;){
-		if( index < 1 )
-			index ++;
-		else
-			index += indexStep;
-
-		if (index>=Duration*PARAM::Vision::FRAME_RATE)
-			continue;
-		while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE)
-			stage = stage + 1;		
-		double t0  = storeData[stage-1][5];
-		double vz0 = storeData[stage-1][1];
-		double az  = storeData[stage-1][2];
-		double rev = storeData[stage-1][3];
-		azList_t = rev*az;	
-		vz_t = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0);
-		if (index!=0 ){
-			if( index == 1 )
-				z_t+=vz_t/PARAM::Vision::FRAME_RATE;
-			else 
-				z_t+=indexStep*vz_t/PARAM::Vision::FRAME_RATE;
-		}
-
-        if ( 1 == index)
-        {
-             z = z_t;
-			 vz = vz_t;
-			 azList = azList_t;
-			 //std::cout<<"stage:"<<stage<<" index: "<<index<<" "<<z<<" "<<vz<<" "<<azList<<endl;
-        }
-        double data[3] = { z_t, vz_t, azList_t };
-		vector<double> storedata(data, data+3);
-		pathList.push_back(storedata);
-	//	std::cout<<"stage:"<<stage<<" index: "<<index<<" "<<z<<" "<<vz<<" "<<azList<<endl;
-
-		
-	// This is to prevent errors when duration = 0
-	if (tfint <= 1){
-		//std::cout<<"duration = 0!!!!!"<<endl;
-		z      = storeData[0][0];
-		vz     = storeData[0][1];
-		azList = storeData[0][2];
-		//std::cout<<"tfint = 0"<<z<<" "<<vz<<" "<<azList<<endl;
-		//std::cout<<z<<" "<<vz<<" "<<azList<<endl;
-		}
-
-	}
-	}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	double syncTG(const PlayerPoseT& start, CGeoPoint finalPos,
-/// 	const PlayerCapabilityT& capability)
-///
-/// @brief	采用二分法来分配 x 和 y  方向的最大速度和最大加速度，以达到 x, y 方向所用的时间
-///         相等. 二分法搜索的变量为 alpha, 其变化范围是 [0, pi/2] 
-///
-/// @author	Yonghai Wu
-/// @date	2009-12-9
-///
-/// @param	start		The start. 
-/// @param	finalPos	The final position. 
-/// @param	capability	The capability. 
-///
-/// @return	期望的整条路径所用的时间. 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-double syncTGNone(const PlayerPoseT& start, CGeoPoint finalPos,const PlayerCapabilityT& capability)
-{
-	// Do a binary search to find the proper control effort, such that the times for the
-	// trajectories in x and y direction are the same.
-	// The variable to vary is "alpha", which runs from 0 to pi/2
-	//The return value is the expected path time.
-	const double factor = 1.2;
-
-	double x0      = start.X();
-	double y0      = start.Y();
-	double xf      = finalPos.x();
-	double yf      = finalPos.y();
-	double vx0     = start.VelX();
-	double vy0     = start.VelY();
-    
-	double maxSpeed = capability.maxSpeed;
-	double maxAccel = capability.maxAccel;
-//	double maxAngleSpeed = capability.maxAngularSpeed;
-//	double maxAngleAccel = capability.maxAngularAccel;
-
-	int numIter = 10;       //number of iterations performed. error in u is 2^(-numIter)
-	double u   =  1.568;    // approx. PI/2
-	double du  = -1.568;
-    double alpha,durX,durY;
-	for (int j=1;j<=numIter;j++)
-	{
-		du = du*0.5;
-		alpha = u+du;
-		double axMax = sin(alpha)*maxAccel;
-		double ayMax = cos(alpha)*maxAccel;
-		double vxMax = sin(alpha)*maxSpeed;
-		double vyMax = cos(alpha)*maxSpeed;
-
-		//Find duration for x-direction    
-		int iter = 0;
-		double t0 = 0;
-		double rev = 1;
-		vector<vector<double> > storeDataX = tg_recurs(t0, x0, xf, vx0, 1,rev, iter, axMax, factor*axMax, vxMax);
-		double num_stagesX = storeDataX.size();
-		durX = storeDataX[num_stagesX-1][5];
-		//Find duration for y-direction    
-		iter = 0;
-		t0 = 0;
-		rev = 1;
-		vector<vector<double> > storeDataY = tg_recurs(t0, y0, yf, vy0, 1,rev, iter, ayMax, factor*ayMax, vyMax);
-		double num_stagesY = storeDataY.size();
-		durY =  storeDataY[num_stagesY-1][5];
-		double tfDelta = durX-durY;
-		//std::cout<<"alpha:"<<alpha<<"durx:"<<durX<<"dury:"<<durY<<endl;
-		if (tfDelta <= 0.0)
-			u=alpha;
-	}
-	double duration = (durX+durY)/2;
-	return duration;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-/// @fn	void trapezoidalVelocityPath( const PlayerPoseT& start, const PlayerPoseT& final,
-/// 	double frameRate, const PlayerCapabilityT& capability, PlayerPoseT& nextStep,
-/// 	vector< vector<double> >& pathList)
-///
-/// @brief	非零速到点路径规划函数。算法流程如下：
-///           1. 采用二分法，同步 x 和 y 方向的位移
-///           2. 独立计算转动方向的所化时间 t_theta, 取 t_x, t_y, t_theta 中的最大值
-///              为最后所花的时间
-///
-/// @author	Yonghai Wu
-/// @date	2010-3-24
-///
-/// @param	start				The start. 
-/// @param	final				The final. 
-/// @param	frameRate			The frame rate. 
-/// @param	capability			The capability. 
-/// @param [in,out]	nextStep	the next step.  下一周期的位移和速度
-/// @param [in,out]	pathList	list of pathes. 整条路径
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void nonetrapezoidalVelocityPath( const PlayerPoseT& start,
-							 const PlayerPoseT& final,
-							 double frameRate,
-							 const PlayerCapabilityT& capability,
-							 PlayerPoseT& nextStep,
-							 vector< vector<double> >& pathList)
-	{
-	
-
-	double x0      = start.X();
-	double y0      = start.Y();
-	double xf      = final.X();
-	double yf      = final.Y();
-	double vx0     = start.VelX();
-	double vy0     = start.VelY();
-	double vxf     = final.VelX();
-	double vyf     = final.VelY();
-	double vtheta0 = start.RotVel();
-	double vthetaf = final.RotVel();
-	double theta0  = start.Dir();
-	double thetaf  = final.Dir();
-
-	double x = x0,vx = vx0 ,ax = 0,y = y0,vy = vy0,ay=0,theta = theta0,vtheta = vtheta0,atheta = 0;
-	if (capability.maxAccel==0
-		|| capability.maxAngularAccel==0
-		|| capability.maxSpeed==0
-		|| capability.maxAngularSpeed==0){
-			cout << "trapezoidalVelocityPath code error: invalid scale(s)" << endl;
-		}
-	/*std::cout<<" maxacc: "<<capability.maxAccel<<" maxdec: "<<capability.maxDec<<" maxangacc: "<<capability.maxAngularAccel
-		<<" maxangdec: "<<capability.maxAngularDec<<" maxspeed: "<<capability.maxSpeed<<endl;*/
-	
-	double maxSpeed = capability.maxSpeed;
-	double maxAccel = capability.maxAccel;
-	double maxAngleSpeed = capability.maxAngularSpeed;
-	double maxAngleAccel = capability.maxAngularAccel;
-	double maxDec = capability.maxDec;
-	double maxAngleDec = capability.maxAngularDec;
-
-	//finds the shortest "rotational path" between theta0 and thetaf
-	double dtheta=thetaf-theta0;
-	if (dtheta < -PARAM::Math::PI)
-		dtheta = dtheta + 2*PARAM::Math::PI;
-	if (dtheta > PARAM::Math::PI)
-		dtheta = dtheta - 2*PARAM::Math::PI;
-	thetaf=theta0 + dtheta;
-
-	//double controlAlpha =
-	// syncTG(x0,y0,vx0,vy0,maxAccel,maxAccel,maxSpeed); Do a binary
-	// search to find the proper control effort, such that the times
-	// for the trajectories in x and y direction are the same. The
-	// variable to vary is "alpha", which runs from 0 to pi/2
-
-	//  syncTG(start, final, capability);
-
-
-	int numIter = 10;       //number of iterations performed. error in u is 2^(-numIter)
-	double u   =  1.568;    // approx. PI/2
-	double du  = -1.568;
-	double alpha,durX,durY;
-	vector<vector<double> > storeDataX,storeDataY;
-	int iter = 0;
-	double t0 = 0;
-    double rev = 1;
-	float normOfFinalSpeed =  sqrt(vxf*vxf + vyf*vyf);
-		if ( normOfFinalSpeed > maxSpeed*0.9)
-		{
-			//std::cout<<"nonetrap: final speed is too big"<<endl;
-            vxf = vxf/normOfFinalSpeed*maxSpeed*0.9;
-			vyf = vyf/normOfFinalSpeed*maxSpeed*0.9;
-		}
-	for (int j=1;j<=numIter;j++)
-		{	
-			storeDataX.clear();
-			storeDataY.clear();
-		du = du*0.5;
-		alpha = u+du;
-		//alpha = PI/2;
-		double axMax = sin(alpha)*maxAccel;
-		double ayMax = cos(alpha)*maxAccel;
-		double adxMax = sin(alpha)*maxDec;
-		double adyMax = cos(alpha)*maxDec;
-		double vxMax = sin(alpha)*maxSpeed;
-		double vyMax = cos(alpha)*maxSpeed;
-		
-		//std::cout<<axMax<<" "<<ayMax<<endl;
-		if ( vxMax < abs(vxf) ){
-			vxMax =abs(vxf);
-		    vyMax = sqrt(maxSpeed*maxSpeed - vxMax*vxMax);       
-		}
-		if ( vyMax < abs(vyf) ){
-			vyMax = abs(vyf);
-		    vxMax = sqrt(maxSpeed*maxSpeed - vyMax*vyMax);
-
-		}
-		//Find duration for x-direction    
-		int iter = 0;
-		double t0 = 0;
-		double rev = 1;
-
-		//////////////////////////////////////////////////////////////////////////
-
-		storeDataX = tg_recurs(t0,
-			x0,
-			xf,
-			vx0,
-			vxf,
-			rev,
-			iter,
-			axMax,
-			adxMax,
-			vxMax);
-		double num_stagesX = storeDataX.size();
-		durX = storeDataX[num_stagesX-1][5];
-		
-		//Find duration for y-direction    
-		iter = 0;
-		t0 = 0;
-		rev = 1;
-		storeDataY = tg_recurs(t0,
-			y0,
-			yf,
-			vy0,
-			vyf,
-			rev,
-			iter,
-			ayMax,
-			adyMax,
-			vyMax);
-		double num_stagesY = storeDataY.size();
-		////////////////////////////////////////////////////////////////////////////
-		
-		
-		////////////////////////////////////////////////////////////////////////////
-		durY =  storeDataY[num_stagesY-1][5];
-		double tfDelta = durX-durY;
-		
-		//std::cout<<"alpha:"<<alpha<<"durx:"<<durX<<"dury:"<<durY<<endl;
-		if (tfDelta <= 0.0001 )
-			{
-			//std::cout<<j<<" alpha: "<<alpha<<" durx: "<<durX<<" dury: "<<durY<<endl;
-			u=alpha;
-			//     break;
-			}
-		if( fabs(tfDelta) < 0.001 )
-			break;
-
-		
-		}
-	
-
-
-	//////////////////////////////////////////////////////////////////////////
-//	_usecTimer.start();
-	//////////////////////////////////////////////////////////////////////////
-
-	//PRINT_DEBUG(alpha);
-	//PRINT_DEBUG(durX);
-	//PRINT_DEBUG(durY);
-
-	vector<vector<double> >  storeDataTheta = tg_recurs(t0,
-		theta0,
-		thetaf,
-		vtheta0,
-		vthetaf,
-		rev,
-		iter,
-		maxAngleAccel,
-		maxAngleDec,
-		maxAngleSpeed);
-
-	vector< vector<double> > pathListR, pathListX, pathListY;
-	buildPathNone(storeDataTheta,theta,vtheta,atheta,pathListR);
-	buildPathNone(storeDataX,x,vx,ax, pathListX);
-	buildPathNone(storeDataY,y,vy,ay, pathListY);
-	
-
-//	double duration = durX > durY ? durX : durY; // 运动控制总时间
-
-	int pathStepX = pathListX.size();
-	int pathStepY = pathListY.size();
-	int pathStepR = pathListR.size();
-	//std::cout<<" path len: r "<<pathStepR<<std::endl;
-	////PRINT_DEBUG(pathStepX);
-	//PRINT_DEBUG(pathStepY);
-	//PRINT_DEBUG(pathStepR);
-   
-	nextStep.SetValid(true);
-	nextStep.SetPos(x,y);
-	nextStep.SetVel(vx,vy);
-	nextStep.SetDir(theta);
-	nextStep.SetRotVel(vtheta);
-	//std::cout<<"~~ vx0: "<<vx0<<" vxf: "<<vxf<<" x0: "<<x0<<" xf: "<<xf<<" ax: "<<ax<<" t= "<<pathStepX<<std::endl;
-	//std::cout<<"~~ vY0: "<<vy0<<" vyf: "<<vy0<<" y0: "<<y0<<" yf: "<<yf<<" yx: "<<ay<<" t= "<<pathStepY<<std::endl;
-	// std::cout<<"~~ vx: "<<vx<<" vy: "<<vy<<std::endl;
-	//std::cout<<" num of X: "<<pathStepX<<" num of Y: "<<pathStepY<<" num of R: "<<pathStepR<<endl;
-	int maxPathStep = max( pathStepX, max(pathStepY, pathStepR) );
-	//PRINT_DEBUG(maxPathStep);
-
-	/// save every step of the path
-	for (int i=0; i < maxPathStep; i++)
-		{
-		double data[7] = {0};
-		data[0] = i;
-
-		if ( i >= pathStepX)
-			{
-			if ( pathStepX > 0)
-			{
-				data[4] = pathListX[pathStepX-1][1];
-				data[1] = pathListX[pathStepX-1][0]+(i-pathStepX+1)*data[4]/PARAM::Vision::FRAME_RATE;
-			} 
-			else
-			{
-				data[4] = vx0;
-				data[1] = 0;
-			}
-			
-			}
-		else{
-			data[1] = pathListX[i][0];
-			data[4] = pathListX[i][1];
-			}
-		if ( i >= pathStepY)
-			{
-				if ( pathStepY > 0 )
-				{
-					data[5] = pathListY[pathStepY-1][1];
-					data[2] = pathListY[pathStepY-1][0]+(i-pathStepY+1)*data[5]/PARAM::Vision::FRAME_RATE;
-				} 
-				else
-				{
-					data[2] = 0;
-					data[5] = vy0;
-				}
-			
-			}
-		else{
-			data[2] = pathListY[i][0];
-			data[5] = pathListY[i][1];
-			}
-
-		if ( i >= pathStepR)
-			{
-				if ( pathStepR > 0)
-				{
-					data[6] = pathListR[pathStepR-1][1];
-					data[3] = pathListR[pathStepR-1][0]+(i-pathStepR+1)*data[6]/PARAM::Vision::FRAME_RATE;
-				} 
-				else
-				{
-					data[3] = 0;
-					data[6] = vtheta0;
-				}
-			
-			}
-		else{
-			data[3] = pathListR[i][0];
-			data[6] = pathListR[i][1];
-			}
-		vector<double>  storeData(data, data+7);
-		pathList.push_back(storeData);
-		//  PRINT_DEBUG(pathList.size());
-		}
-	}
diff --git a/Core/src/MotionControl/noneTrapzodalVelTrajectory.h b/Core/src/MotionControl/noneTrapzodalVelTrajectory.h
deleted file mode 100644
index d7a4113..0000000
--- a/Core/src/MotionControl/noneTrapzodalVelTrajectory.h
+++ /dev/null
@@ -1,89 +0,0 @@
-//////////////////////////////////////////////////////////////////////
-// @file:	algorithm\noneTrapzodalVelTrajectory.h
-// @brief:  非零速到点轨迹规划算法
-//          基于运动学分析的运动控制方法。其算法的核心思想如下：
-//         该方法假设机器人在加速和减速过程中，机器人的加速度是恒定不变的。
-//     在这个假设的基础上，根据机器人的初始状态, 将机器人的运动分为三
-//     大类：
-//        1: 初速度与位移方向不一致，先将减速到 0
-//        2: 初速度与位移方向一致
-//           2.1: 初速度小于最大速度 Vmax, 且加速至最大速度时位移不会
-//         超过目标点，则先加速至最大速度。
-//           2.2: 保持最大速度运动
-//           2.3: 以最大减速度减速，也会超过目标点，则以最大减速减速
-//        3: 初速大于最大速度，先减速到 Vmax
-//        
-//      参考文献:
-//           Trajectory generation and control for four wheeled
-//           omnidirectional vehicles.
-
-///           Oliver Purwin, Raffaello D'Andrea. Robotics and
-//           Autonomous Systems 54 (2006) 13–22 
-// summary:	Declares the nonetrapzodal vel trajectory class
-/////////////////////////////////////////////////////////////////////
-
-#ifndef NONE_TRAPEZOIDAL_VEL_TRAJECTORY_H
-#define NONE_TRAPEZOIDAL_VEL_TRAJECTORY_H
-#include <WorldDefine.h>
-#include <vector>
-
-
-using namespace std;  
-
-vector<vector<double>> genTrajZero(vector<vector<double>> &DataVector, 
-                                   double zfU, 
-                                   double vz0, 
-                                   int rev, 
-                                   double uza, 
-                                   double uzd, 
-                                   double vzMax,
-                                   double tol_pos,
-                                   double tol_vel);
-
-vector<vector<double>> genTrajNone(vector<vector<double>> &DataVector, 
-                                   double zfU,
-                                   double vz0, 
-                                   double vzf,
-                                   int rev, 
-                                   double uza, 
-                                   double uzd, 
-                                   double vzMax,
-                                   double tol_pos,
-                                   double tol_vel);
-
-vector<vector<double>> tg_recurs(double t0, 
-								 double z0, 
-								 double zf,
-								 double vz0, 
-								 double vzf,
-								 int rev, 
-								 int iter,
-								 double uza, 
-								 double uzd, 
-								 double vzMax);
-
-void buildPathNone(const vector< vector<double> > & storeData,
-			   double& z,
-			   double& vz,
-			   double& azList,
-			   vector< vector<double> >& pathList);
-
-void buildPathNone(const vector<vector<double>> & storeData,
-			   double& z, 
-			   double& vz, 
-			   double& azList);
-
-double syncTGNone(const PlayerPoseT& start,
-			  CGeoPoint finalPos,
-			  const PlayerCapabilityT& capability);
-
-
-
-void nonetrapezoidalVelocityPath( const PlayerPoseT& start,
-							 const PlayerPoseT& final,
-							 double frameRate,
-							 const PlayerCapabilityT& capability,
-							 PlayerPoseT& nextStep,
-							 vector< vector<double> >& pathList
-							 );
-#endif

From 8c9858879983e5509453029cda0b4007741c17b6 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 17 Apr 2024 15:21:34 +0800
Subject: [PATCH 26/30] [Core] update

---
 Core/src/MotionControl/CMmotion.cpp  | 15 ++++++++++-----
 Core/tactics/skill/Touch.cpp         |  2 +-
 ZBin/lua_scripts/skill/GoCmuRush.lua |  1 -
 3 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
index c05d3a0..7e6d855 100644
--- a/Core/src/MotionControl/CMmotion.cpp
+++ b/Core/src/MotionControl/CMmotion.cpp
@@ -488,16 +488,21 @@ double expectedCMPathTime(const PlayerPoseT& start, const CGeoPoint& final, doub
 
 double predictedTime(const PlayerVisionT& start, const CGeoPoint & Target, const CVector& targetVel) {
     CVector x = start.Pos() - Target;
-    CVector v = (start.Vel().mod() < 2.5*10) ? CVector(0, 0) : start.Vel();
+    CVector v = start.Vel();
     double time;
     CVector a;
-    double accel_factor = 1.5;
+    double accel_factor = 1.8;
     if(IS_SIMULATION) {
         accel_factor = 1.0;
     }
-    compute_motion_2d(x, v, targetVel,
+    // compute_motion_2d(x, v, targetVel,
+    //                   OUR_MAX_ACC,
+    //                   OUR_MAX_DEC,
+    //                   OUR_MAX_SPEED,
+    //                   accel_factor,
+    //                   a, time);
+    __new_compute_motion_2d(x, v, targetVel,
                       OUR_MAX_ACC,
-                      OUR_MAX_DEC,
                       OUR_MAX_SPEED,
                       accel_factor,
                       a, time);
@@ -507,7 +512,7 @@ double predictedTime(const PlayerVisionT& start, const CGeoPoint & Target, const
 
 double predictedTimeWithRawVel(const PlayerVisionT& start, const CGeoPoint & Target, const CVector& targetVel) {
     CVector x = start.Pos() - Target;
-    CVector v = (start.RawVel().mod() < 2.5*10) ? CVector(0, 0) : start.RawVel();
+    CVector v = start.RawVel();
 //    GDebugEngine::Instance()->gui_debug_msg(start.Pos(), QString("vel: (%1, %2)").arg(v.x()).arg(v.y()).toLatin1());
     double time;
     CVector a;
diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
index 9811f1d..23e7805 100644
--- a/Core/tactics/skill/Touch.cpp
+++ b/Core/tactics/skill/Touch.cpp
@@ -46,7 +46,7 @@ void CTouch::plan(const CVisionModule* pVision){
     CGeoPoint bestPos = BallSpeedModel::instance()->poseForTime(1.0).Pos();
     for(double dist = 0; dist < 3000; dist += 20){
         auto pos = ballPos + Utils::Polar2Vector(dist, ballVelDir);
-        double t1 = predictedTimeWithRawVel(me, pos);
+        double t1 = predictedTime(me, pos);
         double t2 = BallSpeedModel::Instance()->timeForDist(dist);
         if (DEBUG_SWITCH){
             GDebugEngine::Instance()->gui_debug_x(pos,COLOR_GREEN);
diff --git a/ZBin/lua_scripts/skill/GoCmuRush.lua b/ZBin/lua_scripts/skill/GoCmuRush.lua
index 59d85bb..13ce7e8 100644
--- 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

From a3572462389eb051d3d11113495eedf50c335fca Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 17 Apr 2024 16:09:04 +0800
Subject: [PATCH 27/30] [Core] add predictTime ratio

---
 Core/src/MotionControl/CMmotion.cpp | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
index a294d76..0db3711 100644
--- a/Core/src/MotionControl/CMmotion.cpp
+++ b/Core/src/MotionControl/CMmotion.cpp
@@ -28,6 +28,8 @@ double OUR_MAX_ACC = ZSS::ZParamManager::instance()->value("CGotoPositionV2/MNor
 double OUR_MAX_DEC = ZSS::ZParamManager::instance()->value("CGotoPositionV2/MNormalDec",QVariant(450*10)).toDouble();
 bool DISPLAY_ROTATION_LIMIT = ZSS::ZParamManager::instance()->value("Debug/RotationLimit",QVariant(false)).toBool();//true;
 bool DEBUG_TIME = ZSS::ZParamManager::instance()->value("Debug/TimePredict", QVariant(false)).toBool();
+double PREDICT_TIME_ACC_RATIO = ZSS::ZParamManager::instance()->value("CGotoPositionV2/PredictAccRatio", QVariant(1.5)).toDouble();
+double PREDICT_TIME_THEIR_ACC_RATIO = ZSS::ZParamManager::instance()->value("CGotoPositionV2/PredictAccTheirRatio", QVariant(1.5)).toDouble();
 bool addComp = true;
 int timeDebugColor = COLOR_GREEN;
 int timeItor = 0;
@@ -483,11 +485,11 @@ double expectedCMPathTime(const PlayerPoseT& start, const CGeoPoint& final, doub
 
 double predictedTime(const PlayerVisionT& start, const CGeoPoint & Target, const CVector& targetVel) {
     CVector x = start.Pos() - Target;
-    CVector v = (start.Vel().mod() < 2.5*10) ? CVector(0, 0) : start.Vel();
+    CVector v = start.Vel();
     double time;
     CVector a;
     double time_acc, time_dec, time_flat;
-    double accel_factor = 1.5;
+    double accel_factor = PREDICT_TIME_ACC_RATIO;
     if(IS_SIMULATION) {
         accel_factor = 1.0;
     }
@@ -503,12 +505,12 @@ double predictedTime(const PlayerVisionT& start, const CGeoPoint & Target, const
 
 double predictedTimeWithRawVel(const PlayerVisionT& start, const CGeoPoint & Target, const CVector& targetVel) {
     CVector x = start.Pos() - Target;
-    CVector v = (start.RawVel().mod() < 2.5*10) ? CVector(0, 0) : start.RawVel();
+    CVector v = start.RawVel();
 //    GDebugEngine::Instance()->gui_debug_msg(start.Pos(), QString("vel: (%1, %2)").arg(v.x()).arg(v.y()).toLatin1());
     double time;
     CVector a;
     double time_acc, time_dec, time_flat;
-    double accel_factor = 1.5;
+    double accel_factor = PREDICT_TIME_ACC_RATIO;
     if(IS_SIMULATION) {
         accel_factor = 1.0;
     }
@@ -535,7 +537,7 @@ double predictedTheirTime(const PlayerVisionT& start, const CGeoPoint & Target,
                       max_acc,
                       max_acc,
                       max_speed,
-                      1.5,
+                      PREDICT_TIME_THEIR_ACC_RATIO,
                       a, time, time_acc, time_dec, time_flat);
 
     return time;

From 6d971056ddb3e11a612ab3a84e5647a8df17aad9 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 17 Apr 2024 18:28:22 +0800
Subject: [PATCH 28/30] [Core] update touch

---
 Core/tactics/skill/Touch.cpp | 17 ++++++++---------
 ZBin/lua_scripts/Play.lua    |  4 ++--
 2 files changed, 10 insertions(+), 11 deletions(-)

diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
index b7a35c1..58debf0 100644
--- a/Core/tactics/skill/Touch.cpp
+++ b/Core/tactics/skill/Touch.cpp
@@ -41,12 +41,11 @@ void CTouch::plan(const CVisionModule* pVision){
     // predict ball pos
     const auto ballStopPose = BallSpeedModel::instance()->poseForTime(9999);
 
-
     // stupid version of getballPos
     CGeoPoint bestPos = BallSpeedModel::instance()->poseForTime(1.0).Pos();
     for(double dist = 0; dist < 3000; dist += 100){
         auto pos = ballPos + Utils::Polar2Vector(dist, ballVelDir);
-        double t1 = predictedTimeWithRawVel(me, pos);
+        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);
@@ -58,11 +57,11 @@ void CTouch::plan(const CVisionModule* pVision){
     const auto getBallPos = bestPos; // TODO : replace with GetBallPos after skillutils
 
     const CGeoSegment ballRunningSeg(ballPos, ballStopPose.Pos());
-    const auto me2segTime = predictedTimeWithRawVel(me, projectionRobotPos);
+    const auto me2segTime = predictedTime(me, projectionRobotPos);
     const auto ball2segTime = BallSpeedModel::Instance()->timeForDist(ballPos.dist(projectionMousePos));
     const bool canWaitForBall = ballRunningSeg.IsPointOnLineOnSegment(projectionMousePos) && me2segTime < ball2segTime;
-    const auto predictPos = ballVelMod > 300 ? getBallPos : ballPos;
-    const double ballVel_ball2Target_ad = ballVelMod > 300 ? angleDiff(ballVelDir, (target - ballPos).dir()) : 180;
+    const auto predictPos = ballVelMod > 500 ? getBallPos : ballPos;
+    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);
@@ -75,14 +74,14 @@ void CTouch::plan(const CVisionModule* pVision){
     const auto me2TargetSeg = CGeoSegment(me.Pos(), targetRunPos);  
     const auto runTarget2kickTarget = target - targetRunPos;
 
-    const auto BALL_STATIC = ballVelMod < 300;
+    const auto BALL_STATIC = ballVelMod < 500;
     const auto runTargetDiff = angleDiff(me2target.dir(), runTarget2kickTarget.dir());
     const auto needAvoidStatic = BALL_STATIC && runTargetDiff;
-    const auto avoidDistStatic = needAvoidStatic ? 30.0 : 0.0;
+    const auto avoidDistStatic = needAvoidStatic ? 80.0 : 0.0;
 
     const auto diff4avoid_ball = std::abs(angleDiff(me2target.dir(), (targetMousePos - ballPos).dir()));
     const auto needAvoidDynamic = !BALL_STATIC && me2TargetSeg.IsPointOnLineOnSegment(ballPos) && diff4avoid_ball < 90 / 180.0 * PARAM::Math::PI;
-    const auto avoidDistDynamic = needAvoidDynamic ? 3*clip(90-diff4avoid_ball *180.0 / PARAM::Math::PI,0.0,90.0) : 0;
+    const auto avoidDistDynamic = needAvoidDynamic ? 1.5*clip(90-diff4avoid_ball *180.0 / PARAM::Math::PI,30.0,90.0) : 0;
 
     double avoid_dist = 0;
     if (toBallDist > 120 && (needAvoidStatic || needAvoidDynamic)){
@@ -95,7 +94,7 @@ void CTouch::plan(const CVisionModule* pVision){
     newTask.player.angle = targetRunDir;
     newTask.player.vel = targetRunVel;
     newTask.player.flag = taskFlag;
-    newTask.ball.avoid_dist = 300;
+    newTask.ball.avoid_dist = avoid_dist;
     setSubTask("SmartGoto", newTask);
 
     if(DEBUG_SWITCH){
diff --git a/ZBin/lua_scripts/Play.lua b/ZBin/lua_scripts/Play.lua
index f5dc7c9..2756e07 100644
--- a/ZBin/lua_scripts/Play.lua
+++ b/ZBin/lua_scripts/Play.lua
@@ -257,9 +257,9 @@ function RunPlay(name)
 
 						if isDirOk or bit:_and(mflag, flag.force_kick) ~= 0 then
 							if mkick == kick.flat() then
-								kickStatus:setKick(roleNum, mkp)
+								kickStatus:setKick(roleNum, _c(mkp))
 							elseif mkick == kick.chip() then
-								kickStatus:setChipKick(roleNum, mcp)
+								kickStatus:setChipKick(roleNum, _c(mcp))
 							end
 						end
 					end

From 78ce7b98ae0b2ec587fda9daf6ed31200722bc08 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Wed, 17 Apr 2024 22:31:19 +0800
Subject: [PATCH 29/30] [Core] add predict for obs

---
 Core/src/MotionControl/CMmotion.cpp |  3 ++-
 Core/src/OtherLibs/cmu/obstacle.cpp | 19 +++++++++++--------
 Core/tactics/skill/Touch.cpp        | 15 ++++++++-------
 3 files changed, 21 insertions(+), 16 deletions(-)

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);

From ec8e5b8d7c427324dca51fc357f2c0b0374ab4a5 Mon Sep 17 00:00:00 2001
From: mark <mark_zjunlict@zju.edu.cn>
Date: Fri, 26 Apr 2024 22:33:52 +0800
Subject: [PATCH 30/30] [Core] add switch to new capsule obs[default=false]

---
 Core/src/OtherLibs/cmu/obstacle.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/Core/src/OtherLibs/cmu/obstacle.cpp b/Core/src/OtherLibs/cmu/obstacle.cpp
index b03a165..bd212a7 100644
--- a/Core/src/OtherLibs/cmu/obstacle.cpp
+++ b/Core/src/OtherLibs/cmu/obstacle.cpp
@@ -35,6 +35,7 @@ namespace  {
     const double BALL_AVOID_DIST = PARAM::Field::BALL_SIZE + 5.0f;
     const float DEC_MAX = 450;
     const float ACCURATE_AVOID = 250;
+    bool USE_NEW_CAPSULE_OBS = ZSS::ZParamManager::instance()->value("CSmartGotoV2/USE_CAPSULE_OBS", QVariant(false)).toBool();
 }
 //====================================================================//
 //    Obstacle class implementation
@@ -410,7 +411,7 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra
                         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_dist = USE_NEW_CAPSULE_OBS ? std::min(teammate.Vel().mod() * 0.5, 1000.0) : 0.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);
             }
@@ -425,7 +426,7 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra
                 if((target.dist(opp.Pos()) < PARAM::Field::MAX_PLAYER_SIZE / 2) ) {
                     continue;
                 }
-                auto move_dist = std::min(opp.Vel().mod() * 0.5, 1000.0); // move for 0.5s
+                auto move_dist = USE_NEW_CAPSULE_OBS ? std::min(opp.Vel().mod() * 0.5, 1000.0) : 0.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);
             }