summaryrefslogtreecommitdiff
path: root/robot
diff options
context:
space:
mode:
Diffstat (limited to 'robot')
-rw-r--r--robot/mode_maze.c205
-rw-r--r--robot/mode_maze.h4
2 files changed, 98 insertions, 111 deletions
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
index 9570706..a178d3a 100644
--- a/robot/mode_maze.c
+++ b/robot/mode_maze.c
@@ -1,142 +1,133 @@
#include "mode_maze.h"
#include "orangutan_shim.h"
-void full_rotation(){
- set_motors(0,0);
+
+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);
+ set_motors(60, -60);
delay_ms(540);
- set_motors(0,0);
- position = read_line(sensors,IR_EMITTERS_ON);
+ set_motors(0, 0);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
-
-
}
-void half_rotation_left(){
- set_motors(0,0);
- set_motors(50,50);
+void w2_half_rotation_left() {
+ set_motors(0, 0);
+ set_motors(50, 50);
delay_ms(150);
- set_motors(-30,30);
+ set_motors(-30, 30);
delay_ms(600);
- set_motors(0,0);
- position = read_line(sensors,IR_EMITTERS_ON);
+ set_motors(0, 0);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
-
}
-void half_rotation_right(){
- set_motors(0,0);
- set_motors(50,50);
+void w2_half_rotation_right() {
+ set_motors(0, 0);
+ set_motors(50, 50);
delay_ms(150);
- set_motors(30,-30);
+ set_motors(30, -30);
delay_ms(600);
- set_motors(0,0);
- set_motors(50,50);
+ set_motors(0, 0);
+ set_motors(50, 50);
delay_ms(150);
- position = read_line(sensors,IR_EMITTERS_ON);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
}
-void crossway_detection(){
- half_rotation_left();
-}
-void intersection_detection(){
- half_rotation_left();
-}
+void w2_crossway_detection() { w2_half_rotation_left(); }
+void w2_intersection_detection() { w2_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);
+ // 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)position) - 2000;
+ // 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;
+ // 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.
- last_proportional = 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 + integral/10000 + derivative*3/2;
+ // 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;
-
- 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);
+ // 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) {
+ // grid detectie
+ /*set_motors(0,0);
+ delay_ms(450);
+ set_motors(50,50);
+ delay_ms(180);
+ if ( g_w2_sensors[2] >= 100 || g_w2_sensors[3] >= 100 || g_w2_sensors[1] >= 100 ||
+ g_w2_sensors[0] >= 100 || g_w2_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)
+ if (g_w2_sensors[2] >= 100 || g_w2_sensors[3] >= 100 || g_w2_sensors[1] >= 100 ||
+ g_w2_sensors[0] >= 100
+ || g_w2_sensors[4] >= 100 )
{
- set_motors(0,0);
- delay_ms(15000);
+ 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 )
+ if (g_w2_sensors[2] >= 100 || g_w2_sensors[3] >= 100 || g_w2_sensors[1] >= 100 ||
+ g_w2_sensors[0] >= 100 || g_w2_sensors[4] >= 100)
{
+ print("GRID!");
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);
-
- }
+ 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(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_full_rotation();
//}
- 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);
-}
+ } 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[2] < 50 &&g_w2_sensors[4] >= 500){
+ // intersection_detection();
+ //}
+ else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+ w2_half_rotation_left();
+ }
+ // else if(g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[0] < 100){
+ // half_rotation_right();
+ //}
+ 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_maze.h b/robot/mode_maze.h
index 6d481c1..e7490b9 100644
--- a/robot/mode_maze.h
+++ b/robot/mode_maze.h
@@ -7,8 +7,4 @@
*
* 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();