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
|
#include "movement.h"
#include "io.h"
#include "orangutan_shim.h"
unsigned int g_w2_sensors[5] = {0}; // IR sensors on the bottom of the robot
unsigned int g_w2_position = 0; // position on the black line
// full rotation maze/charge
void w2_maze_rotation_full() {
w2_set_motors(0, 0);
delay_ms(500);
w2_set_motors(60, -60);
delay_ms(540);
w2_set_motors(0, 0);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
}
// left turn maze/charge
void w2_maze_rotation_half_left() {
w2_set_motors(0, 0);
w2_set_motors(50, 50);
delay_ms(150);
w2_set_motors(-30, 30);
delay_ms(600);
w2_set_motors(0, 0);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
}
// right turn maze/charge
void w2_maze_rotation_half_right() {
w2_set_motors(0, 0);
w2_set_motors(50, 50);
delay_ms(150);
w2_set_motors(30, -30);
delay_ms(600);
w2_set_motors(0, 0);
w2_set_motors(50, 50);
delay_ms(150);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
}
// 180 turn in grid
void w2_grid_rotation_full() {
w2_set_motors(60, -60);
delay_ms(540);
w2_set_motors(10, 10);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
}
// left turn in grid
void w2_grid_rotation_half_left() {
w2_set_motors(-30, 30);
delay_ms(600);
w2_set_motors(10, 10);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
}
// right turn in grid
void w2_grid_rotation_half_right() {
w2_set_motors(30, -30);
delay_ms(600);
w2_set_motors(10, 10);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
}
|