modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-03-29 03:49:35 +02:00
Code Issues Packages Projects Releases Wiki Activity

ask pos every 50ms

Browse Source
This commit is contained in:
ackimixs
2024-04-10 22:45:12 +02:00
parent 444a65f550
commit fb1c46efc3
2 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
TCPServer.cpp
View File

@@ -160,8 +160,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
if (tokens[2] == "set pos") {
std::vector<std::string> pos = split(tokens[3], ",");
this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100};
std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100) + "\n";
this->broadcastMessage(toSend, clientSocket);
}
if (tokens[2] == "get pos") {
std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100) + "\n";
@@ -257,6 +255,7 @@ int TCPServer::nbClients() const {
void TCPServer::start()
{
std::thread([this]() { acceptConnections(); }).detach();
std::thread([this]() { askArduinoPos(); }).detach();
}
void TCPServer::checkIfAllClientsReady()
@@ -292,7 +291,7 @@ void TCPServer::startGame() {
// pi/4
this->broadcastMessage("strat;arduino;angle;0");
this->robotPose.theta = 0.785398;
this->robotPose.theta = 0;
usleep(1'000'000);
this->broadcastMessage("strat;aruco;get aruco;1\n");
while (this->waitForAruco) {
@@ -355,3 +354,10 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) {
havePot[pince] = true;
this->broadcastMessage("strat;arduino;speed;200\n");
}
void TCPServer::askArduinoPos() {
while (!this->shouldStop) {
this->broadcastMessage("strat;arduino;get pos;1\n");
usleep(50'000);
}
}