void Robot::drive()
{
Logger::log( __PRETTY_FUNCTION__);
try
{
for (std::shared_ptr< AbstractSensor > sensor : sensors)
{
//sensor->setOn();
}
if (speed == 0.0)
{
speed = 10.0;
}
GoalPtr goal = RobotWorld::getRobotWorld().getGoalByName( "Leon");
calculateRoute(goal);
unsigned pathPoint = 0;
while (position.x > 0 && position.x < 500 && position.y > 0 && position.y < 500 && pathPoint < path.size())
{
const Vertex& vertex = path[pathPoint+=speed];
front = Vector( vertex.asPoint(), position);
position.x = vertex.x;
position.y = vertex.y;
if (arrived(goal) || collision())
{
notifyObservers();
break;
}
notifyObservers();
std::this_thread::sleep_for( std::chrono::milliseconds( 100));
// this should be either the last call in the loop or
// part of the while.
if(stop == true)
{
return;
}
} // while
for (std::shared_ptr< AbstractSensor > sensor : sensors)
{
//sensor->setOff();
}
}
catch (std::exception& e)
{
std::cerr << __PRETTY_FUNCTION__ << ": " << e.what() << std::endl;
}
catch (...)
{
std::cerr << __PRETTY_FUNCTION__ << ": unknown exception" << std::endl;
}
}
// Constructor with destination
Car::Car(sf::Texture& texture, Tile* startPosition, Tile* destination) {
mMaxSpeed = 0.12f;
mBlocked = false;
mFinished = true;
mGreenlit = false;
mCurrentIntersection = NULL;
// Set texture
mSprite.setTexture(texture);
// Set position/rotation origin
mSprite.setOrigin(5.0f, 5.0f);
// Set start position
setStartPosition(startPosition);
// Calculate and start route
calculateRoute(destination);
startRoute();
}
/**
* Main robot function that controls the behaviour of the robot.
* Needs a connected socket to communicate with the simulator or HW
*/
void robot(int sck, FILE *sckfd)
{
int val1;
int curX = 2;
int curY = 0;
int direction = NORTH;
initRobot(sck, 100, curX, curY);
//initGrid();
int nummoves = 0;
// Challenge A/B
int destX = 5, destY = 5;
FILE *file;
file = fopen("log.txt", "w");
// Loop until destination is reached
while (1) {
// system("cls");
int nextX, nextY, dir, rotation;
calculateRoute(curX, curY, destX, destY);
d_printGrid(curX, curY);
if (!nodes[curX][curY]->next) {
fprintf(stderr, "No route found\n");
sendExit(sck);
return;
}
nextX = nodes[curX][curY]->next->pos.x;
nextY = nodes[curX][curY]->next->pos.y;
dir = findDirection(curX, curY, nextX, nextY);
rotation = dir - direction;
if (rotation > 2)
rotation -= 4;
if (rotation < -2)
rotation += 4;
fprintf(stderr, "Next (%d,%d), rotation: %d\n", nextX, nextY, dir);
if (rotation < 0) { //turn left
fprintf(file, "Turn left %d\r\n", abs(rotation));
setCommand(sck, TURN, 1, abs(rotation), 0);
} else if (rotation > 0) {//turn right
fprintf(file, "Turn right %d\r\n", abs(rotation));
setCommand(sck, TURN, 2, rotation, 0);
}
fflush(file);
// wait until robot is ready with turning
if (rotation != 0) {
while (1) {
getCommand(sck, sckfd, READY, 1, &val1, NULL);
if (val1 == 1)
break;
}
fprintf(file, "Ready!\r\n");
}
// Update direction
direction = dir;
if (curY == 0 || curY == 6 || curX == 0 || curX == 6)
nummoves = 1;
else
nummoves = 2;
printf("Cur (%d,%d) Nummoves: %d\n", curX, curY, nummoves);
printf("Move forward\n");
fprintf(file, "Move forward %d\r\n", nummoves);
fflush(file);
setCommand(sck, MOVE, 1, nummoves, 0);
if (nummoves == 1) {
// Wait while not ready
while (1) {
getCommand(sck, sckfd, READY, 1, &val1, NULL);
if (val1 == 1) {
break;
}
}
curX = nextX;
curY = nextY;
} else if (nummoves == 2) {
// Wait while not ready
while (1) {
getCommand(sck, sckfd, READY, 1, &val1, NULL);
if (val1 == 1) {
break;
}
}
// Check for mine
printf("Check for mine\n");
getCommand(sck, sckfd, MINE, 1, &val1, NULL);
if (val1 == 1) {
//removeConnection(x1, y1, x2, y2);
// Clear mine flag
setCommand(sck, MINE, 1, 0, 0);
fprintf(file, "Mine found!\r\n");
printf("Mine found!\n");
// Get back!:D
//return 0;
} else {
printf("No mine found!\n");
}
}
}
fclose(file);
sendExit(sck);
}
