From 17788ee4210709e28f0f4839eb972afd6c184a43 Mon Sep 17 00:00:00 2001 From: nonailla Date: Mon, 6 Jun 2022 22:45:30 +0200 Subject: All files. Super messy, works until it goes back. --- robot/mode_grid.c | 828 +++++++++++++++++++++++++++++++++++++++++++++++++++++- robot/mode_grid.h | 19 +- 2 files changed, 837 insertions(+), 10 deletions(-) diff --git a/robot/mode_grid.c b/robot/mode_grid.c index e36162e..e217c28 100644 --- a/robot/mode_grid.c +++ b/robot/mode_grid.c @@ -1,6 +1,6 @@ #include "mode_grid.h" -void w2_mode_grid() { +/*void w2_mode_grid() { initialize(); //only keep this when using this module on its own @@ -460,4 +460,830 @@ void full_rotation(){ set_motors(0,0); position = read_line(sensors,IR_EMITTERS_ON); delay_ms(500); +}*/ + +/* + * 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 + +// 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 + +unsigned int sensors[5]; // an array to hold sensor values +unsigned int position; +int orderNumber; +int transition; +int chargedStatus; + + +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 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); + position = read_line(sensors,IR_EMITTERS_ON); + if(sensors[2] > 100 || sensors[3] > 100 || sensors[1] > 100){ + set_motors(0,0); + print("ZEBRA"); + transition++; + if(transition == 3){ + set_motors(20,20); + delay(700); + parcourMode++ ; + //grid(); + //transition = 0; + //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(){ + 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 = 3; } + +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 = 2; + order[3].y = 2; +} + +void beginLocation(){ + location.x = 4; + location.y = 0; + direction = West; +} + +void endDestination(){ + destination.x = 4; + destination.y = 4 ; + //direction = East; +} + +void turn_North() +{ + //clear(); + //print("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() +{ + //clear(); + //print("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() +{ + //clear(); + //print("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() +{ + //clear(); + //print("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); //1000 +} + +void arrivedMessage(){ + if (location.x == destination.x && location.y == destination.y){ + clear(); + print("ORDER "); + print_long(orderNumber); + 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("HOME"); + set_motors(30,30); + delay_ms(600); + set_motors(0,0); + delay_ms(600); + position = read_line(sensors,IR_EMITTERS_ON); + chargedStatus = 1; + delay_ms(2000); +} + +void charge(){ + unsigned int last_proportional=0; + long integral=0; + + 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(); + } + + else if (sensors[0] >= 500 && sensors[1] >= 200 && sensors[4] < 100){ + half_rotation_left(); + } + + else if((sensors[0] >= 500 && sensors[1] >= 500 && sensors[2] >= 500 && sensors[3] >= 500 &&sensors[4] >= 500) && chargedStatus == 0){ + home(); + delay(1000); + full_rotation(); + shortDrive(); + } + + else if(sensors[0] >= 500 && sensors[1] >= 250 && sensors[2] >= 500 && sensors[3] >= 250 &&sensors[4] >= 500){ + 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() { + + //initialize(); //only keep this when using this module on its own + + + set_motors(0,0); + clear(); + print("GRID"); + delay(500); + + //coordinates of the orders + orderOne(); + orderTwo(); + orderThree(); + orderFour(); + + beginLocation(); + + for (int i = 0; i < 1; i++ ){ //CHANGE THIS TO 4 INSTEAD OF 1 + orderNumber = i+1; + + destination.x = order[i].x; + destination.y = order[i].y; + + locationMessage(); + delay(1000); + goToX(); + goToY(); + arrivedMessage(); + + } + + endDestination(); + + locationMessage(); + delay(1000); + goToY(); + goToX(); + //turn_East(); + arrivedMessage(); + 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 + initialize(); + + 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(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); + 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(); + } + if(parcourMode == gridMode){ + grid(); + } + if(parcourMode == chargeMode){ + set_motors(0,0); + clear(); + print("CHARGE"); + //delay(1000); + charge(); + } + } +} + + +int main(){ + + chargedStatus = 0; + + parcourMode = mazeMode; + + mode(); + +} + diff --git a/robot/mode_grid.h b/robot/mode_grid.h index 42c2c80..b13cc4c 100644 --- a/robot/mode_grid.h +++ b/robot/mode_grid.h @@ -73,6 +73,7 @@ const char levels[] PROGMEM = { }; void w2_mode_grid(); +void initialize(); void full_rotation(); void grid_rotation_left(); void grid_rotation_right(); @@ -80,13 +81,13 @@ 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() +void orderThree(); +void orderFour(); +void beginLocation(); +void turn_North(); +void turn_West(); +void turn_South(); +void turn_East(); +void locationMessage(); +void arrivedMessage(); -- cgit v1.2.3