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

distance to tag

Browse Source
This commit is contained in:
ackimixs
2024-04-29 16:51:37 +02:00
parent 6f481a9247
commit f82cf53358
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TCPServer.cpp
View File

@@ -673,12 +673,12 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
double yPrime = arucoTag.pos()[1];
double roll = arucoTag.rot()[1];
auto centerPlantX = (20 * std::cos(roll)) + xPrime - 50;
auto centerPlantX = (20 * std::cos(roll)) + xPrime;
auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage;
double thetaPrime = std::atan2(centerPlantY, centerPlantX);
this->rotate(this->robotPose.theta + rotate - thetaPrime);
this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime);
awaitRobotIdle();
double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX;