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
|
#include "mode_chrg.h"
#include "../shared/bool.h"
#include "hypervisor.h"
#include "io.h"
#include "mode_grid.h"
#include "modes.h"
#include "movement.h"
#include "orangutan_shim.h"
#include <stdio.h>
void w2_short_drive() {
w2_set_motors(50, 50);
delay(150);
w2_set_motors(0, 0);
}
// crosswalk from charging station back to maze
bool w2_charge_cross_walk() {
if (g_w2_transition == 0) {
w2_set_motors(-30, 30);
delay(50);
}
while (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_set_motors(15, 15);
delay(W2_GRID_CROSSWALK_DISTANCE);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
if (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) {
w2_set_motors(0, 0);
g_w2_transition++;
if (g_w2_transition == 3) { // TODO: document g_w2_transition
w2_set_motors(40, 40);
delay(W2_GRID_CROSSWALK_DISTANCE);
w2_set_motors(0, 0);
g_w2_transition = 0;
w2_modes_swap(W2_M_MAZE);
return true;
}
} else {
g_w2_transition = 0;
w2_maze_rotation_full();
return false;
}
}
return false;
}
// main function for charge mode
void w2_mode_chrg() {
static unsigned int last_proportional = 0;
static long integral = 0;
static bool g_w2_chrg_aligned = false;
if (g_w2_chrg_aligned) {
if (g_w2_target_area == W2_AREA_CHRG) return;
delay(200);
w2_maze_rotation_full();
w2_short_drive();
g_w2_chrg_aligned = false;
return;
}
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)g_w2_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 (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) {
bool success = w2_charge_cross_walk();
if (success) return;
}
else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 &&
g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) {
if (g_w2_target_area == W2_AREA_CHRG) {
if (!g_w2_chrg_aligned) {
w2_set_motors(0, 0);
delay_ms(150);
w2_set_motors(30, 30);
delay_ms(600);
w2_set_motors(0, 0);
delay_ms(600);
g_w2_chrg_aligned = true;
}
} else {
w2_maze_rotation_half_left();
}
} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
w2_maze_rotation_half_left();
} else {
if (power_difference < 0 &&
(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
w2_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)) {
w2_set_motors(max, max - power_difference);
}
}
}
|