/** solveMazeBest **********************************
* Follows the left side of a maze until reaching the target while
* recording the route taken while simplifying.
*
* @params route -- an array that is filled with the route taken
* maxLen -- the maximum number of intersections that can be recorded
*
* @returns -- the number of items in route
*/
int solveMazeBest(char route[], int maxLen) {
int index = 0;
// passed to follow segment to record if a left, right or straight intersection happened
bool left = true;
bool right = true;
bool straight = true;
// keep going while there is a turn or we can go straight and we are not on the target
while (true) {
// just follow the line until something interesting happens
followSegment(left, right, straight);
// if we are finished the maze we break out of the loop
if (done) {
break;
}
// stop when we reach max turns
if (index == maxLen) {
break;
}
// left turn found, record and make the turn
if (left) {
route[index] = turn_left;
index++;
turnLeft();
}
// no left but a straight, record and move ahead a bit to get away from the intersection
else if (straight) {
route[index] = straight;
set_motors(motor_speed, motor_speed);
delay_ms(200);
set_motors(0,0);
index++;
}
// right turn, record and make the turn
else if (right) {
route[index] = turn_right;
index++;
turnRight();
}
// dead end do a 180
else {
route[index] = turn_dead_end;
index++;
// move forward a fair bit before making the turn
// this fixes a problem with a dead end being found shortly after
// making a turn
set_motors(motor_speed, motor_speed);
delay_ms(425);
// make a turn
set_motors(0,0);
makeTurn(false);
}
// remove any dead ends if found
index = trimRoute(route, index);
}
// move forward a tiny bit at the end and stop
set_motors(motor_speed, motor_speed);
delay_ms(200);
set_motors(0,0);
return index;
}
/** turnRight *******************************************
* turns the robot to the right
*/
void turnRight() {
gotoEnd();
makeTurn(false);
}
/** turnLeft *******************************************
* turns the robot to the left
*/
void turnLeft() {
gotoEnd();
makeTurn(true);
}
int makeMove(int x1, int y1, int x2, int y2)
{
int val1;
printf("Making move from (%d,%d) to (%d,%d)\n", x1, y1, x2, y2);
setCommand(new_sck, MINE, 1, 0, 0);
// getchar();
// Dafuq why would you call me?!
if(x1==x2 && y1==y2)
return 1;
// (if needed) Turn and make the move
int dir = findDirection(x1, y1, x2, y2);
makeTurn(curDir, dir, x1, y1);
// update direction
curDir = dir;
printf("Dir set to %d\n", dir);
// getchar();
// Make 2 moves
setCommand(new_sck, MOVE, 1, 2, 0);
wait_till_ready(new_sck);
printf("Check for mine\n");
getCommand(new_sck, sckfd, MINE, 1, &val1, NULL);
// A mine was found, we're not on our destination
if (val1 == 1) {
//wait_till_ready(new_sck);
printf("Mine found!\n");
// getchar();
// Move in reverse
setCommand(new_sck, MOVE, 1, 4, 0);
wait_till_ready(new_sck);
setCommand(new_sck, MINE, 1, 0, 0);
dir=curDir;
return 0;
}
// We're on the destination
else {
printf("No mine found!\n");
// Check wether we are heading to our final destination
if(isPlaceToVisit(x2, y2)) {
// Turn into exit
#ifdef SMART_VISIT
if (x2 == 4){
if (curDir != EAST) {
makeTurn(curDir, EAST, x1, y1);
curDir = EAST;
}
}else if(x2 == 0){
if (curDir != WEST) {
makeTurn(curDir, WEST, x1, y1);
curDir = WEST;
}
}else if(y2 == 0){
if (curDir != SOUTH) {
makeTurn(curDir, SOUTH, x1, y1);
curDir = SOUTH;
}
}else if(y2 == 4){
if (curDir != NORTH) {
makeTurn(curDir, NORTH, x1, y1);
curDir = NORTH;
}
}
#else
if (x2 == 4){
makeTurn(curDir, EAST, x1, y1);
curDir = WEST;
}else if(x2 == 0){
makeTurn(curDir, WEST, x1, y1);
curDir = EAST;
}else if(y2 == 0){
makeTurn(curDir, SOUTH, x1, y1);
curDir = NORTH;
}else if(y2 == 4){
makeTurn(curDir, NORTH, x1, y1);
curDir = SOUTH;
}
// Move until white is seen by sensor
setCommand(new_sck, MOVE, 1, 3, 0);
wait_till_ready(new_sck);
// Draai 180 graden
setCommand(new_sck, TURN, 2, 1, 0);
wait_till_ready(new_sck);
// Ga weer terug naar kruispunt
setCommand(new_sck, MOVE, 1, 1, 0);
wait_till_ready(new_sck);
#endif
}
return 1;
}
return 0;
}
