Something went wrong. Try again.
[READ-ONLY] Mirror of https://github.com/Schniz/robotikka. Lee Ve Tom Ve Ani Doin our Robottttt
Something went wrong. Try again.
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175#include "Manager.h"
#include <libplayercore/playercommon.h>#include <cmath>#include <iostream>#include <vector>
#include "Consts.h"#include "Managers/ConfigurationManager.h"//#include "Managers/WaypointsManager.h"#include "Models/Location.h"//#include "Models/Map.h"#include "Plans/Plan.h"#include "Utils/AStarUtil.h"#include "Utils/MathUtil.h"
#define POSITION_INACCURACY 3
using namespace std;using namespace Utils;
Manager::Manager(Robot* robot, Plan* pln) { this->robot = robot; this->currentPoint = pln->getStartPoint(); this->plan = pln; this->InitApp();}
double circle(double num) { return fmod(num + 4 * 360, 360.0);}
double calRawYaw(Cell* waypoint, Location* bestLocation) { double deltaY = abs(waypoint->getY() - bestLocation->getY()); double deltaX = abs(waypoint->getX() - bestLocation->getX()); double rawYaw = RTOD(atan(deltaY / deltaX)); return rawYaw;}
double getAngleToBe(Cell* waypoint, Location* bestLocation) { double rawYaw = calRawYaw(waypoint, bestLocation); double realYaw = rawYaw;
if (bestLocation->getY() > waypoint->getY()) { if (bestLocation->getX() < waypoint->getX()) { // 1st realYaw = rawYaw; } else { // 2nd realYaw = 180 - rawYaw; } } else { if (bestLocation->getX() < waypoint->getX()) { // 4rd realYaw = 360 - rawYaw; } else { // 3th realYaw = 180 + rawYaw; } }
return circle(realYaw);}
void Manager::rotateLikeShawarma(Cell* waypoint, Location* bestLocation) { double angleToBe = getAngleToBe(waypoint, bestLocation); cout << "angleToBe (radians): " << angleToBe << endl; double yaw = bestLocation->getYaw(); double angleDifference = circle(angleToBe - yaw); while (angleDifference > 5) { int factor = angleDifference >= 180 ? -1 : 1; robot->setSpeed(0, factor * Consts::TURN_ANGULAR_SPEED); robot->Read(); yaw = this->getBestLocation(waypoint).getYaw(); angleDifference = circle(angleToBe - yaw); }}
Location Manager::getBestLocation(Cell* waypoint) { double dx, dy, dyaw; float* allLasers = robot->getAllLasers(); robot->calcLocationDeltas(dx, dy, dyaw, map); this->localizationManager->update((float) dx, (float) dy, (float) dyaw, allLasers, waypoint); Location bestLocation = this->localizationManager->BestLocation(); delete[] allLasers; return Location(bestLocation.getX(), map->gridHeight - bestLocation.getY(), bestLocation.getYaw());}
void Manager::run() { double dx = this->start->getX(); double dy = this->start->getY(); double dyaw = ConfigurationManager::GetInstance()->getStartLocation().getYaw(); Location bestLocation = Location(dx, dy, dyaw);
// For every waypoint... for (Cell* waypoint : waypointsManager->smoothWaypoints) { cout << "Starting to walk to " << waypoint->getX() << "," << waypoint->getY() << endl;
this->waypointsManager->currWaypoint = waypoint;
bestLocation = this->getBestLocation(waypoint);
// Go to the waypoint.. robot->Read();
cout << "LOCATION: " << bestLocation.getX() << "," << bestLocation.getY() << endl;
this->rotateLikeShawarma(waypoint, &bestLocation); this->robot->setSpeed(0.5, 0); while (!waypointsManager->IsInWaypoint(bestLocation.getX(), bestLocation.getY())) { robot->Read(); bestLocation = this->getBestLocation(waypoint); }
this->robot->setSpeed(0, 0);
}
cout << "finished" << endl;}
Cell* locationToMapCell(AnotherMap* map, Location location) { return map->getResizedCellFromImageCoords(location.getX(), location.getY());}
void Manager::InitApp() { // init configuration manager ConfigurationManager* config = ConfigurationManager::LoadFromFile( "Resources/parameters.txt"); AnotherMap* map = this->map = new AnotherMap(config); AStarUtil astar(map); Location startLocation = config->getStartLocation(); Cell* startCell = this->start = locationToMapCell(map, startLocation); Cell* endCell = this->destination = locationToMapCell(map, config->getEndLocation()); vector<Cell*> astarPath = astar.findPath(startCell, endCell); WaypointsManager* wayPointsManager = this->waypointsManager = new WaypointsManager(astarPath);
Location startLocationPx = startCell->getLocation(); startLocationPx.m_Yaw = startLocation.m_Yaw; double odemX = MathUtil::pxToCm(startLocationPx.getX()) / 100; double odemY = MathUtil::pxToCm(map->gridHeight - startLocationPx.getY()) / 100; this->robot->setOdemetry(odemX, odemY, DTOR(startLocationPx.getYaw())); this->robot->setLocation(startLocationPx.getX(), startLocationPx.getY(), startLocationPx.getYaw()); this->robot->kickstart(); this->localizationManager = new LocalizationManager(startLocationPx, map, wayPointsManager->longestDistance, robot->_lp);
for (Cell* cell : astarPath) { cell->Cell_Cost = CellType::PATH; }
for (Cell* cell : wayPointsManager->smoothWaypoints) { cout << "[" << cell->getX() << "," << cell->getY() << "]" << endl; cell->Cell_Cost = CellType::DESTINATION; }
map->saveToFile("/tmp/waypoints.png", true);
cout << astarPath.size() << "-" << wayPointsManager->smoothWaypoints.size() << " = " << (astarPath.size() - wayPointsManager->smoothWaypoints.size()) << endl;}
Manager::~Manager() { delete this->waypointsManager; delete this->map; delete this->localizationManager;}