1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
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 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);
}
}
|