modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-03-30 04:19:46 +02:00
Code Issues Packages Projects Releases Wiki Activity

Stop 10% before pot

Browse Source
This commit is contained in:
ackimixs
2024-04-11 16:42:18 +02:00
parent 54a9b5b8a9
commit 0f793a4cb6
1 changed files with 14 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
TCPServer.cpp
View File

@@ -335,10 +335,10 @@ void TCPServer::startGame() {
}
void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) {
float robotPosX = this->robotPose.pos.x;
float robotPosY = this->robotPose.pos.y;
float theta = this->robotPose.theta;
float decalage;
double robotPosX = this->robotPose.pos.x;
double robotPosY = this->robotPose.pos.y;
double theta = this->robotPose.theta;
double decalage;
if (pince < 0 || pince > 2) {
return;
}
@@ -366,10 +366,16 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) {
double xPrime = arucoTagPos.pos.first[0] - 95;
double yPrime = arucoTagPos.pos.first[1];
double x10Percent = xPrime * 0.1;
double decalage10Percent = decalage * 0.1;
xPrime -= x10Percent;
decalage -= decalage10Percent;
std::cout << "Aruco position1 " << xPrime << " " << yPrime << std::endl;
double posV200X = (xPrime * std::cos(theta) + (yPrime - (3 * decalage / 4)) * std::sin(theta)) + robotPosX;
double posV200Y = (-xPrime * std::sin(theta) + (yPrime - (3 * decalage / 4)) * std::cos(theta)) + robotPosY;
double posV200X = (xPrime * std::cos(theta) + (yPrime - decalage) * std::sin(theta)) + robotPosX;
double posV200Y = (-xPrime * std::sin(theta) + (yPrime - decalage) * std::cos(theta)) + robotPosY;
toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(posV200X)) + "," + std::to_string(static_cast<int>(posV200Y)) + "\n";
this->broadcastMessage(toSend);
@@ -377,7 +383,8 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) {
this->broadcastMessage("strat;arduino;speed;150\n");
usleep(500'000);
xPrime += 90;
xPrime += x10Percent;
decalage += decalage10Percent;
double robotPosForPotX = (xPrime * std::cos(theta) + (yPrime - decalage) * std::sin(theta)) + robotPosX;
double robotPosForPotY = (-xPrime * std::sin(theta) + (yPrime - decalage) * std::cos(theta)) + robotPosY;