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

angle

Browse Source
This commit is contained in:
ackimixs
2024-04-16 13:16:51 +02:00
parent eec4a9105d
commit 014b049cf2
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
TCPServer.cpp
View File

@@ -480,16 +480,16 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
switch (pince) {
case 0:
decalage = -30;
rotate = 0.15;
decalage = 60;
rotate = -0.3;
break;
case 1:
decalage = 0;
rotate = 0;
break;
case 2:
decalage = 30;
rotate = -0.15;
decalage = -60;
rotate = 0.3;
break;
default:
decalage = 0;
@@ -503,11 +503,11 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
this->broadcastMessage(toSend);
double xPrime = arucoTag.pos().first[0] + 30/* - 5*/ + (decalage * 0.2);
double yPrime = arucoTag.pos().first[1] - decalage;
double yPrime = arucoTag.pos().first[1] + decalage;
double thetaPrime = std::atan2(yPrime, xPrime);
double thetaPrime = std::atan2(-yPrime, xPrime);
toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate - thetaPrime) * 100)) + "\n";
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) {