diff options
Diffstat (limited to 'robot')
| -rw-r--r-- | robot/mode_maze.c | 141 | ||||
| -rw-r--r-- | robot/mode_maze.h | 4 |
2 files changed, 144 insertions, 1 deletions
diff --git a/robot/mode_maze.c b/robot/mode_maze.c index 6ae62be..9570706 100644 --- a/robot/mode_maze.c +++ b/robot/mode_maze.c @@ -1,3 +1,142 @@ #include "mode_maze.h" +#include "orangutan_shim.h" +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); + -void w2_mode_maze() {} +} + +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 intersection_detection(){ + half_rotation_left(); +} +void w2_mode_maze() { + 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){ + // grid detectie + /*set_motors(0,0); + delay_ms(450); + set_motors(50,50); + delay_ms(180); + if ( sensors[2] >= 100 || sensors[3] >= 100 || sensors[1] >= 100 || sensors[0] >= 100 || sensors[4] >= 100) + { + set_motors(0,0); + delay_ms(15000); + set_motors(50,50); + delay_ms(180); + if (sensors[2] >= 100 || sensors[3] >= 100 || sensors[1] >= 100 || sensors[0] >= 100 || sensors[4] >= 100 ) + { + set_motors(0,0); + delay_ms(1500); + set_motors(50,50); + delay_ms(180); + if (sensors[2] >= 100 || sensors[3] >= 100 || sensors[1] >= 100 || sensors[0] >= 100 || sensors[4] >= 100) + { + print("GRID!"); + set_motors(0,0); + delay_ms(10000); + + } + } + + } + else if(sensors[0] < 100 && sensors[1] <100 && sensors[2] < 100 && sensors[3] < 100 && sensors[4] < 100){*/ + 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[2] < 50 &&sensors[4] >= 500){ + //intersection_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); + + } + + } + + // This part of the code is never reached. A robot should + // never reach the end of its program, or unpredictable behavior + // will result as random code starts getting executed. If you + // really want to stop all actions at some point, set your motors + // to 0,0 and run the following command to loop forever: + // + // while(1); +} +} diff --git a/robot/mode_maze.h b/robot/mode_maze.h index 9fbeb8c..b97ad5c 100644 --- a/robot/mode_maze.h +++ b/robot/mode_maze.h @@ -5,4 +5,8 @@ * * finds route out of maze */ +unsigned int sensors[5]; // an array to hold sensor values +unsigned int position; +unsigned int last_proportional=0; +long integral=0; void w2_mode_maze(); |