Merge branch 'main' of github.com:Robocup-ssl-China/rocos

Client/src/actionmodule.cpp

@@ -479,7 +479,16 @@ bool ActionModuleSerialVersion::closeSerialPort(){
     return false;
 }
 void ActionModuleSerialVersion::readData(){
-    rx = serial.readAll();
+    QByteArray pack = serial.readAll();
+    if(pack.length() > 0 && int(pack[0]) == 0xff){
+        rx = pack;
+    }else{
+        rx += pack;
+    }
+    if(rx.length() < 25){
+        return;
+    }
+    qDebug() << "recv : 0x" << rx;
     auto& data = rx;
     int id = 0;
     bool infrared = false;
@@ -489,7 +498,7 @@ void ActionModuleSerialVersion::readData(){
     int capacitance = 0;
     if(data[0] == (char)0xff){
         if(data[1] == (char)0x02){
-            id       = (quint8)data[2] - 1;//old protocal
+            id       = (quint8)data[2] + (int(data[23]) == 0?0:-1); // if old protocol data[23] should not be zero, 23 in new protocol is zero
             infrared = (quint8)data[3] & 0x40;
             flat     = (quint8)data[3] & 0x20;
             chip     = (quint8)data[3] & 0x10;
@@ -507,7 +516,7 @@ void ActionModuleSerialVersion::readData(){
         //            capacitance = (quint8)data[4];
         //        }
     }
-
+    rx = "";
 }
 
 namespace {
@@ -582,6 +591,12 @@ quint8 kickStandardization(quint8 id, bool mode, quint16 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;
+    KParamManager::instance()->loadParam(ratio, key, 1.0);
+    return v*ratio;
+}
 void encodeNJLegacy(const NJ_Command& command,QByteArray& tx,int num){
 
     auto& TXBuff = tx;
@@ -591,9 +606,16 @@ void encodeNJLegacy(const NJ_Command& command,QByteArray& tx,int num){
     qint16 vy = (qint16)(command.vy);
     qint16 ivr = (qint16)(command.vr);
     qint16 vr = abs(ivr)> 511 ? (ivr > 0 ? 1 : -1)*(511) : (ivr);
+
+    vx = velRegulation(real_num,vx);
+    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;
+    if(dribble < 0) dribble = 0;
     // vx
     unsigned int vx_value_uint = (unsigned int)abs(vx);
     // vy

Client/src/simmodule.cpp

@@ -16,7 +16,7 @@ namespace ZSS {
 namespace {
 bool NoVelY = false;
 bool trans_dribble(double dribble) {
-    return dribble>1;
+    return dribble>0.5;
 }
 double trans_length(double v) {
     return v / 1000.0;

Controller/src/radiopacket.cpp

@@ -75,7 +75,7 @@ bool RadioPacket::sendCommand(){
     if(times == 0) timer.start();
     if(serialPtr != NULL){
         encode();
-        qDebug() << "0x" << transmitPacket.toHex();
+//        qDebug() << "0x" << transmitPacket.toHex();
         //transmitPacket是包含命令的包
         transmitPacket.data();
         serialPtr->write((transmitPacket.data()),TRANSMIT_PACKET_SIZE);

Controller/src/serialobject.cpp

@@ -88,8 +88,16 @@ void SerialObject::sendStartPacket(){
     radioPacket.sendStartPacket();
 }
 void SerialObject::readData(){
-    QByteArray rx = serial.readAll();
-    qDebug() << rx.toHex();
+    QByteArray pack = serial.readAll();
+    if(pack.length() > 0 && int(pack[0]) == 0xff){
+        rx = pack;
+    }else{
+        rx += pack;
+    }
+    if(rx.length() < 25){
+        return;
+    }
+    qDebug() << "recv : " << rx.toHex();
     auto& data = rx;
     int id = 0;
     bool infrared = false;
@@ -99,11 +107,12 @@ void SerialObject::readData(){
     int capacitance = 0;
     if(data[0] == (char)0xff){
         if(data[1] == (char)0x02){
-            id       = (quint8)data[2] - 1;
+            id       = (quint8)data[2] + (int(data[23]) == 0?0:-1);// ???????
             infrared = (quint8)data[3] & 0x40;
             flat     = (quint8)data[3] & 0x20;
             chip     = (quint8)data[3] & 0x10;
             qDebug() << id << ' ' << infrared << ' ' << flat << ' ' << chip << ' ' << battery << ' ' << capacitance;
         }
     }
+    rx = "";
 }

Controller/src/serialobject.h

@@ -41,6 +41,7 @@ class SerialObject : public QObject
     QByteArray defaultIndex;
     QSerialPort serial;
     RadioPacket radioPacket;
+    QByteArray rx;
 };
 
 #endif // SERIALOBJECT_H

Core/src/Strategy/skill/OpenSpeed.cpp

@@ -19,7 +19,7 @@ CPlayerCommand* COpenSpeed::execute(const CVisionModule* pVision) {
 	double dribblePower = DribbleStatus::Instance()->getDribbleCommand(num);
 	int flag = task().player.flag;
 	if (flag & PlayerStatus::DRIBBLING && dribblePower < 0.5){
-		dribblePower = 1.0;
+        dribblePower = 3.0;
 	}
 	bool needReport = task().player.needReport;
 	return CmdFactory::Instance()->newCommand(CPlayerSpeedV2(num, speedX, speedY, speedR, dribblePower, flag, needReport));

Core/src/Wireless/RobotSensor.cpp

@@ -1,4 +1,3 @@
-#include <fmt/core.h>
 #include "RobotSensor.h"
 CRobotSensor::CRobotSensor(){
     ZSS::ZParamManager::instance()->loadParam(DEBUG_DEVICE,"Debug/DeviceMsg",false);
@@ -50,4 +49,4 @@ void CRobotSensor::Update(const CVisionModule* pVision){
         }
         robotMsg[i]._mutex.unlock();
     }
-}
+}

Core/src/Wireless/RobotSensor.h

@@ -2,6 +2,7 @@
 #define _ROBOT_FILTERED_INFO_H_
 
 //#include <CommControl.h>
+#include <fmt/core.h>
 #include <singleton.h>
 #include "staticparams.h"
 

ZBin/lua_scripts/play/Test/Benchmark/TestBenchmark_Dribble.lua

@@ -1,17 +1,81 @@
 local START_POS = CGeoPoint:new_local(1000,1000)
+
+local VW_MAX = 12.7
+local AW = 16
+
+local vw = 0.0
+local w = 0.0
+local F_vw = function()
+    return vw
+end
+local F_w = function()
+    return w
+end
+
+local result_list = {} -- {acc,vw_max}
+
+local poses = {
+    CGeoPoint:new_local(2500,2500),
+    CGeoPoint:new_local(2500,500),
+    CGeoPoint:new_local(500,500),
+    CGeoPoint:new_local(500,2500),
+}
+
+local DEBUG_MSG = function()
+    local sx,sy = 200,-1000
+    local span = 140
+    local sp = CGeoPoint:new_local(sx,sy)
+    local v = CVector:new_local(0,-span)
+    debugEngine:gui_debug_msg(sp + v*0,string.format("Acc : %4.1f",AW),param.BLUE)
+    debugEngine:gui_debug_msg(sp + v*1,string.format("Vw  : %4.1f",vw),param.BLUE)
+    debugEngine:gui_debug_msg(sp + v*2,string.format("INF : %4d",player.infraredCount(9)),param.BLUE)
+    debugEngine:gui_debug_msg(sp + v*3,string.format("INF : %4d",player.infraredOn(9) and 1 or 0),param.BLUE)
+end
+
+local update = function()
+    vw = vw + AW / param.frameRate
+    if vw > VW_MAX then
+        vw = VW_MAX
+        AW = -AW
+    elseif vw < -VW_MAX then
+        vw = -VW_MAX
+        AW = -AW
+    end
+    w = (w + vw*param.frameRate)
+    w = math.modf(w,math.pi*2)
+    DEBUG_MSG()
+end
+
+local reset = function()
+    vw = 0.0
+end
+
+
 gPlayTable.CreatePlay{
 firstState = "start",
--- ["start"] = {
---     switch = function()
---     end,
---     Leader = task.placeBall(START_POS),
---     match = "(L)"
--- },
+["start"] = {
+    switch = function()
+        DEBUG_MSG()
+        if bufcnt(player.toTargetDist("Leader") < 100 and player.infraredCount("Leader")>20 ,param.frameRate*1) then
+            return "run"
+        end
+    end,
+    -- Leader = task.placeBall(START_POS),
+    Leader = task.goCmuRush(START_POS,0,nil,flag.dribbling),
+    match = "(L)"
+},
 
 ["run"] = {
     switch = function()
+        update()
+        if bufcnt(not player.infraredOn("Leader"), 5) then
+            reset()
+            return "start"
+        end
     end,
-    match = ""
+    -- Leader = task.openSpeed(0,0,F_vw,flag.dribbling),
+    Leader = task.runMultiPos(poses, true, 100, 0.0, 2000, flag.dribbling),
+    match = "{L}"
 },
 
 name = "TestBenchmark_Dribble",

ZBin/lua_scripts/skill/RunMultiPos.lua

@@ -10,6 +10,14 @@ function RunMultiPos(task)
  	local lastCycle = 0
 
 	execute = function(runner)
+		for i = 1, table.getn(task.pos) do
+			if type(task.pos[i]) == "function" then
+				mpos[i] = task.pos[i]()
+			else
+				mpos[i] = task.pos[i]
+			end
+		end
+
 		if player.pos(runner):dist(mpos[curIndex]) < mdist then
 			local nextIndex = (curIndex) % table.getn(mpos) + 1
 			if mclose == nil or mclose then

ZBin/lua_scripts/worldmodel/task.lua

@@ -61,7 +61,7 @@ function goSimplePos(p, d, f)
 	return {mexe, mpos}
 end
 
-function runMultiPos(p, c, d, idir, a)
+function runMultiPos(p, c, d, idir, a, f)
 	if c == nil then
 		c = false
 	end
@@ -74,7 +74,7 @@ function runMultiPos(p, c, d, idir, a)
 		idir = dir.shoot()
 	end
 
-	local mexe, mpos = RunMultiPos{ pos = p, close = c, dir = idir, flag = flag.not_avoid_our_vehicle, dist = d, acc = a}
+	local mexe, mpos = RunMultiPos{ pos = p, close = c, dir = idir, flag = f, dist = d, acc = a}
 	return {mexe, mpos}
 end