summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--robot/mode_grid.c624
1 files changed, 76 insertions, 548 deletions
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index e217c28..23e539e 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,467 +1,3 @@
-#include "mode_grid.h"
-
-/*void w2_mode_grid() {
-
- initialize(); //only keep this when using this module on its own
-
- set_motors(0,0);
-
- //int gridMode = 0; //robot is in the grid
-
- //coordinates of the orders
- orderOne();
- orderTwo();
- orderThree();
- orderFour();
-
- beginLocation();
-
- for (int i = 0; i < 4; i++ ){
-
- destination.x = order[i].x;
- destination.y = order[i].y;
-
- locationMessage();
- delay(1000);
-
- if (location.x != destination.x ){
- while(location.x != destination.x ){
- if (location.x > destination.x){
- turn_West();
- gridFollowLine();
- grid_crossway_detection();
- location.x--;
- locationMessage();
- if (location.x == destination.x){
- arrivedMessage();
- }
- }
-
- else if(location.x < destination.x){
- turn_East();
-
- gridFollowLine();
- grid_crossway_detection();
- location.x++;
- locationMessage();
- if (location.x == destination.x){
- arrivedMessage();
- }
- }
- }
- }
-
- if (location.y != destination.y ){
- while(location.y != destination.y ){
- if (location.y > destination.y){
- turn_South();
- gridFollowLine();
- grid_crossway_detection();
- location.y--;
- locationMessage();
- if (location.y == destination.y){
- arrivedMessage();
- }
- }
-
- else if(location.y < destination.y){
- turn_North();
- gridFollowLine();
- grid_crossway_detection();
- location.y++;
- locationMessage();
- if (location.y == destination.y){
- arrivedMessage();
- }
- }
- }
- }
- }
-}
-
-void full_rotation(){
- delay_ms(500);
- set_motors(60,-60);
- delay_ms(540);
- set_motors(0,0);
- position = read_line(sensors,IR_EMITTERS_ON);
- delay_ms(500);
-}
-
-void grid_rotation_left(){
- delay_ms(500);
- set_motors(-30,30);
- delay_ms(600);
- set_motors(0,0);
- position = read_line(sensors,IR_EMITTERS_ON);
- delay_ms(500);
-}
-
-void grid_rotation_right(){
- delay_ms(500);
- set_motors(30,-30);
- delay_ms(600);
- set_motors(0,0);
- position = read_line(sensors,IR_EMITTERS_ON);
- delay_ms(500);
-}
-
-void grid_crossway_detection(){
- set_motors(0,0);
- set_motors(50,50);
- delay_ms(150); //150
- set_motors(0,0);
- position = read_line(sensors,IR_EMITTERS_ON);
- delay_ms(500);
-}
-
-//variation on "void w2_mode_maze()"
-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){
- 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);}
- }
- }
-}
-
-//coördinates of the orders
-void orderOne(){
- order[0].x = 1; //1
- order[0].y = 3; //0
-}
-
-void orderTwo(){
- order[1].x = 3; //2
- order[1].y = 2; //2
-}
-
-void orderThree(){
- order[2].x = 1; //0
- order[2].y = 4; //4
-}
-
-void orderFour(){
- order[3].x = 0; //3
- order[3].y = 0; //1
-}
-
-//setting coördinate and direction when entering the grid from the maze
-void beginLocation(){
- location.x = 4;
- location.y = 0;
- direction = West;
-}
-
-void turn_North()
-{
- clear();
- print("North");
-
- switch (direction)
- {
- case North:
- break;
-
- case East:
- grid_rotation_left();
- break;
-
- case South:
- full_rotation();
- break;
-
- case West:
- grid_rotation_right();
- break;
- }
- direction = North;
-}
-
-void turn_West()
-{
- clear();
- print("West");
-
- switch (direction)
- {
- case West:
- break;
-
- case North:
- grid_rotation_left();
- break;
-
- case East:
- full_rotation();
- break;
-
- case South:
- grid_rotation_right();;
- break;
- }
- direction = West;
-}
-
-void turn_South()
-{
- clear();
- print("South");
-
- switch (direction)
- {
- case South:
- break;
-
- case West:
- grid_rotation_left();
- break;
-
- case North:
- full_rotation();
- break;
-
- case East:
- grid_rotation_right();;
- break;
- }
- direction = South;
-}
-
-void turn_East()
-{
- clear();
- print("East");
-
- switch (direction)
- {
- case East:
- break;
-
- case South:
- grid_rotation_left();
- break;
-
- case West:
- full_rotation();
- break;
-
- case North:
- grid_rotation_right();;
- break;
- }
- direction = East;
-}
-
-void locationMessage(){
- clear();
- print_long(location.x);
- print(",");
- print_long(location.y);
- delay(200); //1000
-}
-
-void arrivedMessage(){
- clear();
- print("ARRIVED");
-}
-
-//!UNDER THIS CAN ALL BE TAKEN AWAY, THIS IS USED IN MODE_MAZE & Callibration!
-
-// Initializes the 3pi, displays a welcome message, calibrates, and
-// plays the initial music.
-// 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());
-}
-
-// 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);
- }
-}
-
-void full_rotation(){
- set_motors(0,0);
- delay_ms(500);
- set_motors(60,-60);
- delay_ms(540);
- set_motors(0,0);
- position = read_line(sensors,IR_EMITTERS_ON);
- delay_ms(500);
-}*/
-
/*
* 3pi-linefollower-pid - demo code for the Pololu 3pi Robot
*
@@ -489,6 +25,8 @@ int orderNumber;
int transition;
int chargedStatus;
+int mazeStatus;
+
enum section{
mazeMode,
@@ -709,48 +247,29 @@ void half_rotation_left(){
delay_ms(500);
}
-/*
-void 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);
- 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(10,10);
- delay(500);
- print("4");
- //delay(500);
- //set_motors(0,0);
- //print("2");
- //delay(150);
+ 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);
- print("ZEBRA");
+ clear();
+ print("WALK");
transition++;
if(transition == 3){
- set_motors(20,20);
- delay(700);
- parcourMode++ ;
- //grid();
- //transition = 0;
- //break;
+ set_motors(40,40);
+ delay(600);
+ transition = 0;
+ parcourMode = gridMode ;
}
}
+
else{
transition = 0;
full_rotation();
@@ -758,42 +277,60 @@ void cross_walk(){
}
}
+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(0,0);
- //delay_ms(500);
set_motors(60,-60);
delay_ms(540);
set_motors(10,10);
position = read_line(sensors,IR_EMITTERS_ON);
- //delay_ms(500);
-
}
void grid_rotation_left(){
- //delay_ms(500);
set_motors(-30,30);
delay_ms(600);
set_motors(10,10);
position = read_line(sensors,IR_EMITTERS_ON);
- //delay_ms(500);
}
void grid_rotation_right(){
- //delay_ms(500);
set_motors(30,-30);
delay_ms(600);
set_motors(10,10);
position = read_line(sensors,IR_EMITTERS_ON);
- //delay_ms(500);
}
void grid_crossway_detection(){
- //set_motors(0,0);
set_motors(50,50);
delay_ms(150);
set_motors(10,10);
position = read_line(sensors,IR_EMITTERS_ON);
- //delay_ms(500);
}
void gridFollowLine(){
@@ -864,7 +401,7 @@ void shortDrive(){
void orderOne(){
order[0].x = 1;
- order[0].y = 3;
+ order[0].y = 4;
}
void orderTwo(){
@@ -878,10 +415,11 @@ void orderThree(){
}
void orderFour(){
- order[3].x = 2;
- order[3].y = 2;
+ order[3].x = 0;
+ order[3].y = 0;
}
+
void beginLocation(){
location.x = 4;
location.y = 0;
@@ -891,13 +429,10 @@ void beginLocation(){
void endDestination(){
destination.x = 4;
destination.y = 4 ;
- //direction = East;
}
void turn_North()
{
- //clear();
- //print("North");
switch (direction)
{
case North:
@@ -921,9 +456,6 @@ void turn_North()
void turn_West()
{
- //clear();
- //print("West");
-
switch (direction)
{
case West:
@@ -947,8 +479,6 @@ void turn_West()
void turn_South()
{
- //clear();
- //print("South");
switch (direction)
{
case South:
@@ -972,8 +502,6 @@ void turn_South()
void turn_East()
{
- //clear();
- //print("East");
switch (direction)
{
case East:
@@ -1000,7 +528,7 @@ void locationMessage(){
print_long(location.x);
print(",");
print_long(location.y);
- delay(200); //1000
+ delay(200);
}
void arrivedMessage(){
@@ -1008,6 +536,7 @@ void arrivedMessage(){
clear();
print("ORDER ");
print_long(orderNumber);
+ play_frequency(400,500,7);
delay(500);
}
}
@@ -1060,19 +589,25 @@ void home(){
set_motors(0,0);
delay_ms(150);
clear();
- print("HOME");
+ 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
@@ -1109,21 +644,25 @@ void charge(){
if(sensors[0] < 100 && sensors[1] <100 && sensors[2] < 100 && sensors[3] < 100 && sensors[4] < 100){
- cross_walk();
- }
-
- else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){
- half_rotation_left();
+ 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(1000);
+ 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{
@@ -1131,16 +670,12 @@ void charge(){
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() {
- //initialize(); //only keep this when using this module on its own
-
-
set_motors(0,0);
clear();
print("GRID");
@@ -1154,7 +689,7 @@ void grid() {
beginLocation();
- for (int i = 0; i < 1; i++ ){ //CHANGE THIS TO 4 INSTEAD OF 1
+ for (int i = 0; i < 4; i++ ){
orderNumber = i+1;
destination.x = order[i].x;
@@ -1165,19 +700,16 @@ void grid() {
goToX();
goToY();
arrivedMessage();
-
}
-
+ //go to the end of the grid, to transition to charge station
endDestination();
locationMessage();
delay(1000);
goToY();
goToX();
- //turn_East();
- arrivedMessage();
+ turn_East(); //this was uncommented (6.3)
parcourMode = chargeMode;
-
}
@@ -1191,7 +723,12 @@ void maze()
long integral=0;
// set up the 3pi
- initialize();
+ if (mazeStatus == 0){
+ initialize();
+ }
+ clear();
+ print("MAZE");
+
transition = 0;
@@ -1243,9 +780,6 @@ void maze()
else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){
half_rotation_left();
}
- //else if(sensors[4] >= 500 && sensors[3] >= 200 && sensors[0] < 100){
- //half_rotation_right();
- //}
else{
if(power_difference < 0 && (sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100) )
set_motors(max+power_difference, max);
@@ -1258,19 +792,14 @@ void maze()
}
void mode(){
-
while(1){
if(parcourMode == mazeMode){
maze();
}
- if(parcourMode == gridMode){
+ else if(parcourMode == gridMode){
grid();
}
- if(parcourMode == chargeMode){
- set_motors(0,0);
- clear();
- print("CHARGE");
- //delay(1000);
+ else if(parcourMode == chargeMode){
charge();
}
}
@@ -1278,12 +807,11 @@ void mode(){
int main(){
-
chargedStatus = 0;
+ mazeStatus = 0;
parcourMode = mazeMode;
mode();
-
}