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

awaitRobotIdle

Browse Source
This commit is contained in:
ackimixs
2024-05-10 02:02:43 +02:00
parent b3fe74a2a4
commit ae14d4b7ee
2 changed files with 78 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

147
TCPServer.cpp
View File

@@ -531,11 +531,11 @@ void TCPServer::startGame() {
break; break;
case ROTATE_0: case ROTATE_0:
this->rotate(0); this->rotate(0);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
break; break;
case ROTATE_270: case ROTATE_270:
this->rotate(-PI/2); this->rotate(-PI/2);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
break; break;
} }
@@ -589,7 +589,7 @@ void TCPServer::startGameTest() {
// pi/4 // pi/4
this->broadcastMessage("strat;arduino;angle;314\n"); this->broadcastMessage("strat;arduino;angle;314\n");
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
// ReSharper disable once CppDFAUnreachableCode // ReSharper disable once CppDFAUnreachableCode
@@ -631,7 +631,7 @@ void TCPServer::startGameTest() {
} }
this->broadcastMessage("strat;arduino;angle;157\n"); this->broadcastMessage("strat;arduino;angle;157\n");
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
// this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); // this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
@@ -677,10 +677,10 @@ void TCPServer::startGameTest() {
std::string toSend = "strat;arduino;go;762,300\n"; std::string toSend = "strat;arduino;go;762,300\n";
this->broadcastMessage(toSend); this->broadcastMessage(toSend);
usleep(200'000); usleep(200'000);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->broadcastMessage("strat;arduino;angle;157\n"); this->broadcastMessage("strat;arduino;angle;157\n");
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage("strat;arduino;speed;150\n");
this->broadcastMessage("strat;arduino;go;762,0\n"); this->broadcastMessage("strat;arduino;go;762,0\n");
@@ -702,11 +702,11 @@ void TCPServer::startGameTest() {
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->endRobotPose.pos.y)) + "\n"; toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->endRobotPose.pos.y)) + "\n";
this->broadcastMessage(toSend); this->broadcastMessage(toSend);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>(this->endRobotPose.theta * 100)) + "\n"; toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>(this->endRobotPose.theta * 100)) + "\n";
this->broadcastMessage(toSend); this->broadcastMessage(toSend);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
// toSend = "start;arduino;angle;" + std::to_string(this->endRobotPose.theta * 100) + "\n"; // toSend = "start;arduino;angle;" + std::to_string(this->endRobotPose.theta * 100) + "\n";
// this->broadcastMessage(toSend); // this->broadcastMessage(toSend);
@@ -756,14 +756,14 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
this->setSpeed(200); this->setSpeed(200);
this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX; double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX;
double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY; double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY;
this->transit(robotPosForPotX, robotPosForPotY, 130); this->transit(robotPosForPotX, robotPosForPotY, 130);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->closePince(pince); this->closePince(pince);
usleep(500'000); usleep(500'000);
@@ -790,7 +790,7 @@ void TCPServer::askArduinoPos() {
} }
} }
void TCPServer::awaitRobotIdle() { int TCPServer::awaitRobotIdle() {
isRobotIdle = 0; isRobotIdle = 0;
int timeout = 0; int timeout = 0;
// ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAConstantConditions
@@ -807,12 +807,19 @@ void TCPServer::awaitRobotIdle() {
this->broadcastMessage(lastArduinoCommand); this->broadcastMessage(lastArduinoCommand);
awaitRobotIdle(); awaitRobotIdle();
} }
if (gameStarted) {
auto time = std::chrono::system_clock::now();
if (time - gameStart > std::chrono::seconds(87)) {
return -1;
}
}
timeout++; timeout++;
if (timeout > 80) { if (timeout > 80) {
this->broadcastMessage("strat;arduino;clear;1"); this->broadcastMessage("strat;arduino;clear;1");
break; return -2;
} }
} }
return 0;
} }
void TCPServer::handleArucoTag(const ArucoTag &tag) { void TCPServer::handleArucoTag(const ArucoTag &tag) {
@@ -939,7 +946,7 @@ void TCPServer::handleEmergency(int distance, double angle) {
double newY = this->robotPose.pos.y + 200 * std::sin(newAngle); double newY = this->robotPose.pos.y + 200 * std::sin(newAngle);
usleep(200'000); usleep(200'000);
this->go(newX, newY); this->go(newX, newY);
awaitRobotIdle();*/ if (awaitRobotIdle() < 0) return;*/
/*} /*}
this->broadcastMessage("strat;arduino;clear;4\n"); this->broadcastMessage("strat;arduino;clear;4\n");
@@ -998,14 +1005,14 @@ void TCPServer::goEnd() {
for (const auto& checkpoint : checkponts) { for (const auto& checkpoint : checkponts) {
this->go(checkpoint); this->go(checkpoint);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
} }
this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setSpeed(180); this->setSpeed(180);
this->rotate(this->endRobotPose.theta); this->rotate(this->endRobotPose.theta);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->baisserBras(); this->baisserBras();
@@ -1023,34 +1030,34 @@ void TCPServer::findAndGoFlower(const StratPattern sp) {
if (team == BLUE) { if (team == BLUE) {
if (sp == TAKE_FLOWER_TOP) { if (sp == TAKE_FLOWER_TOP) {
this->go(500, 700); this->go(500, 700);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(0); this->rotate(0);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
} }
else if (sp == TAKE_FLOWER_BOTTOM) { else if (sp == TAKE_FLOWER_BOTTOM) {
this->go(500, 1300); this->go(500, 1300);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(0); this->rotate(0);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
} else { } else {
return; return;
} }
} else if (team == YELLOW) { } else if (team == YELLOW) {
if (sp == TAKE_FLOWER_TOP) { if (sp == TAKE_FLOWER_TOP) {
this->go(1500, 700); this->go(1500, 700);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(-PI); this->rotate(-PI);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
} }
else if (sp == TAKE_FLOWER_BOTTOM) { else if (sp == TAKE_FLOWER_BOTTOM) {
this->go(1500, 1300); this->go(1500, 1300);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(-PI); this->rotate(-PI);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
} else { } else {
return; return;
} }
@@ -1123,12 +1130,12 @@ void TCPServer::dropPurpleFlowers() {
if (!pinceHavePurpleFlower.empty()) { if (!pinceHavePurpleFlower.empty()) {
this->go(purpleDropPosition); this->go(purpleDropPosition);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setSpeed(150); this->setSpeed(150);
this->rotate(PI / 2); this->rotate(PI / 2);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->baisserBras(); this->baisserBras();
@@ -1137,10 +1144,10 @@ void TCPServer::dropPurpleFlowers() {
usleep(200'000); usleep(200'000);
this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150); this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
pinceState[toDrop] = NONE; pinceState[toDrop] = NONE;
this->closePince(toDrop); this->closePince(toDrop);
@@ -1205,11 +1212,11 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) {
this->setSpeed(200); this->setSpeed(200);
this->go(whiteDropSetup); this->go(whiteDropSetup);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(angle); this->rotate(angle);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->leverBras(); this->leverBras();
usleep(500'000); usleep(500'000);
@@ -1240,7 +1247,7 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) {
this->setSpeed(200); this->setSpeed(200);
this->go(whiteDropSetup); this->go(whiteDropSetup);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
this->closePince(i); this->closePince(i);
@@ -1255,10 +1262,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
switch (sp) { switch (sp) {
case TURN_SOLAR_PANNEL_1: case TURN_SOLAR_PANNEL_1:
this->go(250, 1800); this->go(250, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(0); this->rotate(0);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->checkPanneau(7); this->checkPanneau(7);
usleep(300'000); usleep(300'000);
@@ -1266,10 +1273,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
break; break;
case TURN_SOLAR_PANNEL_2: case TURN_SOLAR_PANNEL_2:
this->go(460, 1800); this->go(460, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(0); this->rotate(0);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->checkPanneau(7); this->checkPanneau(7);
usleep(300'000); usleep(300'000);
@@ -1277,10 +1284,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
break; break;
case TURN_SOLAR_PANNEL_3: case TURN_SOLAR_PANNEL_3:
this->go(690, 1800); this->go(690, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(0); this->rotate(0);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->checkPanneau(7); this->checkPanneau(7);
usleep(300'000); usleep(300'000);
@@ -1293,10 +1300,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
switch (sp) { switch (sp) {
case TURN_SOLAR_PANNEL_1: case TURN_SOLAR_PANNEL_1:
this->go(2750, 1800); this->go(2750, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(PI); this->rotate(PI);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->checkPanneau(6); this->checkPanneau(6);
usleep(300'000); usleep(300'000);
@@ -1304,10 +1311,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
break; break;
case TURN_SOLAR_PANNEL_2: case TURN_SOLAR_PANNEL_2:
this->go(2540, 1800); this->go(2540, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(PI); this->rotate(PI);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->checkPanneau(6); this->checkPanneau(6);
usleep(300'000); usleep(300'000);
@@ -1315,10 +1322,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
break; break;
case TURN_SOLAR_PANNEL_3: case TURN_SOLAR_PANNEL_3:
this->go(2310, 1800); this->go(2310, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(PI); this->rotate(PI);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->checkPanneau(6); this->checkPanneau(6);
usleep(300'000); usleep(300'000);
@@ -1362,10 +1369,10 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) {
this->setMaxSpeed(); this->setMaxSpeed();
this->transit(whiteDropSetup, 170); this->transit(whiteDropSetup, 170);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(angle); this->rotate(angle);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->leverBras(); this->leverBras();
@@ -1407,7 +1414,7 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) {
this->setMaxSpeed(); this->setMaxSpeed();
this->go(whiteDropSetup); this->go(whiteDropSetup);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->transportBras(); this->transportBras();
} }
@@ -1453,12 +1460,12 @@ void TCPServer::dropBaseFlowers(StratPattern sp) {
this->setMaxSpeed(); this->setMaxSpeed();
this->go(dropPosition); this->go(dropPosition);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setSpeed(170); this->setSpeed(170);
this->rotate(angle); this->rotate(angle);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setMaxSpeed(); this->setMaxSpeed();
@@ -1469,10 +1476,10 @@ void TCPServer::dropBaseFlowers(StratPattern sp) {
} }
this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance); this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance); this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
bool detectedPurple = false; bool detectedPurple = false;
@@ -1547,20 +1554,20 @@ void TCPServer::go3Plants(const StratPattern sp) {
this->transit(plantPosition[0]-(600*direction), plantPosition[1], 170); this->transit(plantPosition[0]-(600*direction), plantPosition[1], 170);
// this->go(plantPosition[0]-500, plantPosition[1]); // this->go(plantPosition[0]-500, plantPosition[1]);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->baisserBras(); this->baisserBras();
this->setMaxSpeed(); this->setMaxSpeed();
this->rotate(angle); this->rotate(angle);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150); this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->rotate(angle); this->rotate(angle);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setMaxSpeed(); this->setMaxSpeed();
@@ -1580,7 +1587,7 @@ void TCPServer::go3Plants(const StratPattern sp) {
usleep(200'000); usleep(200'000);
this->transit(plantPosition[0], this->robotPose.pos.y, 130); this->transit(plantPosition[0], this->robotPose.pos.y, 130);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setMaxSpeed(); this->setMaxSpeed();
@@ -1593,12 +1600,12 @@ void TCPServer::go3Plants(const StratPattern sp) {
usleep(500'000); usleep(500'000);
this->rotate(angle); this->rotate(angle);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setSpeed(160); this->setSpeed(160);
this->go(this->robotPose.pos.x - (200 * direction), this->robotPose.pos.y); this->go(this->robotPose.pos.x - (200 * direction), this->robotPose.pos.y);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
this->openPince(i); this->openPince(i);
@@ -1607,7 +1614,7 @@ void TCPServer::go3Plants(const StratPattern sp) {
this->setSpeed(150); this->setSpeed(150);
this->go(this->robotPose.pos.x + (75 * direction), this->robotPose.pos.y); this->go(this->robotPose.pos.x + (75 * direction), this->robotPose.pos.y);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
this->closePince(i); this->closePince(i);
@@ -1622,26 +1629,26 @@ void TCPServer::removePot(StratPattern sp) {
if (team == BLUE) { if (team == BLUE) {
if (sp == REMOVE_POT_J2) { if (sp == REMOVE_POT_J2) {
this->transit(250, 1100, 150); this->transit(250, 1100, 150);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setMaxSpeed(); this->setMaxSpeed();
this->go(210, 900); this->go(210, 900);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(200, 400); this->go(200, 400);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(250, 650); this->go(250, 650);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
} }
} else if (team == YELLOW) { } else if (team == YELLOW) {
if (sp == REMOVE_POT_J2) { if (sp == REMOVE_POT_J2) {
this->transit(2750, 1100, 150); this->transit(2750, 1100, 150);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->setMaxSpeed(); this->setMaxSpeed();
this->go(2780, 900); this->go(2780, 900);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(2790, 400); this->go(2790, 400);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(2750, 650); this->go(2750, 650);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
} }
} }
} }
@@ -1685,11 +1692,11 @@ void TCPServer::checkpoint(const StratPattern sp) {
switch (sp) { switch (sp) {
case CHECKPOINT_MIDDLE: case CHECKPOINT_MIDDLE:
this->go(500, 1500); this->go(500, 1500);
this->awaitRobotIdle(); this->if (awaitRobotIdle() < 0) return;
break; break;
case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER:
this->go(800, 1800); this->go(800, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(500, 1700); this->go(500, 1700);
usleep(500'000); usleep(500'000);
break; break;
@@ -1700,11 +1707,11 @@ void TCPServer::checkpoint(const StratPattern sp) {
switch (sp) { switch (sp) {
case CHECKPOINT_MIDDLE: case CHECKPOINT_MIDDLE:
this->go(2500, 1500); this->go(2500, 1500);
this->awaitRobotIdle(); this->if (awaitRobotIdle() < 0) return;
break; break;
case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER:
this->go(2200, 1800); this->go(2200, 1800);
awaitRobotIdle(); if (awaitRobotIdle() < 0) return;
this->go(2500, 1700); this->go(2500, 1700);
usleep(500'000); usleep(500'000);
break; break;

2
TCPServer.h
View File

@@ -216,7 +216,7 @@ public:
[[nodiscard]] bool shouldStop() const; [[nodiscard]] bool shouldStop() const;
void awaitRobotIdle(); int awaitRobotIdle();
void handleArucoTag(const ArucoTag &tag); void handleArucoTag(const ArucoTag &tag);