modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-03-25 17:10:41 +01:00
Code Issues Packages Projects Releases Wiki Activity

transit mode

Browse Source
This commit is contained in:
ackimixs
2024-04-17 20:02:10 +02:00
parent 91b73c3256
commit d7f4a16d65
1 changed files with 2 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
TCPServer.cpp
View File

@@ -453,7 +453,7 @@ void TCPServer::startGameBlueTeam() {
}
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y + 400)) + "\n";
this->broadcastMessage(toSend);
this->broadcastMessage("strat;arduino;go;1000,");
awaitRobotIdle();
this->broadcastMessage("strat;arduino;angle;157\n");
@@ -1274,29 +1274,10 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
this->broadcastMessage(toSend);
awaitRobotIdle();
double x15Percent = xPrime * 0.15;
double y15Percent = yPrime * 0.15;
xPrime -= x15Percent;
yPrime -= y15Percent;
double pos30PercentX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX;
double pos30PercentY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY;
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(pos30PercentX)) + "," + std::to_string(static_cast<int>(pos30PercentY)) + "\n";
this->broadcastMessage(toSend);
awaitRobotIdle();
this->broadcastMessage("strat;arduino;speed;150\n");
xPrime += x15Percent;
yPrime += y15Percent;
double robotPosForPotX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX;
double robotPosForPotY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY;
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(robotPosForPotX)) + "," + std::to_string(static_cast<int>(robotPosForPotY)) + "\n";
toSend = "strat;arduino;transit;" + std::to_string(static_cast<int>(robotPosForPotX)) + "," + std::to_string(static_cast<int>(robotPosForPotY)) + "\n";
this->broadcastMessage(toSend);
awaitRobotIdle();