modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-01-20 09:57:20 +01:00
Code Issues Packages Projects Releases Wiki Activity

change aruco things

Browse Source
This commit is contained in:
ackimixs
2024-04-15 21:44:39 +02:00
parent 5326b1e043
commit 28253d33ae
1 changed files with 4 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
TCPServer.cpp
View File

@@ -478,12 +478,9 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
this->broadcastMessage(toSend);
double xPrime = arucoTag.pos().first[0] + 25/* - 5*/;
double xPrime = arucoTag.pos().first[0] + 5/* - 5*/;
double yPrime = arucoTag.pos().first[1] - decalage;
// calculate the angle to be in front of the tag
std::cout << "Tag pos relative : " << xPrime << " " << yPrime << " " << this->robotPose.theta << std::endl;
double thetaPrime = std::atan2(yPrime, xPrime);
toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate + thetaPrime) * 100)) + "\n";
@@ -496,10 +493,10 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
usleep(500'000);
double x30Percent = xPrime * 0.3;
double decalage30Percent = decalage * 0.3;
double y30Percent = yPrime * 0.3;
xPrime -= x30Percent;
yPrime -= decalage30Percent;
yPrime -= y30Percent;
double pos30PercentX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX;
double pos30PercentY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY;
@@ -519,7 +516,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
usleep(1'000'000);
xPrime += x30Percent;
yPrime += decalage30Percent;
yPrime += y30Percent;
double robotPosForPotX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX;
double robotPosForPotY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY;