-
Notifications
You must be signed in to change notification settings - Fork 1
/
Laser.gd
178 lines (152 loc) · 4.87 KB
/
Laser.gd
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
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
tool
extends Node
const SERCOMM = preload("res://addons/GDSerCommDock/bin/GDSerComm.gdns")
var PORT
onready var com = $Com
var maze_measurements = {
"maze_1": {
"1": pipe_bottom_adjustment([[0, 6], [0, 367]]),
"2": pipe_bottom_adjustment([[14, 9], [672, 223]])
},
"maze_2": {
"1": pipe_bottom_adjustment([[2, 0],[96, 702]]),
"2": pipe_bottom_adjustment([[14, 8],[672, 318]])
},
"maze_3": {
"1": pipe_bottom_adjustment([[0, 11], [0, 172]]),
"2": pipe_bottom_adjustment([[12, 0], [582, 701]])
},
"maze_4": {
"1": pipe_bottom_adjustment([[14, 0], [0, 28]]),
"2": pipe_bottom_adjustment([[9, 4], [483, 508]])
},
"maze_5": {
"1": pipe_bottom_adjustment([[0, 0], [6, 702]]),
"2": pipe_bottom_adjustment([[14, 12], [677, 126]])
}
}
var params
func pipe_bottom_adjustment(mapping):
# account for mouse sitting at the bottom of the pipe
return [
[mapping[0][0], mapping[0][1]],
[mapping[1][0]+25, mapping[1][1]],
]
func get_params(mapping_1, mapping_2):
var dx_excel = mapping_2[0][0] - mapping_1[0][0]
var dx_godot = mapping_2[1][0] - mapping_1[1][0]
var dy_excel = mapping_2[0][1] - mapping_1[0][1]
var dy_godot = mapping_2[1][1] - mapping_1[1][1]
var godot_x_multiplier = dx_godot/dx_excel
var godot_y_multiplier = dy_godot/dy_excel
var godot_x1_simulated = godot_x_multiplier * mapping_1[0][0]
var godot_y1_simulated = godot_y_multiplier * mapping_1[0][1]
var godot_x1_diversion = godot_x1_simulated - mapping_1[1][0]
var godot_y1_diversion = godot_y1_simulated - mapping_1[1][1]
return {
"x_multiplier": godot_x_multiplier,
"y_multiplier": godot_y_multiplier,
"x_constant": -godot_x1_diversion,
"y_constant": -godot_y1_diversion
}
func convert_x_from_excel(x, x_multiplier, x_constant):
return x_multiplier * x + x_constant
func convert_y_from_excel(y, y_multiplier, y_constant):
return y_multiplier * y + y_constant
func convert_x_from_godot(x, x_multiplier, x_constant):
return (x - x_constant) / x_multiplier
func convert_y_from_godot(y, y_multiplier, y_constant):
return (y - y_constant) / y_multiplier
func generate_route_progresses(map_idx, x, y):
var y_current = y_start[map_idx]
var x_current = x_start[map_idx]
var map = maps[map_idx]
var progresses = []
for i in len(map):
pass
progresses.append([
i,
[x_current, y_current],
[x, y]
])
var dir = map[i]
if dir == 'u':
y_current += 1
if dir == 'd':
y_current -= 1
if dir == 'l':
x_current -= 1
if dir == 'r':
x_current += 1
return progresses
func find_closest_progress(map_idx, x, y):
var progresses = generate_route_progresses(map_idx, x, y)
progresses.sort_custom(self, "dist_comparison")
return progresses[0]
func get_progress_dist(a):
var dx_a = a[1][0] - a[2][0]
var dy_a = a[1][1] - a[2][1]
return sqrt(pow(dx_a, 2) + pow(dy_a, 2))
func dist_comparison(a, b):
var dist_a = get_progress_dist(a)
var dist_b = get_progress_dist(b)
if dist_a < dist_b:
return true
else:
return false
# WARNING: this has to be the same as in Arduino repo
var x_start = [0, 2, 0, 0, 0];
var y_start = [6, 0, 12, 14, 0];
var maps = [
"rrrrrrrrrrruuurrrr",
"rrrrruuuuurrrrruuurrr",
"rrrrrrdddlllddddrrrrrruuuuurrrddddddddd",
"rddrruurrrrrrdldlulldddrrrrrurrdddlldllululllddrrddllluuuldddddluuuuuldddddddrrrrruurrddrrrrruuulll",
"ruurdrruuuuluuurrdruullllluuurrdruuurddddrrdddrddddrurddruuruuuuruuullldldrrdllluuuuurrrrdrr",
]
var last_change = OS.get_ticks_msec()
func get_map_idx():
var name = get_tree().get_current_scene().get_name()
if name == "Maze1":
return 0
elif name == "Maze2":
return 1
elif name == "Maze3":
return 2
elif name == "Maze4":
return 3
elif name == "Maze5":
return 4
func plugin_installed():
var directory = Directory.new();
return directory.file_exists("res://addons/GDSerCommDock/bin/libGDSercomm.dylib")
func _ready():
if plugin_installed():
PORT = SERCOMM.new()
PORT.open(
"/dev/cu.usbserial-0001",
9600,
1000,
com.bytesz.SER_BYTESZ_8,
com.parity.SER_PAR_NONE,
com.stopbyte.SER_STOPB_ONE)
PORT.flush()
laser_update()
func laser_set_map(map_idx, player_progress):
if plugin_installed():
var line = "%d,%d\n" % [map_idx, player_progress]
PORT.write(line)
PORT.flush()
func laser_update():
if OS.get_ticks_msec() - last_change > 150:
var map_idx = get_map_idx()
var pos = get_node("/root/Maze%d/player" % [map_idx+1]).get_position()
var measurements = maze_measurements["maze_%d" % [map_idx+1]]
var params = get_params(measurements["1"], measurements["2"])
var x = convert_x_from_godot(pos[0], params["x_multiplier"], params["x_constant"])
var y = convert_y_from_godot(pos[1], params["y_multiplier"], params["y_constant"])
var closest_progress = find_closest_progress(map_idx, x, y)
var dist = get_progress_dist(closest_progress)
print("pos: (%d, %d) (%d, %d) %f" % [x, y, pos[0], pos[1], dist])
laser_set_map(map_idx, closest_progress[0] if dist < 1.2 else 255)
last_change = OS.get_ticks_msec()