aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--robot/mode_chrg.c133
-rw-r--r--robot/mode_grid.c943
-rw-r--r--robot/mode_grid.h92
-rw-r--r--robot/mode_maze.c58
-rw-r--r--robot/mode_scal.c1
-rw-r--r--robot/movement.c38
-rw-r--r--robot/movement.h9
-rw-r--r--robot/setup.c3
-rw-r--r--robot/transition.c8
-rw-r--r--robot/transition.h6
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();