Merge branch 'nonailla-master'

2 files changed, 899 insertions, 2 deletions

diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 8526499..23e539e 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,3 +1,817 @@
-#include "mode_grid.h"
+/*
+ * 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();
+
+		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());
+}
+
+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);
+		delay(300);
+		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);	
+				transition = 0;
+				parcourMode = gridMode ;
+			}
+		}
+		
+		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();
+			}
+		}
+}
+
+void grid_rotation_full(){
+	set_motors(60,-60);
+	delay_ms(540);
+	set_motors(10,10);
+	position = read_line(sensors,IR_EMITTERS_ON);
+}
+
+void grid_rotation_left(){
+	set_motors(-30,30);
+	delay_ms(600);
+	set_motors(10,10);
+	position = read_line(sensors,IR_EMITTERS_ON);
+}
+
+void grid_rotation_right(){
+	set_motors(30,-30);
+	delay_ms(600);
+	set_motors(10,10);
+	position = read_line(sensors,IR_EMITTERS_ON);
+}
+
+void 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;
+
+		// 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){ //sharp right corners
+			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);}
+		}
+	}
+}
+
+void shortDrive(){
+	set_motors(50,50);
+	delay(150);
+	set_motors(0,0);
+}
+
+void orderOne(){
+	order[0].x = 1;
+	order[0].y = 4;
+}
+
+void orderTwo(){
+	order[1].x = 3;
+	order[1].y = 2;
+}
+
+void orderThree(){
+	order[2].x = 1;
+	order[2].y = 4;
+}
+
+void orderFour(){
+	order[3].x = 0;
+	order[3].y = 0;
+}
+
+
+void beginLocation(){
+	location.x = 4;
+	location.y = 0;
+	direction = West;
+}
+
+void endDestination(){
+	destination.x = 4;
+	destination.y = 4 ;
+}
+
+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;
+	}
+	
+	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;
+}
+
+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 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;
+	}
+	
+	direction = East;
+}
+
+void locationMessage(){
+	clear();
+	print_long(location.x);
+	print(",");
+	print_long(location.y);
+	delay(200); 
+}
+
+void arrivedMessage(){
+	if (location.x == destination.x && location.y == destination.y){
+		clear();
+		print("ORDER ");
+		print_long(orderNumber);
+		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();
+			}
+			
+			else if(location.x < destination.x){
+				turn_East();
+				gridFollowLine();
+				grid_crossway_detection();
+				location.x++;
+				locationMessage();
+			}
+		}
+	}
+}
+
+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();
+			}
+			
+			else if(location.y < destination.y){
+				turn_North();
+				gridFollowLine();
+				grid_crossway_detection();
+				location.y++;
+				locationMessage();
+			}
+		}
+	}
+}
+
+void 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);
+	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
+			// 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);}
+			}			
+	}
+}
+
+void grid() {
+
+	set_motors(0,0);
+	clear();
+	print("GRID");
+	delay(500);
+
+	//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();
+	}
+	//go to the end of the grid, to transition to charge station
+	endDestination();
+	
+	locationMessage();
+	delay(1000);
+	goToY();
+	goToX();
+	turn_East(); //this was uncommented (6.3)
+	parcourMode = chargeMode;
+}
+
+
+
+// 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();
+	}
+	clear();
+	print("MAZE");
+	
+	
+	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.
+		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){	
+				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();
+		}
+	}
+}
+
+
+int main(){
+	chargedStatus = 0;
+	mazeStatus = 0;
+	
+	parcourMode = mazeMode;
+	
+	mode();
+}
 
-void w2_mode_grid() {}
diff --git a/robot/mode_grid.h b/robot/mode_grid.h
index 55093ad..b13cc4c 100644
--- a/robot/mode_grid.h
+++ b/robot/mode_grid.h
@@ -7,4 +7,87 @@
  *
  * processes orders from the order buffer
  */
+
+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>
+
+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";
+
+// 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 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();
+