void second_main_loop(int i)
{
// SECOND MAIN LOOP BODY
follow_segment();
// Drive straight while slowing down, as before.
set_motors(50,50);
delay_ms(50);
set_motors(40,40);
delay_ms(200);
// Make a turn according to the instruction stored in
// path[i].
turn(path[i]);
}
// This function is called once, from main.c.
void maze_solve()
{
// FIRST MAIN LOOP BODY
// (when we find the goal, we use break; to get out of this)
while(!first_main_loop()) {}
set_motors(0,0);
// Simplify the learned path. You can implement this!
// You can either do this at the end, or during the main loop
// delay(1000);
// simplify_path();
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
while(1)
{
// Beep to show that we finished the maze.
// Display battery voltage and wait for button press
while(!button_is_pressed(BUTTON_B))
{
clear();
print("Ready to");
lcd_goto_xy(0,1);
print("go again");
delay_ms(100);
}
initialize();
// Wait for the user to press a button...
int i;
for(i=0;i<path_length;i++)
{
// SECOND MAIN LOOP BODY
second_main_loop(i);
}
// Follow the last segment up to the finish.
follow_segment();
// Now we should be at the finish! Restart the loop.
set_motors(0,0);
}
}
// This function is called once, from main.c.
void maze_solve() {
// Loop until we have record the maze.
while(1) {
// FIRST MAIN LOOP BODY
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
set_motors(50,50);
delay_ms(50);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, while examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
read_line_white(sensors,IR_EMITTERS_ON);
// Check for left and right exits.
if(sensors[0] < 200){
found_left = 1;
}
if(sensors[4] < 200){
found_right = 1;
}
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(40,40);
delay_ms(200);
// Check for a straight exit.
read_line_white(sensors,IR_EMITTERS_ON);
if(sensors[1] < 200 || sensors[2] < 200 || sensors[3] < 200) {
found_straight = 1;
}
// Check for the ending spot.
// If all 3 center sensors are on white, we have solved the maze.
if(sensors[1] < 200 && sensors[2] < 200 && sensors[3] < 200) {
break;
}
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
unsigned char dir = select_turn(found_left, found_straight, found_right);
int recint = record_intersec(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
// Store the intersection in the path variable.
path[path_length] = dir;
intersection[path_length] = recint;
path_length ++;
// You should check to make sure that the path_length does not
// exceed the bounds of the array. We'll ignore that in this
// example.
}
// Maze Recorded!
while(1) {
// Beep to show that we finished the maze.
paths_length=0;
set_motors(0,0);
play(">>a32");
// Wait for the user to press a button.
while(!button_is_pressed(BUTTON_B)) {
clear();
print("Recorded!");
lcd_goto_xy(0,1);
print("Press B");
delay_ms(30);
}
while(button_is_pressed(BUTTON_B)) {
clear();
print("Here we");
lcd_goto_xy(0,1);
print("Go!");
delay_ms(1000);
}
int i=0;
while (i<4) {
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
set_motors(50,50);
delay_ms(50);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, while examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
read_line_white(sensors,IR_EMITTERS_ON);
// Check for left and right exits.
if(sensors[0] < 200) {
found_left = 1;
}
if(sensors[4] < 200) {
found_right = 1;
}
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(40,40);
delay_ms(200);
// Check for a straight exit.
read_line_white(sensors,IR_EMITTERS_ON);
if(sensors[1] < 200 || sensors[2] < 200 || sensors[3] < 200) {
found_straight = 1;
}
unsigned char dir = select_turn(found_left, found_straight, found_right);
int recint = record_intersec(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
// Store the intersection in the path variable.
paths[paths_length] = dir;
intersections[paths_length] = recint;
if (paths[paths_length] != 'B') {
i++;
paths_length ++;
} else {
paths_length ++;
}
}
// Find the robot location with respect to the end.
// The Robot location code can be simplified more.
// Its Possible to modify this code, so we start by running the robot two steps,
// then we check how many values of m we have, if we have more than one value we add another step and so on until we have one m value.
int i2;
int c2;
for (i=0;i<100;i++) {
if (i<(100-paths_length)) {
i2=0;
while (i2<paths_length) {
c2=0;
if (paths[i2]==path[i+i2] && intersections[i2]==intersection[i+i2]) {
c2=1;
i2++;
}
if (c2!=1) {
i2=100;
}
if (i2 == paths_length ) {
m=i+paths_length;
}
}
}
}
// We found the Robot location, now find the shortest way to the end.
for(i=0;i<(100-m);i++) {
pathsol[pathsol_length]=path[i+m];
pathsol_length ++;
simplify_path();
}
// Go to the end.
i=0;
while(i<pathsol_length) {
// Re-run the maze. It's not necessary to identify the
// intersections, so this loop is really simple.
follow_segment();
// Drive straight while slowing down, as before.
set_motors(50,50);
delay_ms(50);
set_motors(40,40);
delay_ms(200);
// Make a turn according to the instruction stored in
// pathsol[i].
turn(pathsol[i]);
i+=1;
unsigned int sensors[5];
read_line_white(sensors,IR_EMITTERS_ON);
// The end is reached.
if(sensors[1] < 200 && sensors[2] < 200 && sensors[3] < 200) {
pathsol_length=0;
}
}
// Now we should be at the finish! Restart the loop.
}
}
// This function is called once, from main.c.
void maze_solve()
{
// Loop until we have solved the maze.
while(1)
{
// FIRST MAIN LOOP BODY
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
set_motors(50,50);
delay_ms(50);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, whil examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
read_line(sensors,IR_EMITTERS_ON);
// Check for left and right exits.
if(sensors[0] > 100)
found_left = 1;
if(sensors[4] > 100)
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(40,40);
delay_ms(200);
// Check for a straight exit.
read_line(sensors,IR_EMITTERS_ON);
if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_straight = 1;
// Check for the ending spot.
// If all three middle sensors are on dark black, we have
// solved the maze.
if(sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600)
break;
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
// Store the intersection in the path variable.
path[path_length] = dir;
path_length ++;
// You should check to make sure that the path_length does not
// exceed the bounds of the array. We'll ignore that in this
// example.
// Simplify the learned path.
simplify_path();
// Display the path on the LCD.
display_path();
}
// Solved the maze!
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
while(1)
{
// Beep to show that we finished the maze.
set_motors(0,0);
play(">>a32");
// Wait for the user to press a button, while displaying
// the solution.
while(!button_is_pressed(BUTTON_B))
{
if(get_ms() % 2000 < 1000)
{
clear();
print("Solved!");
lcd_goto_xy(0,1);
print("Press B");
}
else
display_path();
delay_ms(30);
}
while(button_is_pressed(BUTTON_B));
delay_ms(1000);
// Re-run the maze. It's not necessary to identify the
// intersections, so this loop is really simple.
int i;
for(i=0;i<path_length;i++)
{
// SECOND MAIN LOOP BODY
follow_segment();
// Drive straight while slowing down, as before.
set_motors(50,50);
delay_ms(50);
set_motors(40,40);
delay_ms(200);
// Make a turn according to the instruction stored in
// path[i].
turn(path[i]);
}
// Follow the last segment up to the finish.
follow_segment();
// Now we should be at the finish! Restart the loop.
}
}
int first_main_loop()
{
/* This function returns 1 when it finds "something interesting"
* Otherwise, it just lets itself finish. It will be run until
* something "truthy" is returned
*/
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
set_motors(50,50);
delay_ms(50);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, while examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
read_line(sensors,IR_EMITTERS_ON);
// Check for left and right exits.
if(sensors[0] > 100)
found_left = 1;
if(sensors[4] > 100)
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(40,40);
delay_ms(200);
// Check for a straight exit.
read_line(sensors,IR_EMITTERS_ON);
if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_straight = 1;
// Check for the ending spot.
// If all three middle sensors are on dark black, we have
// solved the maze.
if(sensors[1] > 400 && sensors[2] > 400 && sensors[3] > 400)
return 1;
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
// from `turn.c`
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
// from `turn.c`
turn(dir);
// Store the intersection in the path variable.
if(path_length < 100){
path[path_length] = dir;
path_length ++;
}
else{
//throw it away for now
}
// Display the path on the LCD.
display_path();
return 0;
}