it all works (but messy)

1 files changed, 76 insertions, 548 deletions

diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index e217c28..23e539e 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,467 +1,3 @@
-#include "mode_grid.h"
-
-/*void w2_mode_grid() {
-
-    initialize(); //only keep this when using this module on its own
-    
-    set_motors(0,0);
-
-    //int gridMode = 0; //robot is in the grid
-
-    //coordinates of the orders
-    orderOne();
-    orderTwo();
-    orderThree();
-    orderFour();
-    
-    beginLocation();
-
-    for (int i = 0; i < 4; i++ ){
-        
-        destination.x = order[i].x;
-        destination.y = order[i].y;
-        
-        locationMessage();
-        delay(1000);    
-        
-        if (location.x != destination.x ){
-            while(location.x != destination.x ){
-                if (location.x > destination.x){
-                    turn_West();
-                    gridFollowLine();
-                    grid_crossway_detection();
-                    location.x--;
-                    locationMessage();
-                    if (location.x == destination.x){
-                        arrivedMessage();
-                    }
-                }
-                
-                else if(location.x < destination.x){
-                    turn_East();
-                    
-                    gridFollowLine();
-                    grid_crossway_detection();
-                    location.x++;
-                    locationMessage();
-                    if (location.x == destination.x){
-                        arrivedMessage();
-                    }
-                }
-            }
-        }
-            
-        if (location.y != destination.y ){
-            while(location.y != destination.y ){
-                if (location.y > destination.y){
-                    turn_South();
-                    gridFollowLine();
-                    grid_crossway_detection();
-                    location.y--;
-                    locationMessage();
-                    if (location.y == destination.y){
-                        arrivedMessage();
-                    }
-                }
-                
-                else if(location.y < destination.y){
-                    turn_North();
-                    gridFollowLine();
-                    grid_crossway_detection();
-                    location.y++;
-                    locationMessage();
-                    if (location.y == destination.y){
-                        arrivedMessage();
-                    }
-                }
-            }
-        }
-    }
-}
-
-void full_rotation(){
-    delay_ms(500);
-    set_motors(60,-60);
-    delay_ms(540);
-    set_motors(0,0);
-    position = read_line(sensors,IR_EMITTERS_ON);
-    delay_ms(500);
-}
-
-void grid_rotation_left(){
-    delay_ms(500);
-    set_motors(-30,30);
-    delay_ms(600);
-    set_motors(0,0);
-    position = read_line(sensors,IR_EMITTERS_ON);
-    delay_ms(500);
-}
-
-void grid_rotation_right(){
-    delay_ms(500);
-    set_motors(30,-30);
-    delay_ms(600);
-    set_motors(0,0);
-    position = read_line(sensors,IR_EMITTERS_ON);
-    delay_ms(500);
-}
-
-void grid_crossway_detection(){
-    set_motors(0,0);
-    set_motors(50,50);
-    delay_ms(150);  //150
-    set_motors(0,0);
-    position = read_line(sensors,IR_EMITTERS_ON);
-    delay_ms(500);
-}
-
-//variation on "void w2_mode_maze()" 
-void gridFollowLine(){
-    unsigned int last_proportional=0;
-    long integral=0;
-
-    // This is the "main loop" - it will run forever.
-    while(1)
-    {    
-        // Get the position of the line.  Note that we *must* provide
-        // the "sensors" argument to read_line() here, even though we
-        // are not interested in the individual sensor readings.
-        position = read_line(sensors,IR_EMITTERS_ON);
-
-        // The "proportional" term should be 0 when we are on the line.
-        int proportional = ((int)position) - 2000;
-
-        // Compute the derivative (change) and integral (sum) of the
-        // position.
-        int derivative = proportional - last_proportional;
-        integral += proportional;
-
-        // Remember the last position.
-        last_proportional = proportional;
-
-        // Compute the difference between the two motor power settings,
-        // m1 - m2.  If this is a positive number the robot will turn
-        // to the right.  If it is a negative number, the robot will
-        // turn to the left, and the magnitude of the number determines
-        // the sharpness of the turn.
-        int power_difference = proportional/20 + integral/10000 + derivative*3/2;
-
-        // Compute the actual motor settings.  We never set either motor
-        // to a negative value.      
-        const int max = 60;
-        if(power_difference > max)
-            power_difference = max;
-        if(power_difference < -max)
-            power_difference = -max;
-        
-        if(sensors[0] >= 500 && sensors[1] >= 250  && sensors[2] >= 500  &&  sensors[3] >= 250  &&sensors[4] >= 500){
-            break;
-        }
-        else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){
-            break;
-        }
-        else if(sensors[4] >= 500 && sensors[3] >= 200 && sensors[0] < 100){ //for the south and west borders of the grid
-            break;
-        }
-        else if(sensors[4] >= 500 && sensors[3] >= 200 && sensors[2] <100 && sensors[0] < 100){
-            break;
-        }
-
-        else{ 
-            if(power_difference < 0 && (sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100) ){
-                set_motors(max+power_difference, max);}
-            else if( power_difference > 0 && ( sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100)){
-                set_motors(max, max-power_difference);} 
-        }                   
-    }
-}
-
-//coördinates of the orders
-void orderOne(){
-    order[0].x = 1; //1
-    order[0].y = 3; //0
-}
-
-void orderTwo(){
-    order[1].x = 3; //2
-    order[1].y = 2; //2
-}
-
-void orderThree(){
-    order[2].x = 1; //0
-    order[2].y = 4; //4
-}
-
-void orderFour(){
-    order[3].x = 0; //3
-    order[3].y = 0; //1
-}
-
-//setting coördinate and direction when entering the grid from the maze
-void beginLocation(){
-    location.x = 4;
-    location.y = 0;
-    direction = West;
-}
-
-void turn_North()
-{
-    clear();
-    print("North");
-
-    switch (direction)
-    {
-        case North:
-        break;
-        
-        case East:
-        grid_rotation_left(); 
-        break;
-        
-        case South: 
-        full_rotation();
-        break;
-        
-        case West:
-        grid_rotation_right();
-        break;
-    }   
-    direction = North;
-}
-
-void turn_West()
-{
-    clear();
-    print("West");
-    
-    switch (direction)
-    {
-        case West:
-        break;
-        
-        case North:
-        grid_rotation_left(); 
-        break;
-        
-        case East:
-        full_rotation();
-        break;
-        
-        case South:
-        grid_rotation_right();; 
-        break;
-    }   
-    direction = West;
-}
-
-void turn_South()
-{
-    clear();
-    print("South");
-
-    switch (direction)
-    {
-        case South:
-        break;
-        
-        case West:
-        grid_rotation_left();
-        break;
-        
-        case North: 
-        full_rotation();
-        break;
-        
-        case East:
-        grid_rotation_right();; 
-        break;
-    }
-    direction = South;
-}
-
-void turn_East()
-{
-    clear();
-    print("East");
-
-    switch (direction)
-    {
-        case East:
-        break;
-        
-        case South:
-        grid_rotation_left(); 
-        break;
-        
-        case West:
-        full_rotation();
-        break;
-        
-        case North:
-        grid_rotation_right();; 
-        break;
-    }
-    direction = East;
-}
-
-void locationMessage(){
-    clear();
-    print_long(location.x);
-    print(",");
-    print_long(location.y);
-    delay(200); //1000
-}
-
-void arrivedMessage(){
-    clear();
-    print("ARRIVED");
-}
-
-//!UNDER THIS CAN ALL BE TAKEN AWAY, THIS IS USED IN MODE_MAZE & Callibration!
-
-// Initializes the 3pi, displays a welcome message, calibrates, and
-// plays the initial music.
-// Initializes the 3pi, displays a welcome message, calibrates, and
-// plays the initial music.
-void initialize()
-{
-    unsigned int counter; // used as a simple timer
-    
-    // This must be called at the beginning of 3pi code, to set up the
-    // sensors.  We use a value of 2000 for the timeout, which
-    // corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
-    pololu_3pi_init(2000);
-    load_custom_characters(); // load the custom characters
-    
-    // Play welcome music and display a message
-    print_from_program_space(welcome_line1);
-    lcd_goto_xy(0,1);
-    print_from_program_space(welcome_line2);
-    play_from_program_space(welcome);
-    delay_ms(1000);
-
-    clear();
-    print_from_program_space(demo_name_line1);
-    lcd_goto_xy(0,1);
-    print_from_program_space(demo_name_line2);
-    delay_ms(1000);
-
-    // Display battery voltage and wait for button press
-    while(!button_is_pressed(BUTTON_B))
-    {
-        int bat = read_battery_millivolts();
-
-        clear();
-        print_long(bat);
-        print("mV");
-        lcd_goto_xy(0,1);
-        print("Press B");
-
-        delay_ms(100);
-    }
-
-    // Always wait for the button to be released so that 3pi doesn't
-    // start moving until your hand is away from it.
-    wait_for_button_release(BUTTON_B);
-    delay_ms(1000);
-
-    // Auto-calibration: turn right and left while calibrating the
-    // sensors.
-    for(counter=0;counter<80;counter++)
-    {
-        if(counter < 20 || counter >= 60)
-        set_motors(40,-40);
-        else
-        set_motors(-40,40);
-
-        // This function records a set of sensor readings and keeps
-        // track of the minimum and maximum values encountered.  The
-        // IR_EMITTERS_ON argument means that the IR LEDs will be
-        // turned on during the reading, which is usually what you
-        // want.
-        calibrate_line_sensors(IR_EMITTERS_ON);
-
-        // Since our counter runs to 80, the total delay will be
-        // 80*20 = 1600 ms.
-        delay_ms(20);
-    }
-    set_motors(0,0);
-
-    // Display calibrated values as a bar graph.
-    while(!button_is_pressed(BUTTON_B))
-    {
-        // Read the sensor values and get the position measurement.
-        unsigned int position = read_line(sensors,IR_EMITTERS_ON);
-
-        // Display the position measurement, which will go from 0
-        // (when the leftmost sensor is over the line) to 4000 (when
-        // the rightmost sensor is over the line) on the 3pi, along
-        // with a bar graph of the sensor readings.  This allows you
-        // to make sure the robot is ready to go.
-        clear();
-        print_long(position);
-        lcd_goto_xy(0,1);
-        display_readings(sensors);
-
-        delay_ms(100);
-    }
-    wait_for_button_release(BUTTON_B);
-
-    clear();
-
-    print("Go!");
-
-    // Play music and wait for it to finish before we start driving.
-    play_from_program_space(go);
-    while(is_playing());
-}
-
-// This function loads custom characters into the LCD.  Up to 8
-// characters can be loaded; we use them for 7 levels of a bar graph.
-void load_custom_characters()
-{
-    lcd_load_custom_character(levels+0,0); // no offset, e.g. one bar
-    lcd_load_custom_character(levels+1,1); // two bars
-    lcd_load_custom_character(levels+2,2); // etc...
-    lcd_load_custom_character(levels+3,3);
-    lcd_load_custom_character(levels+4,4);
-    lcd_load_custom_character(levels+5,5);
-    lcd_load_custom_character(levels+6,6);
-    clear(); // the LCD must be cleared for the characters to take effect
-}
-
-// This function displays the sensor readings using a bar graph.
-void display_readings(const unsigned int *calibrated_values)
-{
-    unsigned char i;
-
-    for(i=0;i<5;i++) {
-        // Initialize the array of characters that we will use for the
-        // graph.  Using the space, an extra copy of the one-bar
-        // character, and character 255 (a full black box), we get 10
-        // characters in the array.
-        const char display_characters[10] = {' ',0,0,1,2,3,4,5,6,255};
-
-        // The variable c will have values from 0 to 9, since
-        // calibrated values are in the range of 0 to 1000, and
-        // 1000/101 is 9 with integer math.
-        char c = display_characters[calibrated_values[i]/101];
-
-        // Display the bar graph character.
-        print_character(c);
-    }
-}
-
-void full_rotation(){
-    set_motors(0,0);
-    delay_ms(500);
-    set_motors(60,-60);
-    delay_ms(540);
-    set_motors(0,0);
-    position = read_line(sensors,IR_EMITTERS_ON);
-    delay_ms(500);
-}*/
-
 /*
  * 3pi-linefollower-pid - demo code for the Pololu 3pi Robot
  * 
@@ -489,6 +25,8 @@ int orderNumber;
 int transition;
 int chargedStatus;
 
+int mazeStatus;
+
 
 enum section{
 	mazeMode,
@@ -709,48 +247,29 @@ void half_rotation_left(){
 	delay_ms(500);
 		
 }
-/*
-void half_rotation_right(){
-	set_motors(0,0);
-	set_motors(50,50);
-	delay_ms(150);
-	set_motors(30,-30);
-	delay_ms(600);
-	set_motors(0,0);
-	set_motors(50,50);
-	delay_ms(150);
-	position = read_line(sensors,IR_EMITTERS_ON);
-	delay_ms(500);
-}*/
+
 void crossway_detection(){
 	half_rotation_left();
 }
 
-
-
 void cross_walk(){
 	while(sensors[0] < 100 && sensors[1] <100 && sensors[2] < 100 &&  sensors[3] < 100 && sensors[4] < 100){
-		set_motors(10,10);
-		delay(500);
-		print("4");
-		//delay(500);
-		//set_motors(0,0);
-		//print("2");
-		//delay(150);
+		set_motors(15,15);
+		delay(300);
 		position = read_line(sensors,IR_EMITTERS_ON);
 		if(sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100){
 			set_motors(0,0);
-			print("ZEBRA");
+			clear();
+			print("WALK");
 			transition++;
 			if(transition == 3){
-				set_motors(20,20);
-				delay(700);
-				parcourMode++ ;
-				//grid();
-				//transition = 0;
-				//break;
+				set_motors(40,40); 
+				delay(600);	
+				transition = 0;
+				parcourMode = gridMode ;
 			}
 		}
+		
 		else{
 			transition = 0;
 			full_rotation();
@@ -758,42 +277,60 @@ void cross_walk(){
 	}
 }
 
+void charge_cross_walk(){
+		while(sensors[0] < 100 && sensors[1] <100 && sensors[2] < 100 &&  sensors[3] < 100 && sensors[4] < 100){
+			set_motors(15,15);
+			delay(500);
+			position = read_line(sensors,IR_EMITTERS_ON);
+			if(sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100){
+				set_motors(0,0);
+				clear();
+				print("WALK");
+				transition++;
+				if(transition == 3){
+					set_motors(40,40);
+					delay(600);	
+					set_motors(0,0);
+					
+					transition =0; 
+					mazeStatus = 1;
+					parcourMode = mazeMode;
+					break;
+				}
+			}
+			else{
+				transition = 0;
+				full_rotation();
+			}
+		}
+}
+
 void grid_rotation_full(){
-	//set_motors(0,0);
-	//delay_ms(500);
 	set_motors(60,-60);
 	delay_ms(540);
 	set_motors(10,10);
 	position = read_line(sensors,IR_EMITTERS_ON);
-	//delay_ms(500);
-
 }
 
 void grid_rotation_left(){
-	//delay_ms(500);
 	set_motors(-30,30);
 	delay_ms(600);
 	set_motors(10,10);
 	position = read_line(sensors,IR_EMITTERS_ON);
-	//delay_ms(500);
 }
 
 void grid_rotation_right(){
-	//delay_ms(500);
 	set_motors(30,-30);
 	delay_ms(600);
 	set_motors(10,10);
 	position = read_line(sensors,IR_EMITTERS_ON);
-	//delay_ms(500);
 }
 
 void grid_crossway_detection(){
-	//set_motors(0,0);
 	set_motors(50,50);
 	delay_ms(150);
 	set_motors(10,10);
 	position = read_line(sensors,IR_EMITTERS_ON);
-	//delay_ms(500);
 }
 
 void gridFollowLine(){
@@ -864,7 +401,7 @@ void shortDrive(){
 
 void orderOne(){
 	order[0].x = 1;
-	order[0].y = 3;
+	order[0].y = 4;
 }
 
 void orderTwo(){
@@ -878,10 +415,11 @@ void orderThree(){
 }
 
 void orderFour(){
-	order[3].x = 2;
-	order[3].y = 2;
+	order[3].x = 0;
+	order[3].y = 0;
 }
 
+
 void beginLocation(){
 	location.x = 4;
 	location.y = 0;
@@ -891,13 +429,10 @@ void beginLocation(){
 void endDestination(){
 	destination.x = 4;
 	destination.y = 4 ;
-	//direction = East;
 }
 
 void turn_North()
 {
-	//clear();
-	//print("North");
 	switch (direction)
 	{
 		case North:
@@ -921,9 +456,6 @@ void turn_North()
 
 void turn_West()
 {
-	//clear();
-	//print("West");
-	
 	switch (direction)
 	{
 		case West:
@@ -947,8 +479,6 @@ void turn_West()
 
 void turn_South()
 {
-	//clear();
-	//print("South");
 	switch (direction)
 	{
 		case South:
@@ -972,8 +502,6 @@ void turn_South()
 
 void turn_East()
 {
-	//clear();
-	//print("East");
 	switch (direction)
 	{
 		case East:
@@ -1000,7 +528,7 @@ void locationMessage(){
 	print_long(location.x);
 	print(",");
 	print_long(location.y);
-	delay(200); //1000
+	delay(200); 
 }
 
 void arrivedMessage(){
@@ -1008,6 +536,7 @@ void arrivedMessage(){
 		clear();
 		print("ORDER ");
 		print_long(orderNumber);
+		play_frequency(400,500,7);
 		delay(500);
 	}
 }
@@ -1060,19 +589,25 @@ void home(){
 	set_motors(0,0);
 	delay_ms(150);
 	clear();
-	print("HOME");
+	print("CHARGING");
 	set_motors(30,30);
 	delay_ms(600);
 	set_motors(0,0);
+	play_frequency(300,500,7);
 	delay_ms(600);
 	position = read_line(sensors,IR_EMITTERS_ON);
 	chargedStatus = 1;
+	clear();
 	delay_ms(2000);
 }
 
 void charge(){
 	unsigned int last_proportional=0;
 	long integral=0;
+	//initialize();
+	chargedStatus = 0;
+	clear();
+	print("CHARGE");
 	
 	while(1){
 			// Get the position of the line.  Note that we *must* provide
@@ -1109,21 +644,25 @@ void charge(){
 			
 
 			if(sensors[0] < 100 && sensors[1] <100 && sensors[2] < 100 &&  sensors[3] < 100 && sensors[4] < 100){
-				cross_walk();
-			}
-				
-			else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){
-				half_rotation_left();
+				charge_cross_walk();
+				if (parcourMode == mazeMode){
+					break;
+				}
 			}
 			
 			else if((sensors[0] >= 500 && sensors[1] >= 500  && sensors[2] >= 500  &&  sensors[3] >= 500  &&sensors[4] >= 500) && chargedStatus == 0){
 				home();
-				delay(1000);
+				delay(200);
 				full_rotation();
 				shortDrive();
 			}
+			else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){
+				clear();
+				half_rotation_left();
+			}
 			
 			else if(sensors[0] >= 500 && sensors[1] >= 250  && sensors[2] >= 500  &&  sensors[3] >= 250  &&sensors[4] >= 500){
+				clear();
 				crossway_detection();
 			}
 			else{
@@ -1131,16 +670,12 @@ void charge(){
 				set_motors(max+power_difference, max);}
 				else if( power_difference > 0 && ( sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100)){
 				set_motors(max, max-power_difference);}
-			}
-			
+			}			
 	}
 }
 
 void grid() {
 
-	//initialize(); //only keep this when using this module on its own
-	
-
 	set_motors(0,0);
 	clear();
 	print("GRID");
@@ -1154,7 +689,7 @@ void grid() {
 	
 	beginLocation();
 
-	for (int i = 0; i < 1; i++ ){				//CHANGE THIS TO 4 INSTEAD OF 1
+	for (int i = 0; i < 4; i++ ){
 		orderNumber = i+1;
 		
 		destination.x = order[i].x;
@@ -1165,19 +700,16 @@ void grid() {
 		goToX();
 		goToY();
 		arrivedMessage();
-
 	}
-	
+	//go to the end of the grid, to transition to charge station
 	endDestination();
 	
 	locationMessage();
 	delay(1000);
 	goToY();
 	goToX();
-	//turn_East();
-	arrivedMessage();
+	turn_East(); //this was uncommented (6.3)
 	parcourMode = chargeMode;
-	
 }
 
 
@@ -1191,7 +723,12 @@ void maze()
 	long integral=0;
 	
 	// set up the 3pi
-	initialize();
+	if (mazeStatus == 0){
+		initialize();
+	}
+	clear();
+	print("MAZE");
+	
 	
 	transition = 0;
 
@@ -1243,9 +780,6 @@ void maze()
 		else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){
 			half_rotation_left();
 		}
-		//else if(sensors[4] >= 500 && sensors[3] >= 200 && sensors[0] < 100){
-			//half_rotation_right();
-		//}
 		else{ 
 			if(power_difference < 0 && (sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100) )
 				set_motors(max+power_difference, max);
@@ -1258,19 +792,14 @@ void maze()
 }
 
 void mode(){
-	
 	while(1){
 		if(parcourMode == mazeMode){
 			maze();
 		}
-		if(parcourMode == gridMode){
+		else if(parcourMode == gridMode){
 			grid();
 		}
-		if(parcourMode == chargeMode){
-			set_motors(0,0);
-			clear();
-			print("CHARGE");
-			//delay(1000);
+		else if(parcourMode == chargeMode){
 			charge();
 		}
 	}
@@ -1278,12 +807,11 @@ void mode(){
 
 
 int main(){
-	
 	chargedStatus = 0;
+	mazeStatus = 0;
 	
 	parcourMode = mazeMode;
 	
 	mode();
-	
 }