summaryrefslogtreecommitdiff
path: root/robot
diff options
context:
space:
mode:
Diffstat (limited to 'robot')
-rw-r--r--robot/mode_grid.c818
-rw-r--r--robot/mode_grid.h83
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();
+