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

angle

Browse Source
This commit is contained in:
ackimixs
2024-04-16 13:31:59 +02:00
parent 8f8ca6570c
commit 947bcd2b72
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
TCPServer.cpp
View File

@@ -502,12 +502,15 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
this->broadcastMessage(toSend);
double xPrime = arucoTag.pos().first[0] + 30/* - 5*/ + (decalage * 0.2);
double xPrime = arucoTag.pos().first[0];
double yPrime = arucoTag.pos().first[1] + decalage;
double thetaPrime = std::atan2(-yPrime, xPrime);
xPrime *= 1.03;
yPrime *= 1.03;
toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate + thetaPrime) * 100)) + "\n";
double thetaPrime = std::atan2(yPrime, xPrime);
toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate - thetaPrime) * 100)) + "\n";
this->broadcastMessage(toSend);
isRobotMoving = 0;
while (this->isRobotMoving < 3) {