From d03de8fec943a4553a21a40b5e98ed3a7ae590d1 Mon Sep 17 00:00:00 2001 From: mark Date: Sun, 14 Apr 2024 16:19:39 +0800 Subject: [PATCH] [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 #include #include #include +#include #include #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;iloadParam(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;itx, 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;iloadParam(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;itx, 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 << " " < 0.1) qDebug() << "id: " << id<< " "< 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: "<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 << " " < 0.1) qDebug() << "id: " << id<< " "< 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: "<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 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 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 -#include -#include -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; }