merge pull request from @nonailla

8 files changed, 49 insertions, 71 deletions

diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 0f02f37..a5910f2 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -4,7 +4,7 @@
 #include "movement.h"
 #include "orangutan_shim.h"
 
-int g_w2_charged_status;
+int g_w2_charged_status;	//used to detect the charging station (once)
 
 void w2_short_drive() {
 	set_motors(50, 50);
@@ -12,6 +12,7 @@ void w2_short_drive() {
 	set_motors(0, 0);
 }
 
+//charging station
 void w2_home() {
 	set_motors(0, 0);
 	delay_ms(150);
@@ -24,6 +25,7 @@ void w2_home() {
 	delay_ms(2000);
 }
 
+//crosswalk from charging station back to maze
 void w2_charge_cross_walk() {
 	if (g_w2_transition == 0) {
 		set_motors(-30, 30);
@@ -53,6 +55,7 @@ void w2_charge_cross_walk() {
 	}
 }
 
+//main function for charge mode
 void w2_mode_chrg() {
 	unsigned int last_proportional = 0;
 	long integral				   = 0;
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 48a7269..4364b67 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -19,12 +19,13 @@ w2_s_grid_coordinate g_w2_destination;
 w2_e_orientation g_w2_direction;
 
 int g_w2_detection = 0;
-int g_w2_transition;
-char g_w2_x_location = 0;
+int g_w2_transition;	//used for the crosswalk, used to count black lines
+char g_w2_x_location = 0; //current location in grid
 char g_w2_y_location = 0;
 
+
+//used for the crosswalk from maze to grid
 void w2_crosswalk_stroll() {
-	print("hoi");
 	while (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 &&
 		   g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) {
 		set_motors(15, 15);
@@ -56,6 +57,7 @@ void w2_grid_crossway_detection() {
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
+//main function for grid mode
 void w2_grid_follow_line() {
 	unsigned int last_proportional = 0;
 	long integral				   = 0;
@@ -73,9 +75,9 @@ void w2_grid_follow_line() {
 		if (power_difference < -max) power_difference = -max;
 
 		if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 &&
-			g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {
+			g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) { //crossways/intersections
 			break;
-		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { //left corners
 			break;
 		} else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 &&
 				   g_w2_sensors[0] < 100) { // for the south and west borders of the grid
@@ -97,17 +99,22 @@ void w2_grid_follow_line() {
 	}
 }
 
+//begin location when entering the grid
 void w2_begin_location() {
 	g_w2_location.x = 4;
 	g_w2_location.y = 0;
 	g_w2_direction	= W2_ORT_WEST;
 }
 
+
+//location of grid exit
 void w2_end_destination() {
 	g_w2_destination.x = 4;
 	g_w2_destination.y = 4;
 }
 
+
+//turns are used to get the correct orientation when picking up orders
 void w2_turn_north() {
 	switch (g_w2_direction) {
 		case W2_ORT_NORTH:
@@ -166,7 +173,6 @@ void w2_turn_south() {
 
 		case W2_ORT_EAST:
 			w2_grid_rotation_half_right();
-			;
 			break;
 	}
 
@@ -195,14 +201,17 @@ void w2_turn_east() {
 	g_w2_direction = W2_ORT_EAST;
 }
 
+
+//signals when the product is picked
 void w2_arrived_message() {
 	if (g_w2_location.x == g_w2_destination.x && g_w2_location.y == g_w2_destination.y) {
-		print_long(g_w2_order_number);
 		play_frequency(400, 500, 7);
 		delay(500);
 	}
 }
 
+
+//go to correct x coordinate
 void w2_go_to_x() {
 	if (g_w2_location.x != g_w2_destination.x) {
 		while (g_w2_location.x != g_w2_destination.x) {
@@ -223,6 +232,7 @@ void w2_go_to_x() {
 	}
 }
 
+//go to correct y coordinate
 void w2_go_to_y() {
 	if (g_w2_location.y != g_w2_destination.y) {
 		while (g_w2_location.y != g_w2_destination.y) {
@@ -243,6 +253,7 @@ void w2_go_to_y() {
 	}
 }
 
+//main function for grid mode
 void w2_mode_grid() {
 	set_motors(0, 0);
 	delay(500);
@@ -259,67 +270,13 @@ void w2_mode_grid() {
 		delay(1000);
 		w2_go_to_x();
 		w2_go_to_y();
+		w2_arrived_message();
 	}
 	w2_end_destination();
 
 	delay(1000);
 	w2_go_to_y();
 	w2_go_to_x();
-	w2_turn_east(); // this was uncommented (6.3)
+	w2_turn_east();
 	w2_modes_swap(W2_M_CHRG);
 }
-
-/*
-void w2_mode_maze() {
-	unsigned int last_proportional = 0;
-	long integral				   = 0;
-
-	clear();
-	print("MAZE");
-
-	g_w2_transition = 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.
-		g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-
-		// The "proportional" term should be 0 when we are on the line.
-		int proportional = ((int)g_w2_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 (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 &&
-g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { } else if (g_w2_sensors[0] >= 500 &&
-g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && g_w2_sensors[3] >= 250 && g_w2_sensors[4] >=
-500) { w2_crossway_detection(); } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 &&
-g_w2_sensors[4] < 100) { w2_half_rotation_left(); } else { if (power_difference < 0 &&
-(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) set_motors(max +
-power_difference, max); else if (power_difference > 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] >
-100 || g_w2_sensors[1] > 100)) set_motors(max, max - power_difference);
-		}
-	}
-}
-*/
diff --git a/robot/mode_halt.c b/robot/mode_halt.c
index 083d45b..8bd051e 100644
--- a/robot/mode_halt.c
+++ b/robot/mode_halt.c
@@ -1,4 +1,5 @@
 #include "mode_halt.h"
 #include "orangutan_shim.h"
 
+//emergency stop
 void w2_mode_halt() { set_motors(0, 0); }
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
index eb75e3c..0350205 100644
--- a/robot/mode_maze.c
+++ b/robot/mode_maze.c
@@ -6,6 +6,8 @@
 unsigned int g_w2_last_proportional = 0;
 long g_w2_integral					= 0;
 
+
+//main function for maze mode
 void w2_mode_maze() {
 	// PID controller
 	g_w2_position	 = read_line(g_w2_sensors, IR_EMITTERS_ON);
@@ -20,14 +22,17 @@ void w2_mode_maze() {
 	if (power_difference < -max) power_difference = -max;
 
 	if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 &&
-		g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) {
+		g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { //dead ends
 		w2_crosswalk_stroll();
-	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 &&
-			   g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {
+	} 
+	else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 &&
+			   g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {	//crossways or intersection
 		w2_maze_rotation_half_left();
-	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+	} 
+	else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {	//left corners
 		w2_maze_rotation_half_left();
-	} else {
+	} 
+	else {		//normal line following
 		if (power_difference < 0 &&
 			(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100))
 			set_motors(max + power_difference, max);
diff --git a/robot/mode_scal.c b/robot/mode_scal.c
index b0efa09..8835183 100644
--- a/robot/mode_scal.c
+++ b/robot/mode_scal.c
@@ -2,6 +2,7 @@
 #include "modes.h"
 #include "orangutan_shim.h"
 
+//callibrates the robot
 void w2_mode_scal() {
 	for (int counter = 0; counter < 80; counter++) {
 		if (counter < 20 || counter >= 60) {
diff --git a/robot/mode_spin.c b/robot/mode_spin.c
index 9ee83b0..79f9dea 100644
--- a/robot/mode_spin.c
+++ b/robot/mode_spin.c
@@ -1,4 +1,6 @@
 #include "mode_spin.h"
 #include "orangutan_shim.h"
 
+
+//wet floor simulation
 void w2_mode_spin() { set_motors(255, -255); }
diff --git a/robot/modes.c b/robot/modes.c
index 4995d6f..aedc02a 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -16,6 +16,7 @@ w2_e_mode g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE];
 uint8_t g_w2_mode_history_index = 0;
 void (*g_w2_modes[W2_MODE_COUNT])();
 
+//all of the different modi
 void w2_modes_init() {
 	g_w2_modes[W2_M_CHRG] = &w2_mode_chrg;
 	g_w2_modes[W2_M_DIRC] = &w2_mode_dirc;
diff --git a/robot/movement.c b/robot/movement.c
index 61a944b..37d66d5 100644
--- a/robot/movement.c
+++ b/robot/movement.c
@@ -1,9 +1,11 @@
 #include "movement.h"
 #include "orangutan_shim.h"
 
-unsigned int g_w2_sensors[5] = {0};
-unsigned int g_w2_position	 = 0;
+unsigned int g_w2_sensors[5] = {0};		//IR sensors on the bottom of the robot
+unsigned int g_w2_position	 = 0;		//position on the black line
 
+
+//full rotation maze/charge
 void w2_maze_rotation_full() {
 	set_motors(0, 0);
 	delay_ms(500);
@@ -14,6 +16,7 @@ void w2_maze_rotation_full() {
 	delay_ms(500);
 }
 
+//left turn maze/charge
 void w2_maze_rotation_half_left() {
 	set_motors(0, 0);
 	set_motors(50, 50);
@@ -24,6 +27,8 @@ void w2_maze_rotation_half_left() {
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 	delay_ms(500);
 }
+
+//right turn maze/charge
 void w2_maze_rotation_half_right() {
 	set_motors(0, 0);
 	set_motors(50, 50);
@@ -37,6 +42,7 @@ void w2_maze_rotation_half_right() {
 	delay_ms(500);
 }
 
+//180 turn in grid
 void w2_grid_rotation_full() {
 	set_motors(60, -60);
 	delay_ms(540);
@@ -44,6 +50,7 @@ void w2_grid_rotation_full() {
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
+//left turn in grid
 void w2_grid_rotation_half_left() {
 	set_motors(-30, 30);
 	delay_ms(600);
@@ -51,6 +58,7 @@ void w2_grid_rotation_half_left() {
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
+//right turn in grid
 void w2_grid_rotation_half_right() {
 	set_motors(30, -30);
 	delay_ms(600);