blob: 04a3bc207a667f89bb19a87fc8a1235c148c2416 (
plain)
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
|
#include "mode_maze.h"
#include "orangutan_shim.h"
#include "movement.h"
#include "transition.h"
unsigned int g_w2_last_proportional = 0;
long g_w2_integral = 0;
void w2_mode_maze() {
// PID controller
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
int proportional = ((int)g_w2_position) - 2000;
int derivative = proportional - g_w2_last_proportional;
g_w2_integral += proportional;
g_w2_last_proportional = proportional;
int power_difference = proportional / 20 + g_w2_integral / 10000 + derivative * 3 / 2;
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 (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(1500);
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)
{
print("GRID!");
set_motors(0,0);
delay_ms(10000);
}
}
}
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 (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);
}
}
|