modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-01-20 01:45:50 +01:00
Code Issues Packages Projects Releases Wiki Activity

change position

Browse Source
This commit is contained in:
ackimixs
2024-05-03 12:16:28 +02:00
parent 4e6fadc00a
commit 2cf2a2d18a
1 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
TCPServer.cpp
View File

@@ -751,10 +751,8 @@ void TCPServer::handleArucoTag(ArucoTag &tag) {
for (auto& t : arucoTags) {
if (tag.id() == t.id()) {
float tPosX = t.pos()[0];
float tPosY = t.pos()[1];
float tagPosX = tag.pos()[0];
float tagPosY = tag.pos()[1];
auto [tPosX, tPosY] = t.pos();
auto [tagPosX, tagPosY] = tag.pos();
if (tagPosX > tPosX - 10 && tagPosX < tPosX + 10 && tagPosY > tPosY - 10 && tagPosY < tPosY + 10) {
t.find();
return;
@@ -878,34 +876,38 @@ void TCPServer::findAndGoFlower(const StratPattern sp) {
this->setSpeed(200);
if (team == BLUE) {
if (sp == TAKE_FLOWER_TOP) {
this->go(1000, 250);
this->go(300, 700);
awaitRobotIdle();
this->rotate(-PI/2);
this->setSpeed(180);
this->rotate(0);
awaitRobotIdle();
}
else if (sp == TAKE_FLOWER_BOTTOM) {
this->go(1000, 1790);
this->go(300, 1300);
awaitRobotIdle();
this->rotate(PI/2);
this->setSpeed(180);
this->rotate(0);
awaitRobotIdle();
} else {
return;
}
} else if (team == YELLOW) {
if (sp == TAKE_FLOWER_TOP) {
this->go(2000, 250);
this->go(1700, 700);
awaitRobotIdle();
this->rotate(-PI/2);
this->setSpeed(180);
this->rotate(-PI);
awaitRobotIdle();
}
else if (sp == TAKE_FLOWER_BOTTOM) {
this->go(2000, 1790);
this->go(1700, 1300);
awaitRobotIdle();
this->rotate(PI/2);
this->setSpeed(180);
this->rotate(-PI);
awaitRobotIdle();
} else {
return;
@@ -914,22 +916,20 @@ void TCPServer::findAndGoFlower(const StratPattern sp) {
return;
}
this->arucoTags.clear();
std::optional<ArucoTag> tag = std::nullopt;
usleep(200'000);
for (int i = 0; i < 4; i++) {
this->broadcastMessage("strat;aruco;get aruco;1\n");
usleep(110'000);
}
tag = getMostCenteredArucoTag(100, 800, -300, 300);
tag = getMostCenteredArucoTag(100, 800, -200, 200);
int timeout = 0;
while (!tag.has_value()) {
this->broadcastMessage("strat;aruco;get aruco;1\n");
usleep(110'000);
tag = getMostCenteredArucoTag(100, 800, -300, 300);
tag = getMostCenteredArucoTag(100, 800, -200, 200);
timeout++;
if (timeout > 3) {