diff --git a/Client/src/actionmodule.cpp b/Client/src/actionmodule.cpp index 4e2c3b7..3db37ef 100644 --- a/Client/src/actionmodule.cpp +++ b/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 diff --git a/Client/src/simmodule.cpp b/Client/src/simmodule.cpp index 7735508..53b7669 100644 --- a/Client/src/simmodule.cpp +++ b/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; diff --git a/Controller/src/radiopacket.cpp b/Controller/src/radiopacket.cpp index a201fb6..900384e 100644 --- a/Controller/src/radiopacket.cpp +++ b/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); diff --git a/Controller/src/serialobject.cpp b/Controller/src/serialobject.cpp index cc43999..b617927 100644 --- a/Controller/src/serialobject.cpp +++ b/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 = ""; } diff --git a/Controller/src/serialobject.h b/Controller/src/serialobject.h index bc0efb3..e26099b 100644 --- a/Controller/src/serialobject.h +++ b/Controller/src/serialobject.h @@ -41,6 +41,7 @@ class SerialObject : public QObject QByteArray defaultIndex; QSerialPort serial; RadioPacket radioPacket; + QByteArray rx; }; #endif // SERIALOBJECT_H diff --git a/Core/src/Strategy/skill/OpenSpeed.cpp b/Core/src/Strategy/skill/OpenSpeed.cpp index e2127bf..a42791a 100644 --- a/Core/src/Strategy/skill/OpenSpeed.cpp +++ b/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)); diff --git a/Core/src/Wireless/RobotSensor.cpp b/Core/src/Wireless/RobotSensor.cpp index b284f5a..f1ac5e7 100644 --- a/Core/src/Wireless/RobotSensor.cpp +++ b/Core/src/Wireless/RobotSensor.cpp @@ -1,4 +1,3 @@ -#include #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(); } -} \ No newline at end of file +} diff --git a/Core/src/Wireless/RobotSensor.h b/Core/src/Wireless/RobotSensor.h index e8aaeab..575d0f0 100644 --- a/Core/src/Wireless/RobotSensor.h +++ b/Core/src/Wireless/RobotSensor.h @@ -2,6 +2,7 @@ #define _ROBOT_FILTERED_INFO_H_ //#include +#include #include #include "staticparams.h" diff --git a/ZBin/lua_scripts/play/Test/Benchmark/TestBenchmark_Dribble.lua b/ZBin/lua_scripts/play/Test/Benchmark/TestBenchmark_Dribble.lua index c0776f1..6b661a5 100644 --- a/ZBin/lua_scripts/play/Test/Benchmark/TestBenchmark_Dribble.lua +++ b/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", diff --git a/ZBin/lua_scripts/skill/RunMultiPos.lua b/ZBin/lua_scripts/skill/RunMultiPos.lua index 529e094..ee45cfd 100644 --- a/ZBin/lua_scripts/skill/RunMultiPos.lua +++ b/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 diff --git a/ZBin/lua_scripts/worldmodel/task.lua b/ZBin/lua_scripts/worldmodel/task.lua index 3b54e33..7377173 100644 --- a/ZBin/lua_scripts/worldmodel/task.lua +++ b/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