diff --git a/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp b/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp index 0232b9c..ab2c6cd 100644 --- a/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp +++ b/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp @@ -34,12 +34,58 @@ void CustomPlayer::update() { } void CustomPlayer::exec() { - if (!field || !frame || !robot) { + + if (!field || !frame || !robot) + return; + if (!frame->has_ball()) return; + auto&& pos_ball = frame->ball(); + + QPointF targetPosition(1000, 4000); + QPointF initialPosition(2000, 0); + int tolerance = 150; + maxdis = 9000000; + if ((pos_ball.distTo(targetPosition)) > tolerance) { + + for (const auto& ally : frame->allies()) { + double distance = ally.distTo(pos_ball); + if (maxdis > distance) { + maxdis = distance; + idrobot = ally.id(); + } + } } - // TODO: here... - // emit sendCommand(...); + if (robot->id() != idrobot) + return; + + if ((pos_ball - targetPosition).manhattanLength() > tolerance) { + if (robot->distTo(pos_ball) > 150) { + SSLMotion::GoToPoint goToPoint(pos_ball, (pos_ball - robot->position()).angle(), true); + auto command = sslNavigation.run(robot.value(), SSLRobotCommand(goToPoint)); + command.set_dribbler(true); + command.set_dribblerVelocity(4); + emit sendCommand(command); + } else { + SSLMotion::GoToPoint goToPoint(targetPosition, + (targetPosition - robot->position()).angle(), + true); + goToPoint.set_maxVelocity(1); + auto command = sslNavigation.run(robot.value(), SSLRobotCommand(goToPoint)); + command.set_dribbler(true); + command.set_dribblerVelocity(4); + emit sendCommand(command); + } + } else if (robot->id() == idrobot && robot->distTo(initialPosition) > tolerance) { + SSLMotion::GoToPoint goToInitial(initialPosition, + (initialPosition - robot->position()).angle(), + true); + auto command = sslNavigation.run(robot.value(), SSLRobotCommand(goToInitial)); + emit sendCommand(command); + } else { + idrobot = -1; + maxdis = 9000000; + } } void CustomPlayer::receiveField(const Field& field) { diff --git a/src/Modules/Processing/CustomPlayer/CustomPlayer.h b/src/Modules/Processing/CustomPlayer/CustomPlayer.h index 3aff99e..c7a12f7 100644 --- a/src/Modules/Processing/CustomPlayer/CustomPlayer.h +++ b/src/Modules/Processing/CustomPlayer/CustomPlayer.h @@ -5,6 +5,9 @@ #include "Modules/Processing/ProcessingUtils/ProcessingUtils.h" class CustomPlayer : public Processing { + int maxdis = 9000000; + int idrobot = -1; + bool ball_placement = true; public: CustomPlayer(int index, QThreadPool* threadPool);