WIP merge

10 files changed, 455 insertions, 836 deletions

diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 81808d5..27e17d9 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -1,3 +1,134 @@
 #include "mode_chrg.h"
+#include "orangutan_shim.h"
+#include "movement.h"
+#include "modes.h"
+#include "transition.h"
 
-void w2_mode_chrg() {}
+int g_w2_charged_status;
+
+void w2_short_drive() {
+	set_motors(50, 50);
+	delay(150);
+	set_motors(0, 0);
+}
+
+void w2_home() {
+	set_motors(0, 0);
+	delay_ms(150);
+	clear();
+	print("CHARGING");
+	set_motors(30, 30);
+	delay_ms(600);
+	set_motors(0, 0);
+	play_frequency(300, 500, 7);
+	delay_ms(600);
+	g_w2_position	  = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	g_w2_charged_status = 1;
+	clear();
+	delay_ms(2000);
+}
+
+
+void w2_charge_cross_walk() {
+	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);
+		delay(500);
+		g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+		if (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) {
+			set_motors(0, 0);
+			clear();
+			print("WALK");
+			g_w2_transition++;
+			if (g_w2_transition == 3) { //TODO: document g_w2_transition
+				set_motors(40, 40);
+				delay(600);
+				set_motors(0, 0);
+
+				g_w2_transition	= 0;
+				// g_w2_maze_status	= 1;
+				w2_modes_swap(W2_M_MAZE);
+				break;
+			}
+		} else {
+			g_w2_transition = 0;
+			w2_full_rotation();
+		}
+	}
+}
+
+
+void w2_mode_chrg() {
+	unsigned int last_proportional = 0;
+	long integral				   = 0;
+	// initialize();
+	g_w2_charged_status = 0;
+	clear();
+	print("CHARGE");
+
+	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) {
+			w2_charge_cross_walk();
+			if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) {
+				break;
+			}
+		}
+
+		else if ((g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 &&
+				  g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) &&
+				 g_w2_charged_status == 0) {
+			w2_home();
+			delay(200);
+			w2_full_rotation();
+			w2_short_drive();
+		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+			clear();
+			w2_half_rotation_left();
+		}
+
+		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) {
+			clear();
+			w2_crossway_detection();
+		} 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_grid.c b/robot/mode_grid.c
index 23e539e..927d4bc 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,747 +1,315 @@
-/*
- * 3pi-linefollower-pid - demo code for the Pololu 3pi Robot
- * 
- * This code will follow a black line on a white background, using a
- * PID-based algorithm.
- *
- * http://www.pololu.com/docs/0J21
- * http://www.pololu.com
- * http://forum.pololu.com
- *
- */
-
-// The 3pi include file must be at the beginning of any program that
-// uses the Pololu AVR library and 3pi.
-#include <pololu/3pi.h>
-
-// This include file allows data to be stored in program space.  The
-// ATmega168 has 16k of program space compared to 1k of RAM, so large
-// pieces of static data should be stored in program space.
-#include <avr/pgmspace.h>
-
-unsigned int sensors[5]; // an array to hold sensor values
-unsigned int position;
-int orderNumber;
-int transition;
-int chargedStatus;
-
-int mazeStatus;
-
-
-enum section{
-	mazeMode,
-	gridMode,
-	chargeMode	
-} parcourMode;
-
-enum orientation{
-	North,
-	East,
-	South,
-	West
-} direction;
-
-
-typedef struct {
-	int x;
-	int y;
-} coordinates;
-
-coordinates order[4];
-coordinates location;
-coordinates destination;
-
-int Detection;
-char xLocation;
-char yLocation;
-
-
-// Introductory messages.  The "PROGMEM" identifier causes the data to
-// go into program space.
-const char welcome_line1[] PROGMEM = " Pololu";
-const char welcome_line2[] PROGMEM = "3\xf7 Robot";
-const char demo_name_line1[] PROGMEM = "PID Line";
-const char demo_name_line2[] PROGMEM = "follower";
-
-// A couple of simple tunes, stored in program space.
-const char welcome[] PROGMEM = ">g32>>c32";
-const char go[] PROGMEM = "L16 cdegreg4";
-
-// Data for generating the characters used in load_custom_characters
-// and display_readings.  By reading levels[] starting at various
-// offsets, we can generate all of the 7 extra characters needed for a
-// bargraph.  This is also stored in program space.
-const char levels[] PROGMEM = {
-	0b00000,
-	0b00000,
-	0b00000,
-	0b00000,
-	0b00000,
-	0b00000,
-	0b00000,
-	0b11111,
-	0b11111,
-	0b11111,
-	0b11111,
-	0b11111,
-	0b11111,
-	0b11111
-};
-
-void grid();
-
-// 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);
-	}
-}
-
-// 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();
+#include "mode_grid.h"
+#include "orangutan_shim.h"
+#include "modes.h"
+#include "movement.h"
+#include "transition.h"
 
-		clear();
-		print_long(bat);
-		print("mV");
-		lcd_goto_xy(0,1);
-		print("Press B");
+int g_w2_order_number;
 
-		delay_ms(100);
-	}
+int g_w2_maze_status = 0;
 
-	// 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);
+w2_s_grid_coordinate g_w2_order[4];
+w2_s_grid_coordinate g_w2_location;
+w2_s_grid_coordinate g_w2_destination;
+w2_e_orientation g_w2_direction;
 
-		delay_ms(100);
-	}
-	wait_for_button_release(BUTTON_B);
+int g_w2_detection = 0;
+char g_w2_x_location = 0;
+char g_w2_y_location = 0;
 
-	clear();
-
-	print("Go!");		
-
-	// Play music and wait for it to finish before we start driving.
-	play_from_program_space(go);
-	while(is_playing());
-}
-
-void full_rotation(){
-	set_motors(0,0);
-	delay_ms(500);
-	set_motors(60,-60);
-	delay_ms(540);
-	set_motors(0,0);
-	delay_ms(100);
-	set_motors(10,10);
-	delay(500);
-	set_motors(0,0);
-	position = read_line(sensors,IR_EMITTERS_ON);
-	delay_ms(500);
-}
-
-void half_rotation_left(){
-	set_motors(0,0);
-	set_motors(50,50);
-	delay_ms(150);
-	set_motors(-30,30);
-	delay_ms(600);
-	set_motors(0,0);
-	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(15,15);
+void w2_crosswalk_detection() {
+	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);
 		delay(300);
-		position = read_line(sensors,IR_EMITTERS_ON);
-		if(sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100){
-			set_motors(0,0);
+		g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+		if (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) {
+			set_motors(0, 0);
 			clear();
 			print("WALK");
-			transition++;
-			if(transition == 3){
-				set_motors(40,40); 
-				delay(600);	
-				transition = 0;
-				parcourMode = gridMode ;
+			g_w2_transition++;
+			if (g_w2_transition == 3) {
+				set_motors(40, 40);
+				delay(600);
+				g_w2_transition	= 0;
+				w2_modes_swap(W2_M_GRID);
 			}
 		}
-		
-		else{
-			transition = 0;
-			full_rotation();
-		}
-	}
-}
 
-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();
-			}
+		else {
+			g_w2_transition = 0;
+			w2_full_rotation();
 		}
+	}
 }
 
-void grid_rotation_full(){
-	set_motors(60,-60);
+void w2_grid_rotation_full() {
+	set_motors(60, -60);
 	delay_ms(540);
-	set_motors(10,10);
-	position = read_line(sensors,IR_EMITTERS_ON);
+	set_motors(10, 10);
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
-void grid_rotation_left(){
-	set_motors(-30,30);
+void w2_grid_rotation_left() {
+	set_motors(-30, 30);
 	delay_ms(600);
-	set_motors(10,10);
-	position = read_line(sensors,IR_EMITTERS_ON);
+	set_motors(10, 10);
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
-void grid_rotation_right(){
-	set_motors(30,-30);
+void w2_grid_rotation_right() {
+	set_motors(30, -30);
 	delay_ms(600);
-	set_motors(10,10);
-	position = read_line(sensors,IR_EMITTERS_ON);
+	set_motors(10, 10);
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
-void grid_crossway_detection(){
-	set_motors(50,50);
+void w2_grid_crossway_detection() {
+	set_motors(50, 50);
 	delay_ms(150);
-	set_motors(10,10);
-	position = read_line(sensors,IR_EMITTERS_ON);
-}
-
-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;
+	set_motors(10, 10);
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+}
+
+void w2_grid_follow_line() {
+	static unsigned int last_proportional = 0;
+	static long integral				   = 0;
+
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	int proportional = ((int)g_w2_position) - 2000;
+	int derivative = proportional - last_proportional;
+	integral += proportional;
+	last_proportional = proportional;
+	int power_difference = proportional / 20 + integral / 10000 + derivative * 3 / 2;
+
+	const int max = 60;
+	if (power_difference > max) power_difference = max;
+	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) {
+		return;
+	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+		return;
+	} 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
+		return;
+	} else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[2] < 100 &&
+			   g_w2_sensors[0] < 100) { // sharp right corners
+		return;
+	}
 
-		// Compute the derivative (change) and integral (sum) of the
-		// position.
-		int derivative = proportional - last_proportional;
-		integral += proportional;
+	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);
+		}
+	}
+}
 
-		// Remember the last position.
-		last_proportional = proportional;
+void w2_begin_location() {
+	g_w2_location.x = 4;
+	g_w2_location.y = 0;
+	g_w2_direction  = W2_ORT_WEST;
+}
 
-		// 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;
+void w2_end_destination() {
+	g_w2_destination.x = 4;
+	g_w2_destination.y = 4;
+}
 
-		// 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){
+void w2_turn_north() {
+	switch (g_w2_direction) {
+		case W2_ORT_NORTH:
 			break;
-		}
-		else if(sensors[4] >= 500 && sensors[3] >= 200 && sensors[0] < 100){ //for the south and west borders of the grid
+
+		case W2_ORT_EAST:
+			w2_grid_rotation_left();
 			break;
-		}
-		else if(sensors[4] >= 500 && sensors[3] >= 200 && sensors[2] <100 && sensors[0] < 100){ //sharp right corners
+
+		case W2_ORT_SOUTH:
+			w2_grid_rotation_full();
 			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);}
-		}
+		case W2_ORT_WEST:
+			w2_grid_rotation_right();
+			break;
 	}
-}
 
-void shortDrive(){
-	set_motors(50,50);
-	delay(150);
-	set_motors(0,0);
+	g_w2_direction = W2_ORT_NORTH;
 }
 
-void orderOne(){
-	order[0].x = 1;
-	order[0].y = 4;
-}
+void w2_turn_west() {
+	switch (g_w2_direction) {
+		case W2_ORT_WEST:
+			break;
 
-void orderTwo(){
-	order[1].x = 3;
-	order[1].y = 2;
-}
+		case W2_ORT_NORTH:
+			w2_grid_rotation_left();
+			break;
 
-void orderThree(){
-	order[2].x = 1;
-	order[2].y = 4;
-}
+		case W2_ORT_EAST:
+			w2_grid_rotation_full();
+			break;
+
+		case W2_ORT_SOUTH:
+			w2_grid_rotation_right();
+			;
+			break;
+	}
 
-void orderFour(){
-	order[3].x = 0;
-	order[3].y = 0;
+	g_w2_direction = W2_ORT_WEST;
 }
 
+void w2_turn_south() {
+	switch (g_w2_direction) {
+		case W2_ORT_SOUTH:
+			break;
 
-void beginLocation(){
-	location.x = 4;
-	location.y = 0;
-	direction = West;
-}
+		case W2_ORT_WEST:
+			w2_grid_rotation_left();
+			break;
 
-void endDestination(){
-	destination.x = 4;
-	destination.y = 4 ;
-}
+		case W2_ORT_NORTH:
+			w2_grid_rotation_full();
+			break;
 
-void turn_North()
-{
-	switch (direction)
-	{
-		case North:
-		break;
-		
-		case East:
-		grid_rotation_left();
-		break;
-		
-		case South:
-		grid_rotation_full();
-		break;
-		
-		case West:
-		grid_rotation_right();
-		break;
+		case W2_ORT_EAST:
+			w2_grid_rotation_right();
+			;
+			break;
 	}
-	
-	direction = North;
-}
 
-void turn_West()
-{
-	switch (direction)
-	{
-		case West:
-		break;
-		
-		case North:
-		grid_rotation_left();
-		break;
-		
-		case East:
-		grid_rotation_full();
-		break;
-		
-		case South:
-		grid_rotation_right();;
-		break;
-	}
-	
-	direction = West;
+	g_w2_direction = W2_ORT_SOUTH;
 }
 
-void turn_South()
-{
-	switch (direction)
-	{
-		case South:
-		break;
-		
-		case West:
-		grid_rotation_left();
-		break;
-		
-		case North:
-		grid_rotation_full();
-		break;
-		
-		case East:
-		grid_rotation_right();;
-		break;
-	}
-	
-	direction = South;
-}
+void w2_turn_east() {
+	switch (g_w2_direction) {
+		case W2_ORT_EAST:
+			break;
+
+		case W2_ORT_SOUTH:
+			w2_grid_rotation_left();
+			break;
 
-void turn_East()
-{
-	switch (direction)
-	{
-		case East:
-		break;
-		
-		case South:
-		grid_rotation_left();
-		break;
-		
-		case West:
-		grid_rotation_full();
-		break;
-		
-		case North:
-		grid_rotation_right();;
-		break;
+		case W2_ORT_WEST:
+			w2_grid_rotation_full();
+			break;
+
+		case W2_ORT_NORTH:
+			w2_grid_rotation_right();
+			;
+			break;
 	}
-	
-	direction = East;
-}
 
-void locationMessage(){
-	clear();
-	print_long(location.x);
-	print(",");
-	print_long(location.y);
-	delay(200); 
+	g_w2_direction = W2_ORT_EAST;
 }
 
-void arrivedMessage(){
-	if (location.x == destination.x && location.y == destination.y){
+void w2_arrived_message() {
+	if (g_w2_location.x == g_w2_destination.x && g_w2_location.y == g_w2_destination.y) {
 		clear();
 		print("ORDER ");
-		print_long(orderNumber);
-		play_frequency(400,500,7);
+		print_long(g_w2_order_number);
+		play_frequency(400, 500, 7);
 		delay(500);
 	}
 }
 
-void goToX(){
-	if (location.x != destination.x ){
-		while(location.x != destination.x ){
-			if (location.x > destination.x){
-				turn_West();
-				gridFollowLine();
-				grid_crossway_detection();
-				location.x--;
-				locationMessage();
+void w2_go_to_x() {
+	if (g_w2_location.x != g_w2_destination.x) {
+		while (g_w2_location.x != g_w2_destination.x) {
+			if (g_w2_location.x > g_w2_destination.x) {
+				w2_turn_west();
+				w2_grid_follow_line();
+				w2_grid_crossway_detection();
+				g_w2_location.x--;
 			}
-			
-			else if(location.x < destination.x){
-				turn_East();
-				gridFollowLine();
-				grid_crossway_detection();
-				location.x++;
-				locationMessage();
+
+			else if (g_w2_location.x < g_w2_destination.x) {
+				w2_turn_east();
+				w2_grid_follow_line();
+				w2_grid_crossway_detection();
+				g_w2_location.x++;
 			}
 		}
 	}
 }
 
-void goToY(){
-	if (location.y != destination.y ){
-		while(location.y != destination.y ){
-			if (location.y > destination.y){
-				turn_South();
-				gridFollowLine();
-				grid_crossway_detection();
-				location.y--;
-				locationMessage();
+void w2_go_to_y() {
+	if (g_w2_location.y != g_w2_destination.y) {
+		while (g_w2_location.y != g_w2_destination.y) {
+			if (g_w2_location.y > g_w2_destination.y) {
+				w2_turn_south();
+				w2_grid_follow_line();
+				w2_grid_crossway_detection();
+				g_w2_location.y--;
 			}
-			
-			else if(location.y < destination.y){
-				turn_North();
-				gridFollowLine();
-				grid_crossway_detection();
-				location.y++;
-				locationMessage();
+
+			else if (g_w2_location.y < g_w2_destination.y) {
+				w2_turn_north();
+				w2_grid_follow_line();
+				w2_grid_crossway_detection();
+				g_w2_location.y++;
 			}
 		}
 	}
 }
 
-void home(){
-	set_motors(0,0);
-	delay_ms(150);
+void w2_mode_grid() {
+	set_motors(0, 0);
 	clear();
-	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);
-}
+	print("GRID");
+	delay(500);
 
-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
-			// 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] < 100 && sensors[1] <100 && sensors[2] < 100 &&  sensors[3] < 100 && sensors[4] < 100){
-				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(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{
-				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);}
-			}			
-	}
-}
+	w2_begin_location();
 
-void grid() {
+	// TODO: orders read here
+	for (int i = 0; i < 4; i++) {
+		g_w2_order_number = i + 1;
 
-	set_motors(0,0);
-	clear();
-	print("GRID");
-	delay(500);
+		g_w2_destination.x = g_w2_order[i].x;
+		g_w2_destination.y = g_w2_order[i].y;
 
-	//coordinates of the orders
-	orderOne();
-	orderTwo();
-	orderThree();
-	orderFour();
-	
-	beginLocation();
-
-	for (int i = 0; i < 4; i++ ){
-		orderNumber = i+1;
-		
-		destination.x = order[i].x;
-		destination.y = order[i].y;
-		
-		locationMessage();
 		delay(1000);
-		goToX();
-		goToY();
-		arrivedMessage();
+		w2_go_to_x();
+		w2_go_to_y();
 	}
-	//go to the end of the grid, to transition to charge station
-	endDestination();
-	
-	locationMessage();
+	w2_end_destination();
+
 	delay(1000);
-	goToY();
-	goToX();
-	turn_East(); //this was uncommented (6.3)
-	parcourMode = chargeMode;
+	w2_go_to_y();
+	w2_go_to_x();
+	w2_turn_east(); // this was uncommented (6.3)
+	w2_modes_swap(W2_M_CHRG);
 }
 
+/*
+void w2_mode_maze() {
 
+	unsigned int last_proportional = 0;
+	long integral				   = 0;
 
-// This is the main function, where the code starts.  All C programs
-// must have a main() function defined somewhere.
-void maze()
-{
-	
-	unsigned int last_proportional=0;
-	long integral=0;
-	
 	// set up the 3pi
-	if (mazeStatus == 0){
-		initialize();
+	if (g_w2_maze_status == 0) {
+		w2_initialize();
 	}
 	clear();
 	print("MAZE");
-	
-	
-	transition = 0;
+
+	g_w2_transition = 0;
 
 	// This is the "main loop" - it will run forever.
-	while(1)
-	{    
+	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);
+		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)position) - 2000;
+		int proportional = ((int)g_w2_position) - 2000;
 
 		// Compute the derivative (change) and integral (sum) of the
 		// position.
@@ -756,62 +324,35 @@ void maze()
 		// 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;
+		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] < 100 && sensors[1] <100 && sensors[2] < 100 &&  sensors[3] < 100 && sensors[4] < 100){	
-				cross_walk();
-				if (parcourMode == gridMode){
-					break;
-				}
-				//full_rotation();			
-		}
-		else if(sensors[0] >= 500 && sensors[1] >= 250  && sensors[2] >= 500  &&  sensors[3] >= 250  &&sensors[4] >= 500){
-			crossway_detection();
-		}
-		else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){
-			half_rotation_left();
-		}
-		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);			
-		}
-		       		
-	}
 
-}
-
-void mode(){
-	while(1){
-		if(parcourMode == mazeMode){
-			maze();
-		}
-		else if(parcourMode == gridMode){
-			grid();
-		}
-		else if(parcourMode == chargeMode){
-			charge();
+		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) {
+			w2_cross_walk();
+			if (g_w2_parcour_mode == gridMode) {
+				break;
+			}
+			// full_rotation();
+		} 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);
 		}
 	}
 }
-
-
-int main(){
-	chargedStatus = 0;
-	mazeStatus = 0;
-	
-	parcourMode = mazeMode;
-	
-	mode();
-}
+*/
 
diff --git a/robot/mode_grid.h b/robot/mode_grid.h
index b13cc4c..c8e649f 100644
--- a/robot/mode_grid.h
+++ b/robot/mode_grid.h
@@ -7,87 +7,23 @@
  *
  * processes orders from the order buffer
  */
+void w2_mode_grid();
 
-enum orientation{
-    North,
-    East,
-    South,
-    West
-} direction;
-
-typedef struct {
-    int x;
-    int y;
-} coordinates;
-
-coordinates order[4];
-coordinates location;
-coordinates destination;
-
-char xLocation;
-char yLocation;
-
-//LINE 25-68 CAN BE TAKEN AWAY WHEN USED WITH MODE_MAZE
-
-// The 3pi include file must be at the beginning of any program that
-// uses the Pololu AVR library and 3pi.
-#include <pololu/3pi.h>
-
-// This include file allows data to be stored in program space.  The
-// ATmega168 has 16k of program space compared to 1k of RAM, so large
-// pieces of static data should be stored in program space.
-#include <avr/pgmspace.h>
+void w2_crosswalk_detection();
 
-unsigned int sensors[5]; // an array to hold sensor values
-unsigned int position;
-// Introductory messages.  The "PROGMEM" identifier causes the data to
-// go into program space.
-const char welcome_line1[] PROGMEM = " Pololu";
-const char welcome_line2[] PROGMEM = "3\xf7 Robot";
-const char demo_name_line1[] PROGMEM = "PID Line";
-const char demo_name_line2[] PROGMEM = "follower";
+typedef enum {
+	W2_ORT_NORTH,
+	W2_ORT_EAST,
+	W2_ORT_SOUTH,
+	W2_ORT_WEST
+} w2_e_orientation;
 
-// A couple of simple tunes, stored in program space.
-const char welcome[] PROGMEM = ">g32>>c32";
-const char go[] PROGMEM = "L16 cdegreg4";
+// enum w2_e_section { mazeMode, gridMode, chargeMode } g_w2_parcour_mode;
 
-// Data for generating the characters used in load_custom_characters
-// and display_readings.  By reading levels[] starting at various
-// offsets, we can generate all of the 7 extra characters needed for a
-// bargraph.  This is also stored in program space.
-const char levels[] PROGMEM = {
-    0b00000,
-    0b00000,
-    0b00000,
-    0b00000,
-    0b00000,
-    0b00000,
-    0b00000,
-    0b11111,
-    0b11111,
-    0b11111,
-    0b11111,
-    0b11111,
-    0b11111,
-    0b11111
-};
+typedef struct {
+	int x;
+	int y;
+} w2_s_grid_coordinate;
 
-void w2_mode_grid();
-void initialize();
-void full_rotation();
-void grid_rotation_left();
-void grid_rotation_right();
-void grid_crossway_detection();
-void gridFollowLine();
-void orderOne();
-void orderTwo();
-void orderThree();
-void orderFour();
-void beginLocation();
-void turn_North();
-void turn_West();
-void turn_South();
-void turn_East();
-void locationMessage();
-void arrivedMessage();
+extern w2_s_grid_coordinate g_w2_order[4];
 
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
index a178d3a..04a3bc2 100644
--- a/robot/mode_maze.c
+++ b/robot/mode_maze.c
@@ -1,72 +1,20 @@
 #include "mode_maze.h"
 #include "orangutan_shim.h"
+#include "movement.h"
+#include "transition.h"
 
-unsigned int g_w2_sensors[5]		= {0};
-unsigned int g_w2_position			= 0;
 unsigned int g_w2_last_proportional = 0;
 long g_w2_integral					= 0;
 
-void w2_full_rotation() {
-	set_motors(0, 0);
-	delay_ms(500);
-	set_motors(60, -60);
-	delay_ms(540);
-	set_motors(0, 0);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-	delay_ms(500);
-}
-
-void w2_half_rotation_left() {
-	set_motors(0, 0);
-	set_motors(50, 50);
-	delay_ms(150);
-	set_motors(-30, 30);
-	delay_ms(600);
-	set_motors(0, 0);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-	delay_ms(500);
-}
-void w2_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);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-	delay_ms(500);
-}
-void w2_crossway_detection() { w2_half_rotation_left(); }
-void w2_intersection_detection() { w2_half_rotation_left(); }
 void w2_mode_maze() {
-	// 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.
+	// PID controller
 	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 - g_w2_last_proportional;
 	g_w2_integral += proportional;
-
-	// Remember the last position.
 	g_w2_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 + g_w2_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;
diff --git a/robot/mode_scal.c b/robot/mode_scal.c
index 53cbf67..b0efa09 100644
--- a/robot/mode_scal.c
+++ b/robot/mode_scal.c
@@ -3,7 +3,6 @@
 #include "orangutan_shim.h"
 
 void w2_mode_scal() {
-	pololu_3pi_init(2000);
 	for (int counter = 0; counter < 80; counter++) {
 		if (counter < 20 || counter >= 60) {
 			set_motors(40, -40);
diff --git a/robot/movement.c b/robot/movement.c
new file mode 100644
index 0000000..b4ad3c1
--- /dev/null
+++ b/robot/movement.c
@@ -0,0 +1,38 @@
+#include "orangutan_shim.h"
+#include "movement.h"
+
+unsigned int g_w2_sensors[5]		= {0};
+unsigned int g_w2_position			= 0;
+
+void w2_full_rotation() {
+	set_motors(0, 0);
+	delay_ms(500);
+	set_motors(60, -60);
+	delay_ms(540);
+	set_motors(0, 0);
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	delay_ms(500);
+}
+
+void w2_half_rotation_left() {
+	set_motors(0, 0);
+	set_motors(50, 50);
+	delay_ms(150);
+	set_motors(-30, 30);
+	delay_ms(600);
+	set_motors(0, 0);
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	delay_ms(500);
+}
+void w2_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);
+	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	delay_ms(500);
+}
diff --git a/robot/movement.h b/robot/movement.h
new file mode 100644
index 0000000..20a5d70
--- /dev/null
+++ b/robot/movement.h
@@ -0,0 +1,9 @@
+#pragma once
+
+extern unsigned int g_w2_sensors[5];
+extern unsigned int g_w2_position;
+
+void w2_full_rotation();
+void w2_half_rotation_left();
+void w2_half_rotation_right();
+
diff --git a/robot/setup.c b/robot/setup.c
index dfd88bd..1c59a0f 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -37,6 +37,9 @@ void w2_setup_main() {
 	// send info
 	w2_cmd_info_rx(NULL);
 
+	// initialize sensors using pololu library function
+	pololu_3pi_init(2000);
+
 	// indicate startup done
 	play("L50 c>c");
 }
diff --git a/robot/transition.c b/robot/transition.c
new file mode 100644
index 0000000..5b3244f
--- /dev/null
+++ b/robot/transition.c
@@ -0,0 +1,8 @@
+#include "transition.h"
+#include "movement.h"
+
+int g_w2_transition; //TODO: document
+					 //
+void w2_crossway_detection() { w2_half_rotation_left(); }
+void w2_intersection_detection() { w2_half_rotation_left(); }
+
diff --git a/robot/transition.h b/robot/transition.h
new file mode 100644
index 0000000..854c517
--- /dev/null
+++ b/robot/transition.h
@@ -0,0 +1,6 @@
+#pragma once
+
+extern int g_w2_transition;
+
+void w2_crossway_detection();
+void w2_intersection_detection();