Update origin location on Spline GUI
Still plenty of other work that needs to be done on the spline gui, but
at least update things so that (0, 0) is at the center of the field
again.
Change-Id: I5ff552588ab76d83a3c85600089b4558281f36b3
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