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

aruco things

Browse Source
This commit is contained in:
ackimixs
2024-04-30 11:30:24 +02:00
parent 7ba9adb2bd
commit af3254f086
1 changed files with 28 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
TCPServer.cpp
View File

@@ -810,13 +810,20 @@ void TCPServer::handleEmergency(int distance, double angle) {
void TCPServer::startTestAruco(int pince) {
this->arucoTags.clear();
this->broadcastMessage("strat;aruco;get aruco;1\n");
for (int i = 0; i < 5; i++) {
usleep(500'000);
this->broadcastMessage("strat;aruco;get aruco;1\n");
}
std::optional<ArucoTag> tag = getBiggestArucoTag(300, 1500, -600, 600);
std::optional<ArucoTag> tag = std::nullopt;
int timeout = 0;
while (!tag.has_value()) {
this->broadcastMessage("strat;aruco;get aruco;1\n");
usleep(500'000);
tag = getMostCenteredArucoTag(100, 800, -400, 400);
timeout++;
if (timeout > 10) {
break;
}
}
if (tag.has_value()) {
goToAruco(tag.value(), pince);
@@ -895,15 +902,22 @@ void TCPServer::findAndGoFlower(StratPattern sp) {
return;
}
this->arucoTags.clear();
this->broadcastMessage("strat;aruco;get aruco;1\n");
for (int i = 0; i < 4; i++) {
usleep(500'000);
this->broadcastMessage("strat;aruco;get aruco;1\n");
}
usleep(100'000);
std::optional<ArucoTag> tag = getMostCenteredArucoTag(100, 800, -200, 200);
this->arucoTags.clear();
std::optional<ArucoTag> tag = std::nullopt;
int timeout = 0;
while (!tag.has_value()) {
this->broadcastMessage("strat;aruco;get aruco;1\n");
usleep(500'000);
tag = getMostCenteredArucoTag(100, 800, -400, 400);
timeout++;
if (timeout > 10) {
break;
}
}
if (tag.has_value()) {
if (pinceState[1] == NONE) {