modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-03-22 07:30:42 +01:00
Code Issues Packages Projects Releases Wiki Activity

fix v2

Browse Source
This commit is contained in:
ackimixs
2024-04-17 23:34:19 +02:00
parent 343bac0229
commit c4f92323c5
1 changed files with 68 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
TCPServer.cpp
View File

@@ -596,7 +596,6 @@ void TCPServer::startGameBlueTeam() {
this->broadcastMessage("strat;arduino;speed;130\n");
this->broadcastMessage("strat;arduino;go;762,0\n");
usleep(3'000'000);
this->broadcastMessage("strat;arduino;speed;200\n");
for (int i = 0; i < 3; i++) {
if (pinceState[i] == WHITE_FLOWER) {
@@ -609,6 +608,12 @@ void TCPServer::startGameBlueTeam() {
usleep(100'000);
}
}
this->broadcastMessage("strat;arduino;speed;200\n");
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y + 350)) + "\n";
this->broadcastMessage(toSend);
awaitRobotIdle();
}
/*
@@ -626,45 +631,8 @@ void TCPServer::startGameBlueTeam() {
this->broadcastMessage("strat;arduino;angle;-157\n");
awaitRobotIdle();
timeout = 0;
found = false;
while (!found) {
for (const auto & arucoTag : this->arucoTags) {
if (TCPUtils::endWith(arucoTag.name(), "flower")) {
if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
tag = arucoTag;
found = true;
break;
}
}
}
if (!found) {
this->broadcastMessage("start;aruco;get aruco;1\n");
usleep(500'000);
timeout++;
if (timeout > 10) {
return;
}
}
}
if (pinceState[1] == NONE) {
goToAruco(tag, 1);
} else if (pinceState[2] == NONE) {
goToAruco(tag, 2);
} else if (pinceState[0] == NONE) {
goToAruco(tag, 0);
} else {
return;
}
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y - 350)) + "\n";
this->broadcastMessage(toSend);
awaitRobotIdle();
this->broadcastMessage("strat;arduino;angle;157\n");
awaitRobotIdle();
arucoTags.clear();
this->broadcastMessage("strat;aruco;get aruco;1\n");
timeout = 0;
found = false;
@@ -706,6 +674,52 @@ void TCPServer::startGameBlueTeam() {
this->broadcastMessage("strat;arduino;angle;157\n");
awaitRobotIdle();
arucoTags.clear();
this->broadcastMessage("strat;aruco;get aruco;1\n");
timeout = 0;
found = false;
while (!found) {
for (const auto & arucoTag : this->arucoTags) {
if (TCPUtils::endWith(arucoTag.name(), "flower")) {
if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
tag = arucoTag;
found = true;
break;
}
}
}
if (!found) {
this->broadcastMessage("start;aruco;get aruco;1\n");
usleep(500'000);
timeout++;
if (timeout > 10) {
return;
}
}
}
if (pinceState[1] == NONE) {
goToAruco(tag, 1);
} else if (pinceState[2] == NONE) {
goToAruco(tag, 2);
} else if (pinceState[0] == NONE) {
goToAruco(tag, 0);
} else {
return;
}
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y - 350)) + "\n";
this->broadcastMessage(toSend);
awaitRobotIdle();
this->broadcastMessage("strat;arduino;angle;157\n");
awaitRobotIdle();
arucoTags.clear();
this->broadcastMessage("strat;aruco;get aruco;1\n");
timeout = 0;
found = false;
while (!found) {
@@ -746,7 +760,7 @@ void TCPServer::startGameBlueTeam() {
this->broadcastMessage("strat;arduino;angle;314\n");
awaitRobotIdle();
std::vector<int> pinceHavePurpleFlower;
pinceHavePurpleFlower.clear();
for (int i = 0; i < 3; i++) {
if (pinceState[i] == PURPLE_FLOWER) {
pinceHavePurpleFlower.push_back(i);
@@ -800,6 +814,12 @@ void TCPServer::startGameBlueTeam() {
this->broadcastMessage(toSend);
usleep(100'000);
}
this->broadcastMessage("strat;arduino;speed;200\n");
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y + 350)) + "\n";
this->broadcastMessage(toSend);
awaitRobotIdle();
}
}*/
@@ -932,7 +952,7 @@ void TCPServer::startGameBlueTeam() {
this->broadcastMessage("strat;arduino;angle;314\n");
awaitRobotIdle();
std::vector<int> pinceHavePurpleFlower;
pinceHavePurpleFlower.clear();
for (int i = 0; i < 3; i++) {
if (pinceState[i] == PURPLE_FLOWER) {
pinceHavePurpleFlower.push_back(i);
@@ -986,6 +1006,12 @@ void TCPServer::startGameBlueTeam() {
this->broadcastMessage(toSend);
usleep(100'000);
}
this->broadcastMessage("strat;arduino;speed;200\n");
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y + 350)) + "\n";
this->broadcastMessage(toSend);
awaitRobotIdle();
}
}*/