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 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 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 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;j7s}",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;igui_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