Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / a573df154b900b6618d0b46f8d2635a8dba1082f / . / frc971 / control_loops / python / spline_drawing.py
blob: 4e8b04b7da913597402d4f67599ea97d7cb75382 [file] [log] [blame]
John Park91e69732019-03-03 13:12:43 -0800[diff] [blame]1from color import Color, palette
2import cairo
3
4import numpy as np
5from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
6WIDTH_OF_FIELD_IN_METERS = 8.258302
7PIXELS_ON_SCREEN = 800
8
9X_TRANSLATE = 0
10Y_TRANSLATE = 0
11
12
13def pxToM(p):
14 return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
15
16
17def mToPx(m):
18 return (m * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
19
20
21def px(cr):
22 return OverrideMatrix(cr, identity)
23
24
25def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
26 """Draws a cross with fixed dimensions in pixel space."""
27 set_color(cr, color)
28 cr.move_to(x, y - length_px)
29 cr.line_to(x, y + length_px)
30 cr.stroke()
31
32 cr.move_to(x - length_px, y)
33 cr.line_to(x + length_px, y)
34 cr.stroke()
35 set_color(cr, palette["LIGHT_GREY"])
36
37
38def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
39 """Draws a x with fixed dimensions in pixel space."""
40 length_px = length_px1 / np.sqrt(2)
41 set_color(cr, color)
42 cr.move_to(x - length_px, y - length_px)
43 cr.line_to(x + length_px, y + length_px)
44 cr.stroke()
45
46 cr.move_to(x - length_px, y + length_px)
47 cr.line_to(x + length_px, y - length_px)
48 cr.stroke()
49 set_color(cr, palette["LIGHT_GREY"])
50
51
52def display_text(cr, text, widtha, heighta, widthb, heightb):
53 cr.scale(widtha, -heighta)
54 cr.show_text(text)
55 cr.scale(widthb, -heightb)
56
57
58def redraw(needs_redraw, window):
59 print("Redrew")
60 if not needs_redraw:
61 window.queue_draw()
62
63
64def draw_HAB(cr):
65 print("WENT IN")
66 # BASE Constants
67 X_BASE = 0 + X_TRANSLATE #(2.41568)
68 Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
69 BACKWALL_X = X_TRANSLATE
70 LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
71 # HAB Levels 2 and 3 called in variables backhab
72 # draw loading stations
73 cr.move_to(0, LOADING_Y)
74 cr.line_to(mToPx(0.6), LOADING_Y)
75 cr.move_to(0, -1.0 * LOADING_Y)
76 cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
77
78 BACKWALL_X = X_TRANSLATE
79
80 # HAB Levels 2 and 3 called in variables backhab
81
82 WIDTH_BACKHAB = mToPx(1.2192)
83
84 Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
85 BACKHAB_LV2_LENGTH = mToPx(1.016)
86
87 BACKHAB_LV3_LENGTH = mToPx(1.2192)
88 Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
89
90 Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
91
92 # HAB LEVEL 1
93 X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
94
95 WIDTH_LV1_BOX = mToPx(0.90805)
96 LENGTH_LV1_BOX = mToPx(1.6256)
97
98 Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
99
100 # Ramp off Level 1
101 X_RAMP = X_LV1_BOX
102
103 Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
104 WIDTH_TOP_RAMP = mToPx(1.20015)
105 LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
106
107 X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
108 Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
109 LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
110 WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
111
112 Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
113
114 # Side Bars to Hold in balls
115 X_BARS = BACKWALL_X
116 WIDTH_BARS = WIDTH_BACKHAB
117 LENGTH_BARS = mToPx(0.574675)
118
119 Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
120
121 Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
122
123 set_color(cr, palette["BLACK"])
124 cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
125 BACKHAB_LV2_LENGTH)
126 cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
127 cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
128 BACKHAB_LV2_LENGTH)
129 cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
130 cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
131 cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
132 cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
133 LENGTH_MIDDLE_RAMP)
134 cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
135 cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
136 cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
137 cr.stroke()
138
139 cr.set_line_join(cairo.LINE_JOIN_ROUND)
140
141 cr.stroke()
142
143 #draw 0, 0
144 set_color(cr, palette["BLACK"])
145 cr.move_to(X_TRANSLATE, Y_TRANSLATE)
146 cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(8.2296 / 2.0))
147 cr.move_to(X_TRANSLATE, Y_TRANSLATE)
148 cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(-8.2296 / 2.0))
149 cr.move_to(X_TRANSLATE, Y_TRANSLATE)
150 cr.line_to(X_TRANSLATE + mToPx(8.2296), Y_TRANSLATE)
151
152 cr.stroke()
153
154
155def draw_rockets(cr):
156 # BASE Constants
157 X_BASE = 0 + X_TRANSLATE + mToPx(2.41568)
158 Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
159
160 near_side_rocket_center = [
161 X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
162 (3.86305 + 3.39548) / 2.0)
163 ]
164 middle_rocket_center = [
165 X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
166 (3.39548 + 3.392380) / 2.0)
167 ]
168 far_side_rocket_center = [
169 X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
170 (3.39238 + 3.86305) / 2.0)
171 ]
172
173 cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
174 cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
175 near_side_rocket_center[1] - 0.4 * mToPx(0.5))
176
177 cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
178 cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.4))
179
180 cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
181 cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
182 far_side_rocket_center[1] - 0.4 * mToPx(0.5))
183
184 print("FAR SIDE ROCKET")
185 #print(far_side_rocket_center)
186 near_side_rocket_center = [
187 X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
188 (3.86305 + 3.39548) / 2.0)
189 ]
190 middle_rocket_center = [
191 X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
192 (3.39548 + 3.392380) / 2.0)
193 ]
194 far_side_rocket_center = [
195 X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
196 (3.39238 + 3.86305) / 2.0)
197 ]
198
199 cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
200 cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
201 near_side_rocket_center[1] + 0.4 * mToPx(0.5))
202
203 cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
204 cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.4))
205
206 cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
207 cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
208 far_side_rocket_center[1] + 0.4 * mToPx(0.5))
209
210 X_BASE = 0 + X_TRANSLATE #mToPx(2.41568)
211 Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
212
213 # Leftmost Line
214 cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
215 cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
216
217 # Top Line
218 cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
219 cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
220
221 #Rightmost Line
222 cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
223 cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
224
225 #Back Line
226 cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
227 cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
228
229 # Leftmost Line
230 cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
231 cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
232
233 # Top Line
234 cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
235 cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
236
237 #Rightmost Line
238 cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
239 cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
240
241 #Back Line
242 cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
243 cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
244
245 cr.stroke()
246
247
248def draw_cargo_ship(cr):
249 # BASE Constants
250
251 X_BASE = X_TRANSLATE + mToPx(5.59435)
252 Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
253
254 FRONT_PEG_DELTA_Y = mToPx(0.276352)
255 cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
256 cr.line_to(X_BASE - mToPx(0.4), Y_BASE + FRONT_PEG_DELTA_Y)
257
258 cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
259 cr.line_to(X_BASE - mToPx(0.4), Y_BASE - FRONT_PEG_DELTA_Y)
260
261 SIDE_PEG_Y = mToPx(1.41605 / 2.0)
262 SIDE_PEG_X = X_BASE + mToPx(1.148842)
263 SIDE_PEG_DX = mToPx(0.55245)
264
265 cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
266 cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.4))
267
268 cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
269 cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
270
271 cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
272 cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
273
274 cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
275 cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.4))
276
277 cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
278 cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
279
280 cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
281 cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
282
283 cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
284 mToPx(1.41605))
285
286 X_BASE = X_TRANSLATE + mToPx(2.41568)
287 Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
288
289 cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
290 mToPx(2.43205), mToPx(1.41605))
291
292 cr.stroke()