예제 #1
0
파일: main.c 프로젝트: calvin-ieee/robots
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]);
}
예제 #2
0
파일: main.c 프로젝트: calvin-ieee/robots
// 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);
    }
}
예제 #3
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.
   }
}
예제 #4
0
// 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.
	}
}
예제 #5
0
파일: main.c 프로젝트: calvin-ieee/robots
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;
}