Merge "Update origin location on Spline GUI"
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index e0b4878..4a74b4a 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -1,41 +1,39 @@
import argparse
arg_parser = argparse.ArgumentParser(description='spline_editor')
-arg_parser.add_argument(
- 'size',
- metavar='N',
- default=800,
- type=int,
- nargs='?',
- help="size of the screen")
+arg_parser.add_argument('size',
+ metavar='N',
+ default=800,
+ type=int,
+ nargs='?',
+ help="size of the screen")
args = arg_parser.parse_args()
SCREEN_SIZE = args.size
WIDTH_OF_ROBOT = 0.65
LENGTH_OF_ROBOT = 0.8
-ROBOT_SIDE_TO_BALL_CENTER = 0.15 # Placeholder value
+ROBOT_SIDE_TO_BALL_CENTER = 0.15 # Placeholder value
BALL_RADIUS = 0.165
-ROBOT_SIDE_TO_HATCH_PANEL = 0.1 # Placeholder value
+ROBOT_SIDE_TO_HATCH_PANEL = 0.1 # Placeholder value
HATCH_PANEL_WIDTH = 0.4826
FIELD = 2020
if FIELD == 2019:
- WIDTH_OF_FIELD_IN_METERS = 8.258302 # Half Field
+ WIDTH_OF_FIELD_IN_METERS = 8.258302 # Half Field
elif FIELD == 2020:
- WIDTH_OF_FIELD_IN_METERS = 15.98295 # Full Field
- LENGTH_OF_FIELD_IN_METERS = 8.21055 # Full Field
+ WIDTH_OF_FIELD_IN_METERS = 15.98295 # Full Field
+ LENGTH_OF_FIELD_IN_METERS = 8.21055 # Full Field
-def pxToM(p, length = False):
- if(length):
- return p * LENGTH_OF_FIELD_IN_METERS / (SCREEN_SIZE/2)
+
+def pxToM(p):
return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
-def mToPx(m, length = False):
- if(length):
- return (m*(SCREEN_SIZE/2)/LENGTH_OF_FIELD_IN_METERS)
- return (m*SCREEN_SIZE/WIDTH_OF_FIELD_IN_METERS)
+
+def mToPx(m):
+ return (m * SCREEN_SIZE / WIDTH_OF_FIELD_IN_METERS)
+
def inToM(i):
- return (i*0.0254)
+ return (i * 0.0254)
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index a2ddf5d..332f37e6 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -3,6 +3,7 @@
from constants import *
import numpy as np
+
def set_color(cr, color, a=1):
if color.a == 1.0:
cr.set_source_rgba(color.r, color.g, color.b, a)
@@ -36,12 +37,14 @@
cr.stroke()
set_color(cr, palette["WHITE"])
-def draw_circle(cr, x, y, radius, color=palette["RED"]):
+
+def draw_circle(cr, x, y, radius, color=palette["RED"]):
set_color(cr, color)
- cr.arc(x, y, radius, 0, 2*np.pi)
+ cr.arc(x, y, radius, 0, 2 * np.pi)
cr.fill()
cr.stroke()
+
def draw_control_points(cr, points, width=10, radius=4, color=palette["BLUE"]):
for i in range(0, len(points)):
draw_px_x(cr, points[i][0], points[i][1], width, color)
@@ -50,25 +53,31 @@
cr.fill()
set_color(cr, palette["WHITE"])
+
def display_text(cr, text, widtha, heighta, widthb, heightb):
cr.scale(widtha, -heighta)
cr.show_text(text)
cr.scale(widthb, -heightb)
+
def markers(cr):
SHOW_MARKERS = False
if SHOW_MARKERS:
# Shield Generator Reference
color = palette["BLUE"]
- yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+ yorigin = 0 - SCREEN_SIZE / 2 # Move origin to bottom left
# Top Left
- draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True), 10, color)
+ draw_circle(cr, mToPx(inToM(206.625)),
+ yorigin + mToPx(inToM(212.097), True), 10, color)
# Top Right
- draw_circle(cr, mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True), 10, color)
+ draw_circle(cr, mToPx(inToM(373.616)),
+ yorigin + mToPx(inToM(281.26), True), 10, color)
# Bottom Right
- draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True), 10, color)
+ draw_circle(cr, mToPx(inToM(422.625)),
+ yorigin + mToPx(inToM(124.67), True), 10, color)
# Bottom Left
- draw_circle(cr, mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True), 10, color)
+ draw_circle(cr, mToPx(inToM(255.634)),
+ yorigin + mToPx(inToM(55.5), True), 10, color)
# Trench Run Reference
color = palette["GREEN"]
@@ -76,106 +85,101 @@
# Bottom Right
draw_circle(cr, mToPx(inToM(206.625)), yorigin, 10, color)
# Top Right
- draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True), 10, color)
+ draw_circle(cr, mToPx(inToM(206.625)),
+ yorigin + mToPx(inToM(55.5), True), 10, color)
# Top Left
- draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True), 10, color)
+ draw_circle(cr, mToPx(inToM(422.625)),
+ yorigin + mToPx(inToM(55.5), True), 10, color)
# Bottom Left
draw_circle(cr, mToPx(inToM(422.625)), yorigin, 10, color)
# Top Trench Run
# Top Right
- draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(323.25), True), 10, color)
+ draw_circle(cr, mToPx(inToM(206.625)),
+ yorigin + mToPx(inToM(323.25), True), 10, color)
# Bottom Right
- draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True), 10, color)
+ draw_circle(cr, mToPx(inToM(206.625)),
+ yorigin + mToPx(inToM(281.26), True), 10, color)
# Top Left
- draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True), 10, color)
+ draw_circle(cr, mToPx(inToM(422.625)),
+ yorigin + mToPx(inToM(281.26), True), 10, color)
# Bottom Left
- draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(323.25), True), 10, color)
+ draw_circle(cr, mToPx(inToM(422.625)),
+ yorigin + mToPx(inToM(323.25), True), 10, color)
cr.stroke()
-def draw_init_lines(cr):
- yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
- set_color(cr, palette["RED"])
- cr.move_to(mToPx(inToM(120)), yorigin)
- cr.line_to(mToPx(inToM(120)), yorigin + mToPx(8.21055, True))
- cr.move_to(mToPx(inToM(505.25)), yorigin)
- cr.line_to(mToPx(inToM(505.25)), yorigin + mToPx(8.21055, True))
+def draw_init_lines(cr):
+ set_color(cr, palette["RED"])
+ init_line_x = WIDTH_OF_FIELD_IN_METERS / 2.0 - inToM(120)
+ init_start_y = -LENGTH_OF_FIELD_IN_METERS / 2.0
+ init_end_y = LENGTH_OF_FIELD_IN_METERS / 2.0
+ cr.move_to(mToPx(init_line_x), mToPx(init_start_y))
+ cr.line_to(mToPx(init_line_x), mToPx(init_end_y))
+
+ cr.move_to(mToPx(-init_line_x), mToPx(init_start_y))
+ cr.line_to(mToPx(-init_line_x), mToPx(init_end_y))
cr.stroke()
+
def draw_trench_run(cr):
- set_color(cr, palette["GREEN"])
- yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
- cr.move_to(mToPx(inToM(206.625)), yorigin)
- cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True))
+ edge_of_field_y = LENGTH_OF_FIELD_IN_METERS / 2.0
+ edge_of_trench_y = edge_of_field_y - inToM(55.5)
+ trench_start_x = inToM(-108.0)
+ trench_length_x = inToM(216.0)
+ ball_line_y = edge_of_field_y - inToM(27.75)
+ ball_one_x = -inToM(72)
+ ball_two_x = -inToM(36)
+ ball_three_x = 0.0
+ # The fourth/fifth balls are referenced off of the init line...
+ ball_fourfive_x = WIDTH_OF_FIELD_IN_METERS / 2.0 - inToM(120.0 + 130.36)
- cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True))
- cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True))
-
- cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True))
- cr.line_to(mToPx(inToM(422.625)), yorigin)
-
- cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(323.25), True))
- cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True))
-
- cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True))
- cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True))
-
- cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True))
- cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(323.25), True))
+ for sign in [1.0, -1.0]:
+ set_color(cr, palette["GREEN"])
+ cr.rectangle(mToPx(sign * trench_start_x),
+ mToPx(sign * edge_of_field_y),
+ mToPx(sign * trench_length_x),
+ mToPx(sign * (edge_of_trench_y - edge_of_field_y)))
+ cr.stroke()
+ draw_circle(cr, mToPx(sign * ball_one_x), mToPx(sign * ball_line_y),
+ mToPx(0.1), palette["YELLOW"])
+ draw_circle(cr, mToPx(sign * ball_two_x), mToPx(sign * ball_line_y),
+ mToPx(0.1), palette["YELLOW"])
+ draw_circle(cr, mToPx(sign * ball_three_x), mToPx(sign * ball_line_y),
+ mToPx(0.1), palette["YELLOW"])
cr.stroke()
+
def draw_shield_generator(cr):
set_color(cr, palette["BLUE"])
- yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
-
- cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True))
- cr.line_to(mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True))
-
- cr.move_to(mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True))
- cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True))
-
- cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True))
- cr.line_to(mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True))
-
- cr.move_to(mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True))
- cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True))
+ cr.save()
+ cr.rotate(22.5 * np.pi / 180.)
+ generator_width = mToPx(inToM(14 * 12 + 0.75))
+ generator_height = mToPx(inToM(13 * 12 + 1.5))
+ cr.rectangle(-generator_width / 2.0, -generator_height / 2.0,
+ generator_width, generator_height)
+ cr.restore()
cr.stroke()
-def draw_control_panel(cr): # Base plates are not included
+
+def draw_control_panel(cr): # Base plates are not included
set_color(cr, palette["LIGHT_GREY"])
- yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
- # Bottom Control Panel
- # Top Line
- cr.move_to(mToPx(inToM(225.624)), yorigin + mToPx(inToM(55.5), True))
- cr.line_to(mToPx(inToM(285.624)), yorigin + mToPx(inToM(55.5), True))
-
- # Left Line
- cr.move_to(mToPx(inToM(225.624)), yorigin + mToPx(inToM(55.5), True))
- cr.line_to(mToPx(inToM(225.624)), yorigin)
-
- # Right Line
- cr.move_to(mToPx(inToM(285.624)), yorigin + mToPx(inToM(55.5), True))
- cr.line_to(mToPx(inToM(285.624)), yorigin)
-
- # Top Control Panel
- # Bottom Line
- cr.move_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(281.26), True))
- cr.line_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(281.26), True))
-
- # Right Line
- cr.move_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(281.26), True))
- cr.line_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(323.25), True))
-
- # Left Line
- cr.move_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(281.26), True))
- cr.line_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(323.25), True))
+ edge_of_field_y = LENGTH_OF_FIELD_IN_METERS / 2.0
+ edge_of_trench_y = edge_of_field_y - inToM(55.5)
+ high_x = inToM(374.59) - WIDTH_OF_FIELD_IN_METERS / 2.0
+ low_x = high_x - inToM(30)
+ for sign in [1.0, -1.0]:
+ # Bottom Control Panel
+ # Top Line
+ cr.rectangle(sign * mToPx(high_x), sign * mToPx(edge_of_field_y),
+ -sign * mToPx(inToM(30)), -sign * mToPx(inToM(55.5)))
cr.stroke()
+
def draw_HAB(cr):
# BASE Constants
X_BASE = 0
@@ -430,6 +434,7 @@
mToPx(1.41605))
cr.stroke()
+
def draw_points(cr, p, size):
for i in range(0, len(p)):
draw_px_cross(cr, p[i][0], p[i][1], size,
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index c186711..31d7a37 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -75,7 +75,6 @@
self.module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
"""set extents on images"""
-
def reinit_extents(self):
self.extents_x_min = -1.0 * SCREEN_SIZE
self.extents_x_max = SCREEN_SIZE
@@ -220,12 +219,16 @@
cr.show_text('Press "e" to export')
cr.show_text('Press "i" to import')
+ cr.save()
+ cr.translate(mToPx(WIDTH_OF_FIELD_IN_METERS) / 2.0, 0.0)
set_color(cr, palette["BLACK"])
if FIELD == 2020:
- cr.rectangle(0, mToPx(-7.991475), SCREEN_SIZE, SCREEN_SIZE/2)
+ cr.rectangle(-mToPx(WIDTH_OF_FIELD_IN_METERS) / 2.0,
+ -mToPx(LENGTH_OF_FIELD_IN_METERS) / 2.0,
+ mToPx(WIDTH_OF_FIELD_IN_METERS),
+ mToPx(LENGTH_OF_FIELD_IN_METERS))
else:
- cr.rectangle(0, mToPx(-7.991475), SCREEN_SIZE, SCREEN_SIZE)
- print(mToPx(-7.991475))
+ cr.rectangle(0, -SCREEN_SIZE / 2, SCREEN_SIZE, SCREEN_SIZE)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
self.draw_field_elements(cr)
@@ -282,8 +285,9 @@
cr.paint_with_alpha(0.2)
- mygraph = Graph(cr, self.points)
draw_px_cross(cr, self.x, self.y, 10)
+ cr.restore()
+ mygraph = Graph(cr, self.points)
def draw_splines(self, cr):
holder_spline = []
@@ -312,10 +316,10 @@
def mouse_move(self, event):
old_x = self.x
old_y = self.y
- self.x = event.x
+ self.x = event.x - mToPx(WIDTH_OF_FIELD_IN_METERS / 2.0)
self.y = event.y
- dif_x = event.x - old_x
- dif_y = event.y - old_y
+ dif_x = self.x - old_x
+ dif_y = self.y - old_y
difs = np.array([pxToM(dif_x), pxToM(dif_y)])
if self.mode == Mode.kEditing:
@@ -344,7 +348,7 @@
self.mode = Mode.kEditing
self.points.resetPoints()
self.points.resetSplines()
- print("LOADING LOAD FROM " + file_name) # Load takes a few seconds
+ print("LOADING LOAD FROM " + file_name) # Load takes a few seconds
with open(self.path_to_export) as points_file:
self.points.setUpSplines(json.load(points_file))
@@ -399,7 +403,8 @@
# Save the index of the point closest
nearest = 1 # Max distance away a the selected point can be in meters
index_of_closest = 0
- for index_splines, points in enumerate(self.points.getSplines()):
+ for index_splines, points in enumerate(
+ self.points.getSplines()):
for index_points, val in enumerate(points):
distance = np.sqrt((cur_p[0] - val[0])**2 +
(cur_p[1] - val[1])**2)
@@ -414,6 +419,6 @@
def do_button_press(self, event):
# Be consistent with the scaling in the drawing_area
- self.x = event.x * 2
+ self.x = event.x * 2 - mToPx(WIDTH_OF_FIELD_IN_METERS / 2.0)
self.y = event.y * 2
self.button_press_action()
diff --git a/frc971/control_loops/python/spline_jsons/output_file_name.json b/frc971/control_loops/python/spline_jsons/output_file_name.json
new file mode 100644
index 0000000..3c0f1cd
--- /dev/null
+++ b/frc971/control_loops/python/spline_jsons/output_file_name.json
@@ -0,0 +1 @@
+[[[-3.8872582495409955, 0.28597845199632693], [-2.395108571601685, 0.25006215846088564], [-1.4261356362129025, 0.21880273565743844], [3.1831247467772927, 0.09566624396267474], [3.89779063039706, 0.1120965139633672], [4.3348366261980456, 0.10474398848262147]]]
\ No newline at end of file