Spline Gui Commit
The Spline Gui Functionality:
Voltage and Acceleration Graph
Exportation to Json files
Ability to click and drag points
Added Usage Document
Moved many pieces around into seperate classes
Removed useless Functionality
Change-Id: I8fd819d4259b0b9b90c68f91ac73e01b3718a510
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 50764c7..1fae02b 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -164,18 +164,25 @@
)
py_binary(
- name = "path_edit",
+ name = "spline_graph",
srcs = [
- "basic_window.py",
"color.py",
+ "spline_drawing.py",
+ "spline_writer.py",
"path_edit.py",
+ "points.py",
+ "graph.py",
+ "spline_graph.py",
],
+ legacy_create_init = False,
restricted_to = ["//tools:k8"],
visibility = ["//visibility:public"],
deps = [
":libspline",
":python_init",
"@python_gtk",
+ "@matplotlib_repo//:matplotlib2.7",
+ ":basic_window",
],
)
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
old mode 100644
new mode 100755
index 78324a3..827fe64
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -1,3 +1,4 @@
+#!/usr/bin/python3
import gi
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
@@ -5,10 +6,10 @@
from gi.repository import Gdk
from gi.repository import GdkX11
import cairo
+from constants import *
identity = cairo.Matrix()
-
# Override the matrix of a cairo context.
class OverrideMatrix(object):
def __init__(self, cr, matrix):
@@ -34,7 +35,6 @@
def quit_main_loop(*args):
mainloop.quit()
-
def RunApp():
try:
mainloop.run()
@@ -49,11 +49,10 @@
# Draw in response to an expose-event
def __init__(self):
super(BaseWindow, self).__init__()
- #self.window.connect("destroy", quit_main_loop)
- self.set_size_request(640, 600)
+ self.set_size_request(2*SCREEN_SIZE, SCREEN_SIZE)
self.center = (0, 0)
- self.shape = (640, 400)
+ self.shape = (2*SCREEN_SIZE, SCREEN_SIZE)
self.needs_redraw = False
def get_current_scale(self):
@@ -81,3 +80,4 @@
# Handle the expose-event by drawing
def handle_draw(self, cr):
pass
+
diff --git a/frc971/control_loops/python/color.py b/frc971/control_loops/python/color.py
index ec53e82..5634042 100644
--- a/frc971/control_loops/python/color.py
+++ b/frc971/control_loops/python/color.py
@@ -1,9 +1,10 @@
class Color:
def __init__(self, r, g, b, a=1.0):
- self.r = r
- self.g = g
- self.b = b
- self.a = a
+ self.r = r
+ self.g = g
+ self.b = b
+ self.a = a
+
palette = {
"RED": Color(1, 0, 0),
@@ -16,5 +17,6 @@
"WHITE": Color(1, 1, 1),
"GREY": Color(0.5, 0.5, 0.5),
"LIGHT_GREY": Color(0.75, 0.75, 0.75),
- "DARK_GREY": Color(0.25, 0.25, 0.25)
+ "DARK_GREY": Color(0.25, 0.25, 0.25),
+ "ORANGE": Color(1, 0.65, 0)
}
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
new file mode 100644
index 0000000..7b45215
--- /dev/null
+++ b/frc971/control_loops/python/constants.py
@@ -0,0 +1,28 @@
+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")
+args = arg_parser.parse_args()
+SCREEN_SIZE = args.size
+
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+
+WIDTH_OF_ROBOT = 0.65
+LENGTH_OF_ROBOT = 0.8
+
+ROBOT_SIDE_TO_BALL_CENTER = 0.15 #Placeholder value
+BALL_RADIUS = 0.165
+ROBOT_SIDE_TO_HATCH_PANEL = 0.1 #Placeholder value
+HATCH_PANEL_WIDTH = 0.4826
+
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
+
+def mToPx(m):
+ return (m*SCREEN_SIZE/WIDTH_OF_FIELD_IN_METERS)
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
new file mode 100644
index 0000000..89979f0
--- /dev/null
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -0,0 +1,314 @@
+import cairo
+from color import Color, palette
+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)
+ else:
+ cr.set_source_rgba(color.r, color.g, color.b, color.a)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+ """Draws a cross with fixed dimensions in pixel space."""
+ set_color(cr, color)
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+ set_color(cr, palette["WHITE"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+ """Draws a x with fixed dimensions in pixel space."""
+ length_px = length_px1 / np.sqrt(2)
+ set_color(cr, color)
+ cr.move_to(x - length_px, y - length_px)
+ cr.line_to(x + length_px, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y + length_px)
+ cr.line_to(x + length_px, y - length_px)
+ cr.stroke()
+ set_color(cr, palette["WHITE"])
+
+
+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)
+ set_color(cr, color)
+ cr.arc(points[i][0], points[i][1], radius, 0, 2.0 * np.pi)
+ 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 draw_HAB(cr):
+ # BASE Constants
+ X_BASE = 0
+ Y_BASE = 0
+ R = 0.381 - .1
+ BACKWALL_X = X_BASE
+ LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+ # HAB Levels 2 and 3 called in variables backhab
+ # draw loading stations
+ cr.move_to(0, LOADING_Y)
+ cr.line_to(mToPx(0.6), LOADING_Y)
+ cr.move_to(mToPx(R), LOADING_Y)
+ cr.arc(mToPx(R), LOADING_Y, 5, 0, np.pi * 2.0)
+ cr.move_to(0, -1.0 * LOADING_Y)
+ cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+ cr.move_to(mToPx(R), -1.0 * LOADING_Y)
+ cr.arc(mToPx(R), -1.0 * LOADING_Y, 5, 0, np.pi * 2.0)
+
+ # HAB Levels 2 and 3 called in variables backhab
+ WIDTH_BACKHAB = mToPx(1.2192)
+
+ Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+ BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+ BACKHAB_LV3_LENGTH = mToPx(1.2192)
+ Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+ Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+ # HAB LEVEL 1
+ X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+ WIDTH_LV1_BOX = mToPx(0.90805)
+ LENGTH_LV1_BOX = mToPx(1.6256)
+
+ Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+ # Ramp off Level 1
+ X_RAMP = X_LV1_BOX
+
+ Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+ WIDTH_TOP_RAMP = mToPx(1.20015)
+ LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+ X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+ Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+ LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+ WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+ Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+ # Side Bars to Hold in balls
+ X_BARS = BACKWALL_X
+ WIDTH_BARS = WIDTH_BACKHAB
+ LENGTH_BARS = mToPx(0.574675)
+
+ Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+ Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+ set_color(cr, palette["BLACK"])
+ cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+ LENGTH_MIDDLE_RAMP)
+ cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.stroke()
+
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+ cr.stroke()
+
+ #draw 0, 0
+ set_color(cr, palette["BLACK"])
+ cr.move_to(0, 0)
+ cr.line_to(0, 0 + mToPx(8.2296 / 2.0))
+ cr.move_to(0, 0)
+ cr.line_to(0, 0 + mToPx(-8.2296 / 2.0))
+ cr.move_to(0, 0)
+ cr.line_to(0 + mToPx(8.2296), 0)
+
+ cr.stroke()
+
+
+def draw_rockets(cr):
+ # BASE Constants
+ X_BASE = mToPx(2.41568)
+ Y_BASE = 0
+ # Robot longitudinal radius
+ R = 0.381
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+ near_side_rocket_center[1] - 0.8 * mToPx(0.5))
+ cr.move_to(near_side_rocket_center[0] - R * mToPx(0.866),
+ near_side_rocket_center[1] - R * mToPx(0.5))
+ cr.arc(near_side_rocket_center[0] - R * mToPx(0.866),
+ near_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.8))
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R))
+ cr.arc(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+ far_side_rocket_center[1] - 0.8 * mToPx(0.5))
+ cr.move_to(far_side_rocket_center[0] + R * mToPx(0.866),
+ far_side_rocket_center[1] - R * mToPx(0.5))
+ cr.arc(far_side_rocket_center[0] + R * mToPx(0.866),
+ far_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+ #print(far_side_rocket_center)
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+ near_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.8))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+ far_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ # Bottom Rocket
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ cr.stroke()
+
+
+def draw_cargo_ship(cr):
+ # BASE Constants
+ X_BASE = 0 + mToPx(5.59435)
+ Y_BASE = 0 + 0 #mToPx(4.129151)
+ R = 0.381 - 0.1
+
+ FRONT_PEG_DELTA_Y = mToPx(0.276352)
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.8), Y_BASE + FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.arc(X_BASE - mToPx(R), Y_BASE + FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.8), Y_BASE - FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.arc(X_BASE - mToPx(R), Y_BASE - FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+ SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+ SIDE_PEG_X = X_BASE + mToPx(1.148842)
+ SIDE_PEG_DX = mToPx(0.55245)
+
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+ 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,
+ Color(0, np.sqrt(0.2 * i), 0))
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
new file mode 100644
index 0000000..7e98a38
--- /dev/null
+++ b/frc971/control_loops/python/graph.py
@@ -0,0 +1,178 @@
+from constants import *
+import cairo
+from color import Color, palette
+from points import Points
+from drawing_constants import *
+from libspline import Spline, DistanceSpline, Trajectory
+
+AXIS_MARGIN_SPACE = 40
+
+
+class Graph(): # (TODO): Remove Computer Calculation
+ def __init__(self, cr, mypoints):
+ # Background Box
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+ SCREEN_SIZE * 0.6)
+ cr.fill()
+
+ cr.set_source_rgb(0, 0, 0)
+ cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+ SCREEN_SIZE * 0.6)
+ #Axis
+ cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE) # Y
+ cr.line_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ 0.1 * SCREEN_SIZE - 10)
+
+ cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE) # X
+ cr.line_to(-10, -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE)
+ cr.stroke()
+
+ skip = 2
+ dT = 0.00505
+ start = AXIS_MARGIN_SPACE - SCREEN_SIZE
+ end = -2.0 * AXIS_MARGIN_SPACE
+ height = 0.5 * (SCREEN_SIZE) - AXIS_MARGIN_SPACE
+ zero = AXIS_MARGIN_SPACE - SCREEN_SIZE / 2.0
+ if mypoints.getLibsplines():
+ distanceSpline = DistanceSpline(mypoints.getLibsplines())
+ traj = Trajectory(distanceSpline)
+ traj.Plan()
+ XVA = traj.GetPlanXVA(dT)
+ self.draw_x_axis(cr, start, height, zero, XVA, end)
+ self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+ self.drawAcceleration(cr, XVA, start, height, skip, zero,
+ AXIS_MARGIN_SPACE, end)
+ self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
+ cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
+ cr.stroke()
+
+ def connectLines(self, cr, points, color):
+ for i in range(0, len(points) - 1):
+ set_color(cr, color)
+ cr.move_to(points[i][0], points[i][1])
+ cr.line_to(points[i + 1][0], points[i + 1][1])
+ cr.stroke()
+
+ def draw_x_axis(self, cr, start, height, zero, xva, end):
+ total_time = 0.00505 * len(xva[0])
+ for k in np.linspace(0, 1, 11):
+ self.tickMark(cr,
+ k * np.abs(start - end) + start, zero + height / 2.0,
+ 10, palette["BLACK"])
+ cr.move_to(k * np.abs(start - end) + start,
+ 10 + zero + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(k * total_time, 3)), txt_scale,
+ txt_scale, 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def tickMark(self, cr, x, y, height, COLOR):
+ # X, Y is in the middle of the tick mark
+ set_color(cr, COLOR)
+ cr.move_to(x, y + (height / 2))
+ cr.line_to(x, y - (height / 2))
+ cr.stroke()
+
+ def HtickMark(self, cr, x, y, width, COLOR):
+ # X, Y is in the middle of the tick mark
+ set_color(cr, COLOR)
+ cr.move_to(x + (width / 2), y)
+ cr.line_to(x - (width / 2), y)
+ cr.stroke()
+
+ def drawVelocity(self, cr, xva, start, height, skip, zero, end):
+ COLOR = palette["RED"]
+ velocity = xva[1]
+ n_timesteps = len(velocity)
+ max_v = np.amax(velocity)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ scaler = height / max_v
+ cr.set_source_rgb(1, 0, 0)
+ points = []
+ for i in range(0, len(velocity)):
+ if i % skip == 0:
+ points.append([
+ start + (i * spacing),
+ zero + height / 2.0 + (velocity[i] * scaler / 2.0)
+ ])
+ self.connectLines(cr, points, COLOR)
+
+ # draw axes marking
+ for i in np.linspace(-1, 1, 11):
+ self.HtickMark(cr, start, zero + i * height / 2.0 + height / 2.0,
+ 10, palette["BLACK"])
+ cr.set_source_rgb(1, 0, 0)
+ cr.move_to(start + 5, zero + i * height / 2.0 + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * max_v, 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def drawAcceleration(self, cr, xva, start, height, skip, zero, margin,
+ end):
+ COLOR = palette["BLUE"]
+ accel = xva[2]
+ max_a = np.amax(accel)
+ min_a = np.amin(accel)
+ n_timesteps = len(accel)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ scaler = height / (max_a - min_a)
+ cr.set_source_rgb(1, 0, 0)
+ points = []
+ for i in range(0, len(accel)):
+ if i % skip == 0:
+ points.append([
+ start + (i * spacing), zero + ((accel[i] - min_a) * scaler)
+ ])
+ self.connectLines(cr, points, COLOR)
+
+ # draw axes marking
+ for i in np.linspace(0, 1, 11):
+ self.HtickMark(cr, -1.5 * margin, zero + i * height, 10,
+ palette["BLACK"])
+ cr.set_source_rgb(0, 0, 1)
+ cr.move_to(-1.2 * margin, zero + i * height)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * (max_a - min_a) + min_a,
+ 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def drawVoltage(self, cr, xva, start, height, skip, traj, zero, end):
+ COLOR1 = palette["GREEN"]
+ COLOR2 = palette["CYAN"]
+ poses = xva[0]
+ n_timesteps = len(poses)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ points1 = []
+ points2 = []
+ for i in range(0, len(poses)):
+ if i % skip == 0:
+ voltage = traj.Voltage(poses[i])
+ points1.append([
+ start + (i * spacing),
+ zero + height / 2 + height * (voltage[0] / 24.0)
+ ])
+ points2.append([
+ start + (i * spacing),
+ zero + height / 2 + height * (voltage[1] / 24.0)
+ ])
+ self.connectLines(cr, points1, COLOR1)
+ self.connectLines(cr, points2, COLOR2)
+
+ for i in np.linspace(-1, 1, 7):
+ self.HtickMark(cr, -1.0 * SCREEN_SIZE,
+ zero + i * height / 2.0 + height / 2.0, 10,
+ palette["BLACK"])
+ cr.set_source_rgb(0, 1, 1)
+ cr.move_to(-1.0 * SCREEN_SIZE,
+ zero + i * height / 2.0 + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * 12.0, 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
old mode 100644
new mode 100755
index e59286f..0d1b263
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -3,67 +3,23 @@
import os
import sys
import copy
-import basic_window
from color import Color, palette
import random
import gi
import numpy as np
-from libspline import Spline
import scipy.spatial.distance
gi.require_version('Gtk', '3.0')
+gi.require_version('Gdk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
+from libspline import Spline, DistanceSpline, Trajectory
import enum
-import json # For writing to json files
-
-from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
-
-WIDTH_OF_FIELD_IN_METERS = 8.258302
-PIXELS_ON_SCREEN = 300
-
-
-def pxToM(p):
- return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-
-
-def mToPx(m):
- return (m*PIXELS_ON_SCREEN/WIDTH_OF_FIELD_IN_METERS)
-
-def px(cr):
- return OverrideMatrix(cr, identity)
-
-
-def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
- """Draws a cross with fixed dimensions in pixel space."""
- set_color(cr, color)
- cr.move_to(x, y - length_px)
- cr.line_to(x, y + length_px)
- cr.stroke()
-
- cr.move_to(x - length_px, y)
- cr.line_to(x + length_px, y)
- cr.stroke()
- set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
- """Draws a x with fixed dimensions in pixel space."""
- length_px = length_px1 / np.sqrt(2)
- set_color(cr, color)
- cr.move_to(x - length_px, y - length_px)
- cr.line_to(x + length_px, y + length_px)
- cr.stroke()
-
- cr.move_to(x - length_px, y + length_px)
- cr.line_to(x + length_px, y - length_px)
- cr.stroke()
- set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_points(cr, p, size):
- for i in range(0, len(p)):
- draw_px_cross(cr, p[i][0], p[i][1], size, Color(
- 0, np.sqrt(0.2 * i), 0))
+import json
+from basic_window import *
+from constants import *
+from drawing_constants import *
+from points import Points
+from graph import Graph
class Mode(enum.Enum):
@@ -72,61 +28,25 @@
kEditing = 2
kExporting = 3
kImporting = 4
- kConstraint = 5
-class ConstraintType(enum.Enum):
- kMaxSpeed = 0
- kMaxAcceleration = 1
-
-
-def display_text(cr, text, widtha, heighta, widthb, heightb):
- cr.scale(widtha, -heighta)
- cr.show_text(text)
- cr.scale(widthb, -heightb)
-
-
-def redraw(needs_redraw, window):
- print("Redrew")
- if not needs_redraw:
- window.queue_draw()
-
-
-class Constraint():
- def __init__(self, start, end, constraint, value):
- self.start = start #Array with index and distance from start of spline
- self.end = end #Array with index and distance from start of spline
- self.constraint = constraint #INT
- self.value = value #INT
- if self.constraint == 0:
- self.conName = "kMaxSpeed"
- else:
- self.conName = "kMaxAcceleration"
-
- def toString(self):
-
- return "START: " + str(self.start[0]) + ", " + str(
- self.start[1]) + " | END: " + str(self.end[0]) + ", " + str(
- self.end[1]) + " | " + str(self.conName) + ": " + str(
- self.value)
-
-
-class GTK_Widget(basic_window.BaseWindow):
+class GTK_Widget(BaseWindow):
"""Create a GTK+ widget on which we will draw using Cairo"""
-
def __init__(self):
super(GTK_Widget, self).__init__()
+ self.points = Points()
+
# init field drawing
# add default spline for testing purposes
# init editing / viewing modes and pointer location
self.mode = Mode.kPlacing
self.x = 0
self.y = 0
-
module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
self.path_to_export = os.path.join(module_path,
- 'points_for_pathedit.json')
+ 'points_for_pathedit.json')
+
# update list of control points
self.point_selected = False
# self.adding_spline = False
@@ -145,32 +65,20 @@
for c in palette:
self.colors.append(palette[c])
- self.selected_points = []
- self.splines = []
self.reinit_extents()
self.inStart = None
self.inEnd = None
- self.inConstraint = None
self.inValue = None
self.startSet = False
- #John also wrote this
- def add_point(self, x, y):
- if (len(self.selected_points) < 6):
- self.selected_points.append([pxToM(x), pxToM(y)])
- if (len(self.selected_points) == 6):
- self.mode = Mode.kEditing
- self.splines.append(np.array(self.selected_points))
- self.selected_points = []
-
"""set extents on images"""
def reinit_extents(self):
- self.extents_x_min = -800
- self.extents_x_max = 800
- self.extents_y_min = -800
- self.extents_y_max = 800
+ self.extents_x_min = -1.0 * SCREEN_SIZE
+ self.extents_x_max = SCREEN_SIZE
+ self.extents_y_min = -1.0 * SCREEN_SIZE
+ self.extents_y_max = SCREEN_SIZE
# this needs to be rewritten with numpy, i dont think this ought to have
# SciPy as a dependecy
@@ -184,192 +92,112 @@
def get_nearest_point(self):
return self.all_controls[self.get_index_of_nearest_point()]
- def set_index_to_nearest_spline_point(self):
- nearest = 50
- index_of_closest = 0
- self.spline_edit = 0
- cur_p = [self.x, self.y]
-
- for index_splines, points in enumerate(self.spline):
- for index_points, point in enumerate(points.curve):
- # pythagorean
- distance = np.sqrt((cur_p[0] - mToPx(point[0]))**2 +
- (cur_p[1] - mToPx(point[1])**2))
- if distance < nearest:
- nearest = distance
- print("DISTANCE: ", distance, " | INDEX: ", index_points)
- index_of_closest = index_points
- self.index_of_edit = index_of_closest
- self.spline_edit = index_splines
- self.held_x = self.x
- if self.startSet == False:
- self.inStart = [self.index_of_edit, self.findDistance()]
- self.startSet = True
- else:
- self.inEnd = [self.index_of_edit, self.findDistance()]
- self.startSet = False
- self.mode = Mode.kEditing
- self.spline_edit = -1
- self.index_of_edit = -1
-
- print("Nearest: " + str(nearest))
- print("Spline: " + str(self.spline_edit))
- print("Index: " + str(index_of_closest))
-
- def findDistance(self):
- """ findDistance goes through each point on the spline finding distance to that point from the point before.
- It does this to find the the length of the spline to the point that is currently selected.
- """
- distance = 0
- points = self.curves[self.spline_edit]
- for index, point in enumerate(points):
- if index > 0 and index <= self.index_of_edit:
- distance += np.sqrt((points[index - 1][0] - point[0])**2 +
- (points[index - 1][1] - point[1])**2)
- return distance
-
- def draw_HAB(self, cr):
- print("WENT IN")
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- BACKWALL_X = -450
-
- # HAB Levels 2 and 3 called in variables backhab
-
- WIDTH_BACKHAB = mToPx(1.2192)
-
- Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
- BACKHAB_LV2_LENGTH = mToPx(1.016)
-
- BACKHAB_LV3_LENGTH = mToPx(1.2192)
- Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
-
- Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX -BACKHAB_LV2_LENGTH
-
- # HAB LEVEL 1
- X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
-
- WIDTH_LV1_BOX = mToPx(0.90805)
- LENGTH_LV1_BOX = mToPx(1.6256)
-
- Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
-
- # Ramp off Level 1
- X_RAMP = X_LV1_BOX
-
- Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
- WIDTH_TOP_RAMP = mToPx(1.20015)
- LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
-
- X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
- Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
- LENGTH_MIDDLE_RAMP = 2*LENGTH_LV1_BOX
- WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
-
- Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
-
- # Side Bars to Hold in balls
- X_BARS = BACKWALL_X
- WIDTH_BARS = WIDTH_BACKHAB
- LENGTH_BARS = mToPx(0.574675)
-
- Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
-
- Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
-
- set_color(cr, palette["BLACK"])
- cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB,
- BACKHAB_LV3_LENGTH)
- cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
- cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX,
- LENGTH_LV1_BOX)
- cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
- LENGTH_MIDDLE_RAMP)
- cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.stroke()
- #draw_px_x(cr, BACKWALL_X, 0, 10) # Midline Point
- #draw_px_x(cr, X_BASE, Y_BASE, 10) # Bases
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
-
- cr.stroke()
-
- def draw_rockets(self, cr):
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- # Top Rocket
-
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- # Bottom Rocket
-
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- cr.stroke()
-
- def draw_cargo_ship(self, cr):
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
- mToPx(2.43205), mToPx(1.41605))
-
- cr.stroke()
-
def draw_field_elements(self, cr):
- self.draw_HAB(cr)
- self.draw_rockets(cr)
- self.draw_cargo_ship(cr)
+ draw_HAB(cr)
+ draw_rockets(cr)
+ draw_cargo_ship(cr)
- def handle_draw(self, cr):
- # print(self.new_point)
- # print("SELF.POINT_SELECTED: " + str(self.point_selected))
+ def draw_robot_at_point(self, cr, i, p, spline):
+ p1 = [mToPx(spline.Point(i)[0]), mToPx(spline.Point(i)[1])]
+ p2 = [mToPx(spline.Point(i + p)[0]), mToPx(spline.Point(i + p)[1])]
- # begin drawing
+ #Calculate Robot
+ distance = np.sqrt((p2[1] - p1[1])**2 + (p2[0] - p1[0])**2)
+ x_difference_o = p2[0] - p1[0]
+ y_difference_o = p2[1] - p1[1]
+ x_difference = x_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
+ y_difference = y_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
+
+ front_middle = []
+ front_middle.append(p1[0] + x_difference)
+ front_middle.append(p1[1] + y_difference)
+
+ back_middle = []
+ back_middle.append(p1[0] - x_difference)
+ back_middle.append(p1[1] - y_difference)
+
+ slope = [-(1 / x_difference_o) / (1 / y_difference_o)]
+ angle = np.arctan(slope)
+
+ x_difference = np.sin(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
+ y_difference = np.cos(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
+
+ front_1 = []
+ front_1.append(front_middle[0] - x_difference)
+ front_1.append(front_middle[1] - y_difference)
+
+ front_2 = []
+ front_2.append(front_middle[0] + x_difference)
+ front_2.append(front_middle[1] + y_difference)
+
+ back_1 = []
+ back_1.append(back_middle[0] - x_difference)
+ back_1.append(back_middle[1] - y_difference)
+
+ back_2 = []
+ back_2.append(back_middle[0] + x_difference)
+ back_2.append(back_middle[1] + y_difference)
+
+ x_difference = x_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+ y_difference = y_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+
+ #Calculate Ball
+ ball_center = []
+ ball_center.append(p1[0] + x_difference)
+ ball_center.append(p1[1] + y_difference)
+
+ x_difference = x_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+ y_difference = y_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+
+ #Calculate Panel
+ panel_center = []
+ panel_center.append(p1[0] + x_difference)
+ panel_center.append(p1[1] + y_difference)
+
+ x_difference = np.sin(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+ y_difference = np.cos(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+
+ panel_1 = []
+ panel_1.append(panel_center[0] + x_difference)
+ panel_1.append(panel_center[1] + y_difference)
+
+ panel_2 = []
+ panel_2.append(panel_center[0] - x_difference)
+ panel_2.append(panel_center[1] - y_difference)
+
+ #Draw Robot
+ cr.move_to(front_1[0], front_1[1])
+ cr.line_to(back_1[0], back_1[1])
+ cr.line_to(back_2[0], back_2[1])
+ cr.line_to(front_2[0], front_2[1])
+ cr.line_to(front_1[0], front_1[1])
+
+ cr.stroke()
+
+ #Draw Ball
+ set_color(cr, palette["ORANGE"], 0.5)
+ cr.move_to(back_middle[0], back_middle[1])
+ cr.line_to(ball_center[0], ball_center[1])
+ cr.arc(ball_center[0], ball_center[1], mToPx(BALL_RADIUS), 0,
+ 2 * np.pi)
+ cr.stroke()
+
+ #Draw Panel
+ set_color(cr, palette["YELLOW"], 0.5)
+ cr.move_to(panel_1[0], panel_1[1])
+ cr.line_to(panel_2[0], panel_2[1])
+
+ cr.stroke()
+ cr.set_source_rgba(0, 0, 0, 1)
+
+ def handle_draw(self, cr): # main
# Fill the background color of the window with grey
- set_color(cr, palette["GREY"])
+ set_color(cr, palette["WHITE"])
cr.paint()
- #Scale the field to fit within drawing area
- cr.scale(0.5, 0.5)
# Draw a extents rectangle
set_color(cr, palette["WHITE"])
@@ -378,209 +206,181 @@
self.extents_y_max - self.extents_y_min)
cr.fill()
- #Drawing the field
+ #Drawing the switch and scale in the field
cr.move_to(0, 50)
cr.show_text('Press "e" to export')
cr.show_text('Press "i" to import')
- set_color(cr, Color(0.3, 0.3, 0.3))
- cr.rectangle(-450, -150, 300, 300)
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
cr.fill()
set_color(cr, palette["BLACK"])
- cr.rectangle(-450, -150, 300, 300)
+ cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
-
self.draw_field_elements(cr)
-
y = 0
- # update all the things
+ # update everything
- if self.mode == Mode.kViewing:
+ if self.mode == Mode.kPlacing or self.mode == Mode.kViewing:
set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- cr.show_text("VIEWING")
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- # for item in self.selected_points:
- # print(str(item))
- for i, point in enumerate(self.selected_points):
- # print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
- elif self.mode == Mode.kPlacing:
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "ADD", 1, 1, 1, 1)
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- for item in self.selected_points:
- print(str(item))
- for i, point in enumerate(self.selected_points):
- print("I: " + str(i))
+ cr.move_to(-SCREEN_SIZE, 170)
+ plotPoints = self.points.getPoints()
+ if plotPoints:
+ for i, point in enumerate(plotPoints):
draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ set_color(cr, palette["WHITE"])
elif self.mode == Mode.kEditing:
set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
+ cr.move_to(-SCREEN_SIZE, 170)
display_text(cr, "EDITING", 1, 1, 1, 1)
- if len(self.splines) > 0:
- holder_spline = []
- for i, points in enumerate(self.splines):
- array = np.zeros(shape=(6, 2), dtype=float)
- for j, point in enumerate(points):
- array[j, 0] = mToPx(point[0])
- array[j, 1] = mToPx(point[1])
- spline = Spline(np.ascontiguousarray(np.transpose(array)))
- for k in np.linspace(0.01, 1, 100):
- cr.move_to(
- spline.Point(k - 0.01)[0],
- spline.Point(k - 0.01)[1])
- cr.line_to(spline.Point(k)[0], spline.Point(k)[1])
- cr.stroke()
- holding = [
- spline.Point(k - 0.01)[0],
- spline.Point(k - 0.01)[1]
- ]
- holder_spline.append(holding)
- self.curves.append(holder_spline)
+ if self.points.getSplines():
+ self.draw_splines(cr)
+ for i, points in enumerate(self.points.getSplines()):
- for spline, points in enumerate(self.splines):
- # for item in points:
- # print(str(item))
- for i, point in enumerate(points):
- # print("I: " + str(i))
- if spline == self.spline_edit and i == self.index_of_edit:
- draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 15,
- self.colors[spline])
- elif (spline == 0 and not i == 5) or (not i == 0
- and not i == 5):
- draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10,
- self.colors[spline])
- cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ p0 = np.array([mToPx(points[0][0]), mToPx(points[0][1])])
+ p1 = np.array([mToPx(points[1][0]), mToPx(points[1][1])])
+ p2 = np.array([mToPx(points[2][0]), mToPx(points[2][1])])
+ p3 = np.array([mToPx(points[3][0]), mToPx(points[3][1])])
+ p4 = np.array([mToPx(points[4][0]), mToPx(points[4][1])])
+ p5 = np.array([mToPx(points[5][0]), mToPx(points[5][1])])
- elif self.mode == Mode.kConstraint:
- print("Drawn")
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "Adding Constraint", 1, 1, 1, 1)
- if len(self.splines) > 0:
- # print("Splines: " + str(len(self.splines)))
- # print("ITEMS:")
- for s, points in enumerate(self.splines):
- # for item in points:
- # print(str(item))
- for i, point in enumerate(points):
- # print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10, self.colors[s])
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ draw_control_points(cr, [p0, p1, p2, p3, p4, p5])
+ first_tangent = p0 + 2.0 * (p1 - p0)
+ second_tangent = p5 + 2.0 * (p4 - p5)
+ cr.set_source_rgb(0, 0.5, 0)
+ cr.move_to(p0[0], p0[1])
+ cr.set_line_width(1.0)
+ cr.line_to(first_tangent[0], first_tangent[1])
+ cr.move_to(first_tangent[0], first_tangent[1])
+ cr.line_to(p2[0], p2[1])
- cr.paint_with_alpha(.65)
+ cr.move_to(p5[0], p5[1])
+ cr.line_to(second_tangent[0], second_tangent[1])
+ cr.move_to(second_tangent[0], second_tangent[1])
+ cr.line_to(p3[0], p3[1])
+
+ cr.stroke()
+ cr.set_line_width(2.0)
+ self.points.update_lib_spline()
+ set_color(cr, palette["WHITE"])
+
+ cr.paint_with_alpha(0.2)
+
+ mygraph = Graph(cr, self.points)
draw_px_cross(cr, self.x, self.y, 10)
- def do_key_press(self, event):
+ def draw_splines(self, cr):
+ holder_spline = []
+ for i, points in enumerate(self.points.getSplines()):
+ array = np.zeros(shape=(6, 2), dtype=float)
+ for j, point in enumerate(points):
+ array[j, 0] = point[0]
+ array[j, 1] = point[1]
+ spline = Spline(np.ascontiguousarray(np.transpose(array)))
+ for k in np.linspace(0.01, 1, 100):
+ cr.move_to(mToPx(spline.Point(k - 0.01)[0]),
+ mToPx(spline.Point(k - 0.01)[1]))
+ cr.line_to(mToPx(spline.Point(k)[0]),
+ mToPx(spline.Point(k)[1]))
+ cr.stroke()
+ holding = [
+ spline.Point(k - 0.01)[0],
+ spline.Point(k - 0.01)[1]
+ ]
+ holder_spline.append(holding)
+ if i == 0:
+ self.draw_robot_at_point(cr, 0.00, 0.01, spline)
+ self.draw_robot_at_point(cr, 1, 0.01, spline)
+ self.curves.append(holder_spline)
+
+ def mouse_move(self, event):
+ old_x = self.x
+ old_y = self.y
+ self.x = event.x
+ self.y = event.y
+ dif_x = event.x - old_x
+ dif_y = event.y - old_y
+ difs = np.array([pxToM(dif_x), pxToM(dif_y)])
+
+ if self.mode == Mode.kEditing:
+ self.spline_edit = self.points.updates_for_mouse_move(
+ self.index_of_edit, self.spline_edit, self.x, self.y, difs)
+
+ def do_key_press(self, event, file_name):
keyval = Gdk.keyval_to_lower(event.keyval)
- # print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+ module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+ self.path_to_export = os.path.join(module_path,
+ "spline_jsons/" + file_name)
if keyval == Gdk.KEY_q:
print("Found q key and exiting.")
quit_main_loop()
- if keyval == Gdk.KEY_e:
- # Will export to json file
- self.mode = Mode.kEditing
- print(str(sys.argv))
- print('out to: ', self.path_to_export)
- exportList = [l.tolist() for l in self.splines]
- with open(self.path_to_export, mode='w') as points_file:
- json.dump(exportList, points_file)
- print("Wrote: " + str(self.splines))
- if keyval == Gdk.KEY_i:
- # import from json file
- self.mode = Mode.kEditing
- self.selected_points = []
- self.splines = []
- with open(self.path_to_export) as points_file:
- self.splines = json.load(points_file)
- print("Added: " + str(self.splines))
+ file_name_end = file_name[-5:]
+ if file_name_end != ".json":
+ print("Error: Filename doesn't end in .json")
+ else:
+ if keyval == Gdk.KEY_e:
+ # Will export to json file
+ self.mode = Mode.kEditing
+ print('out to: ', self.path_to_export)
+ exportList = [l.tolist() for l in self.points.getSplines()]
+ with open(self.path_to_export, mode='w') as points_file:
+ json.dump(exportList, points_file)
+
+ if keyval == Gdk.KEY_i:
+ # import from json file
+ self.mode = Mode.kEditing
+ self.points.resetPoints()
+ self.points.resetSplines()
+ with open(self.path_to_export) as points_file:
+ self.points.setUpSplines(json.load(points_file))
+
+ self.points.update_lib_spline()
+
if keyval == Gdk.KEY_p:
self.mode = Mode.kPlacing
# F0 = A1
# B1 = 2F0 - E0
# C1= d0 + 4F0 - 4E0
- spline_index = len(self.splines) - 1
- self.selected_points = []
- f = self.splines[spline_index][5]
- e = self.splines[spline_index][4]
- d = self.splines[spline_index][3]
- self.selected_points.append(f)
- self.selected_points.append(f * 2 + e * -1)
- self.selected_points.append(d + f * 4 + e * -4)
-
- if keyval == Gdk.KEY_c:
- self.mode = Mode.kConstraint
+ spline_index = len(self.points.getSplines()) - 1
+ self.points.resetPoints()
+ self.points.extrapolate(
+ self.points.getSplines()[len(self.points.getSplines()) - 1][5],
+ self.points.getSplines()[len(self.points.getSplines()) - 1][4],
+ self.points.getSplines()[len(self.points.getSplines()) - 1][3])
def button_press_action(self):
if self.mode == Mode.kPlacing:
- #Check that the point clicked is on the field
- if (self.x < -150 and self.x > -450 and self.y < 150
- and self.y > -150):
- self.add_point(self.x, self.y)
+ if self.points.add_point(self.x, self.y):
+ self.mode = Mode.kEditing
elif self.mode == Mode.kEditing:
# Now after index_of_edit is not -1, the point is selected, so
# user can click for new point
if self.index_of_edit > -1 and self.held_x != self.x:
- print("INDEX OF EDIT: " + str(self.index_of_edit))
- self.splines[self.spline_edit][self.index_of_edit] = [
- pxToM(self.x), pxToM(self.y)
- ]
+ self.points.setSplines(self.spline_edit, self.index_of_edit,
+ pxToM(self.x), pxToM(self.y))
- if not self.spline_edit == len(self.splines) - 1:
- spline_edit = self.spline_edit + 1
- f = self.splines[self.spline_edit][5]
- e = self.splines[self.spline_edit][4]
- d = self.splines[self.spline_edit][3]
- self.splines[spline_edit][0] = f
- self.splines[spline_edit][1] = f * 2 + e * -1
- self.splines[spline_edit][2] = d + f * 4 + e * -4
-
- if not self.spline_edit == 0:
- spline_edit = self.spline_edit - 1
- a = self.splines[self.spline_edit][0]
- b = self.splines[self.spline_edit][1]
- c = self.splines[self.spline_edit][2]
- self.splines[spline_edit][5] = a
- self.splines[spline_edit][4] = a * 2 + b * -1
- self.splines[spline_edit][3] = c + a * 4 + b * -4
+ self.spline_edit = self.points.splineExtrapolate(
+ self.spline_edit)
self.index_of_edit = -1
self.spline_edit = -1
else:
- print("mode == 2")
# Get clicked point
# Find nearest
# Move nearest to clicked
cur_p = [pxToM(self.x), pxToM(self.y)]
- print("CUR_P: " + str(self.x) + " " + str(self.y))
# Get the distance between each for x and y
# 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.splines):
+ for index_splines, points in enumerate(self.points.getSplines()):
for index_points, val in enumerate(points):
- # pythagorean
distance = np.sqrt((cur_p[0] - val[0])**2 +
(cur_p[1] - val[1])**2)
if distance < nearest:
@@ -591,10 +391,6 @@
self.index_of_edit = index_of_closest
self.spline_edit = index_splines
self.held_x = self.x
- elif self.mode == Mode.kConstraint:
- print("RAN")
- self.set_index_to_nearest_spline_point()
- print("FINISHED")
def do_button_press(self, event):
print("button press activated")
@@ -612,13 +408,23 @@
print("Method_connect ran")
self.connect(event, handler)
+ def mouse_move(self, event):
+ #Changes event.x and event.y to be relative to the center.
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.mouse_move(event)
+ self.queue_draw()
+
def button_press(self, event):
print("button press activated")
o_x = event.x
o_y = event.y
x = event.x - self.drawing_area.window_shape[0] / 2
y = self.drawing_area.window_shape[1] / 2 - event.y
- scale = self.drawing_area.get_current_scale()
+ scale = 2 * self.drawing_area.get_current_scale()
event.x = x / scale + self.drawing_area.center[0]
event.y = y / scale + self.drawing_area.center[1]
self.drawing_area.do_button_press(event)
@@ -627,21 +433,17 @@
def key_press(self, event):
print("key press activated")
- self.drawing_area.do_key_press(event)
+ self.drawing_area.do_key_press(event, self.file_name_box.get_text())
self.queue_draw()
def configure(self, event):
print("configure activated")
self.drawing_area.window_shape = (event.width, event.height)
- def on_submit_click(self, widget):
- self.drawing_area.inConstraint = int(self.constraint_box.get_text())
- self.drawing_area.inValue = int(self.value_box.get_text())
-
def __init__(self):
Gtk.Window.__init__(self)
- self.set_default_size(1366, 738)
+ self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
flowBox = Gtk.FlowBox()
flowBox.set_valign(Gtk.Align.START)
@@ -663,41 +465,25 @@
| Gdk.EventMask.SCROLL_MASK
| Gdk.EventMask.KEY_PRESS_MASK)
+ #add the graph box
self.drawing_area = GTK_Widget()
self.eventBox.add(self.drawing_area)
self.method_connect("key-release-event", self.key_press)
self.method_connect("button-release-event", self.button_press)
self.method_connect("configure-event", self.configure)
+ self.method_connect("motion_notify_event", self.mouse_move)
- # Constraint Boxes
+ self.file_name_box = Gtk.Entry()
+ self.file_name_box.set_size_request(200, 40)
- self.start_box = Gtk.Entry()
- self.start_box.set_size_request(100, 20)
+ self.file_name_box.set_text("File")
+ self.file_name_box.set_editable(True)
- self.constraint_box = Gtk.Entry()
- self.constraint_box.set_size_request(100, 20)
-
- self.constraint_box.set_text("Constraint")
- self.constraint_box.set_editable(True)
-
- container.put(self.constraint_box, 700, 0)
-
- self.value_box = Gtk.Entry()
- self.value_box.set_size_request(100, 20)
-
- self.value_box.set_text("Value")
- self.value_box.set_editable(True)
-
- container.put(self.value_box, 700, 40)
-
- self.submit_button = Gtk.Button("Submit")
- self.submit_button.connect('clicked', self.on_submit_click)
-
- container.put(self.submit_button, 880, 0)
+ container.put(self.file_name_box, 0, 0)
self.show_all()
window = GridWindow()
-basic_window.RunApp()
+RunApp()
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
new file mode 100644
index 0000000..8f94e3f
--- /dev/null
+++ b/frc971/control_loops/python/points.py
@@ -0,0 +1,110 @@
+from constants import *
+import numpy as np
+from libspline import Spline, DistanceSpline, Trajectory
+
+
+class Points():
+ def __init__(self):
+ self.points = [] # Holds all points not yet in spline
+ self.libsplines = [] # Formatted for libspline library usage
+ self.splines = [] # Formatted for drawing
+
+ def getPoints(self):
+ return self.points
+
+ def resetPoints(self):
+ self.points = []
+
+ def getLibsplines(self):
+ return self.libsplines
+
+ def splineExtrapolate(self, o_spline_edit):
+ spline_edit = o_spline_edit
+ if not spline_edit == len(self.splines) - 1:
+ spline_edit = spline_edit + 1
+ f = self.splines[spline_edit][5]
+ e = self.splines[spline_edit][4]
+ d = self.splines[spline_edit][3]
+ self.splines[spline_edit][0] = f
+ self.splines[spline_edit][1] = f * 2 + e * -1
+ self.splines[spline_edit][2] = d + f * 4 + e * -4
+
+ if not spline_edit == 0:
+ spline_edit = spline_edit - 1
+ a = self.splines[spline_edit][0]
+ b = self.splines[spline_edit][1]
+ c = self.splines[spline_edit][2]
+ self.splines[spline_edit][5] = a
+ self.splines[spline_edit][4] = a * 2 + b * -1
+ self.splines[spline_edit][3] = c + a * 4 + b * -4
+
+ return spline_edit
+
+ def updates_for_mouse_move(self, index_of_edit, spline_edit, x, y, difs):
+ if index_of_edit > -1:
+ self.splines[spline_edit][index_of_edit] = [pxToM(x), pxToM(y)]
+
+ if index_of_edit == 5:
+ self.splines[spline_edit][
+ index_of_edit -
+ 2] = self.splines[spline_edit][index_of_edit - 2] + difs
+ self.splines[spline_edit][
+ index_of_edit -
+ 1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+ if index_of_edit == 0:
+ self.splines[spline_edit][
+ index_of_edit +
+ 2] = self.splines[spline_edit][index_of_edit + 2] + difs
+ self.splines[spline_edit][
+ index_of_edit +
+ 1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+ if index_of_edit == 4:
+ self.splines[spline_edit][
+ index_of_edit -
+ 1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+ if index_of_edit == 1:
+ self.splines[spline_edit][
+ index_of_edit +
+ 1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+ return self.splineExtrapolate(spline_edit)
+
+ def update_lib_spline(self):
+ self.libsplines = []
+ array = np.zeros(shape=(6, 2), dtype=float)
+ for points in self.splines:
+ for j, point in enumerate(points):
+ array[j, 0] = point[0]
+ array[j, 1] = point[1]
+ spline = Spline(np.ascontiguousarray(np.transpose(array)))
+ self.libsplines.append(spline)
+
+ def getSplines(self):
+ return self.splines
+
+ def resetSplines(self):
+ self.splines = []
+
+ def setUpSplines(self, newSplines):
+ self.splines = newSplines
+
+ def setSplines(self, spline_edit, index_of_edit, x, y):
+ self.splines[spline_edit][index_of_edit] = [x, y]
+
+ def add_point(self, x, y):
+ if (len(self.points) < 6):
+ self.points.append([pxToM(x), pxToM(y)])
+ if (len(self.points) == 6):
+ self.splines.append(np.array(self.points))
+ self.points = []
+ self.update_lib_spline()
+ return True
+
+ def extrapolate(self, point1, point2,
+ point3): # where point3 is 3rd to last point
+ self.points.append(point1)
+ self.points.append(point1 * 2 - point2)
+ self.points.append(point3 + point1 * 4 - point2 * 4)
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index e69d874..b428c9a 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -15,7 +15,6 @@
from frc971.control_loops.python import drivetrain
from frc971.control_loops.python import controls
import y2016.control_loops.python.drivetrain
-
"""This file is my playground for implementing spline following.
All splines here are cubic bezier splines. See
@@ -73,8 +72,9 @@
if numpy.isscalar(alpha):
alpha = [alpha]
dalpha_matrix = [[
- -3.0 * (1.0 - a)**2.0, 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 *
- (1.0 - a) * a, -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
+ -3.0 * (1.0 - a)**2.0,
+ 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 * (1.0 - a) * a,
+ -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
] for a in alpha]
return control_points * numpy.matrix(dalpha_matrix).T
@@ -97,8 +97,7 @@
ddalpha_matrix = [[
2.0 * 3.0 * (1.0 - a),
-2.0 * 3.0 * (1.0 - a) + -2.0 * 3.0 * (1.0 - a) + 2.0 * 3.0 * a,
- -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a,
- 2.0 * 3.0 * a
+ -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a, 2.0 * 3.0 * a
] for a in alpha]
return control_points * numpy.matrix(ddalpha_matrix).T
@@ -119,10 +118,8 @@
if numpy.isscalar(alpha):
alpha = [alpha]
ddalpha_matrix = [[
- -2.0 * 3.0,
- 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
- -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0,
- 2.0 * 3.0
+ -2.0 * 3.0, 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
+ -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0, 2.0 * 3.0
] for a in alpha]
return control_points * numpy.matrix(ddalpha_matrix).T
@@ -143,7 +140,8 @@
dspline_points = dspline(alpha, control_points)
return numpy.arctan2(
- numpy.array(dspline_points)[1, :], numpy.array(dspline_points)[0, :])
+ numpy.array(dspline_points)[1, :],
+ numpy.array(dspline_points)[0, :])
def dspline_theta(alpha,
@@ -219,8 +217,8 @@
dddy = numpy.array(dddspline_points)[1, :]
return -1.0 / ((dx**2.0 + dy**2.0)**2.0) * (dx * ddy - dy * ddx) * 2.0 * (
- dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy - dy *
- dddx)
+ dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy -
+ dy * dddx)
class Path(object):
@@ -230,7 +228,8 @@
self._control_points = control_points
def spline_velocity(alpha):
- return numpy.linalg.norm(dspline(alpha, self._control_points), axis=0)
+ return numpy.linalg.norm(dspline(alpha, self._control_points),
+ axis=0)
self._point_distances = [0.0]
num_alpha = 100
@@ -240,8 +239,8 @@
# might think.
for alpha in numpy.linspace(0.0, 1.0, num_alpha)[1:]:
self._point_distances.append(
- scipy.integrate.fixed_quad(spline_velocity, alpha, alpha + 1.0
- / (num_alpha - 1.0))[0] +
+ scipy.integrate.fixed_quad(spline_velocity, alpha, alpha +
+ 1.0 / (num_alpha - 1.0))[0] +
self._point_distances[-1])
def distance_to_alpha(self, distance):
@@ -256,7 +255,8 @@
if numpy.isscalar(distance):
return numpy.array([self._distance_to_alpha_scalar(distance)])
else:
- return numpy.array([self._distance_to_alpha_scalar(d) for d in distance])
+ return numpy.array(
+ [self._distance_to_alpha_scalar(d) for d in distance])
def _distance_to_alpha_scalar(self, distance):
"""Helper to compute alpha for a distance for a single scalar."""
@@ -264,15 +264,17 @@
return 0.0
elif distance >= self.length():
return 1.0
- after_index = numpy.searchsorted(
- self._point_distances, distance, side='right')
+ after_index = numpy.searchsorted(self._point_distances,
+ distance,
+ side='right')
before_index = after_index - 1
# Linearly interpolate alpha from our (sorted) distance table.
return (distance - self._point_distances[before_index]) / (
self._point_distances[after_index] -
- self._point_distances[before_index]) * (1.0 / (
- len(self._point_distances) - 1.0)) + float(before_index) / (
+ self._point_distances[before_index]) * (
+ 1.0 /
+ (len(self._point_distances) - 1.0)) + float(before_index) / (
len(self._point_distances) - 1.0)
def length(self):
@@ -287,8 +289,8 @@
# TODO(austin): need a better name...
def dxy(self, distance):
"""Returns the xy velocity as a function of distance."""
- dspline_point = dspline(
- self.distance_to_alpha(distance), self._control_points)
+ dspline_point = dspline(self.distance_to_alpha(distance),
+ self._control_points)
return dspline_point / numpy.linalg.norm(dspline_point, axis=0)
# TODO(austin): need a better name...
@@ -298,8 +300,7 @@
dspline_points = dspline(alpha, self._control_points)
ddspline_points = ddspline(alpha, self._control_points)
- norm = numpy.linalg.norm(
- dspline_points, axis=0)**2.0
+ norm = numpy.linalg.norm(dspline_points, axis=0)**2.0
return ddspline_points / norm - numpy.multiply(
dspline_points, (numpy.array(dspline_points)[0, :] *
@@ -309,10 +310,9 @@
def theta(self, distance, dspline_points=None):
"""Returns the heading as a function of distance."""
- return spline_theta(
- self.distance_to_alpha(distance),
- self._control_points,
- dspline_points=dspline_points)
+ return spline_theta(self.distance_to_alpha(distance),
+ self._control_points,
+ dspline_points=dspline_points)
def dtheta(self, distance, dspline_points=None, ddspline_points=None):
"""Returns the angular velocity as a function of distance."""
@@ -327,9 +327,14 @@
return dtheta_points / numpy.linalg.norm(dspline_points, axis=0)
- def dtheta_dt(self, distance, velocity, dspline_points=None, ddspline_points=None):
+ def dtheta_dt(self,
+ distance,
+ velocity,
+ dspline_points=None,
+ ddspline_points=None):
"""Returns the angular velocity as a function of time."""
- return self.dtheta(distance, dspline_points, ddspline_points) * velocity
+ return self.dtheta(distance, dspline_points,
+ ddspline_points) * velocity
def ddtheta(self,
distance,
@@ -400,8 +405,8 @@
return 0.0
return (f(t, y) - a0) / y
- return (RungeKutta(integrablef, v, x, dx) - v
- ) + numpy.sqrt(2.0 * a0 * dx + v * v)
+ return (RungeKutta(integrablef, v, x, dx) - v) + numpy.sqrt(2.0 * a0 * dx +
+ v * v)
class Trajectory(object):
@@ -409,8 +414,8 @@
distance_count):
self._path = path
self._drivetrain = drivetrain
- self.distances = numpy.linspace(0.0,
- self._path.length(), distance_count)
+ self.distances = numpy.linspace(0.0, self._path.length(),
+ distance_count)
self._longitudal_accel = longitudal_accel
self._lateral_accel = lateral_accel
@@ -460,9 +465,10 @@
C = K3 * v * v + K4 * v
# Note: K345 are not quite constant over the step, but we are going
# to assume they are for now.
- accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
- K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
- (-12.0 - C[1, 0]) / K5[1, 0]]
+ accelerations = [
+ (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+ (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+ ]
maxa = -float('inf')
for a in accelerations:
U = K5 * a + K3 * v * v + K4 * v
@@ -473,8 +479,8 @@
# Constrain the longitudinal acceleration to keep in a pseudo friction
# circle. This will make it so we don't floor it while in a turn and
# cause extra wheel slip.
- long_accel = numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
- 2.0) * self._longitudal_accel
+ long_accel = numpy.sqrt(1.0 - (
+ lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
return min(long_accel, maxa)
def forward_pass(self, plan):
@@ -503,9 +509,10 @@
C = K3 * v * v + K4 * v
# Note: K345 are not quite constant over the step, but we are going
# to assume they are for now.
- accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
- K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
- (-12.0 - C[1, 0]) / K5[1, 0]]
+ accelerations = [
+ (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+ (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+ ]
mina = float('inf')
for a in accelerations:
U = K5 * a + K3 * v * v + K4 * v
@@ -516,8 +523,8 @@
# Constrain the longitudinal acceleration to keep in a pseudo friction
# circle. This will make it so we don't floor it while in a turn and
# cause extra wheel slip.
- long_accel = -numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
- 2.0) * self._longitudal_accel
+ long_accel = -numpy.sqrt(1.0 - (
+ lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
return max(long_accel, mina)
def backward_pass(self, plan):
@@ -546,8 +553,9 @@
if distance > self.distances[-1]:
distance = self.distances[-1]
else:
- after_index = numpy.searchsorted(
- self.distances, distance, side='right')
+ after_index = numpy.searchsorted(self.distances,
+ distance,
+ side='right')
before_index = after_index - 1
vforwards = integrate_accel_for_distance(
@@ -586,14 +594,15 @@
return U
def goal_state(self, distance, velocity):
- width = (
- self._drivetrain.robot_radius_l + self._drivetrain.robot_radius_r)
+ width = (self._drivetrain.robot_radius_l +
+ self._drivetrain.robot_radius_r)
goal = numpy.matrix(numpy.zeros((5, 1)))
goal[0:2, :] = self._path.xy(distance)
goal[2, :] = self._path.theta(distance)
- Ter = numpy.linalg.inv(numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
+ Ter = numpy.linalg.inv(
+ numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
goal[3:5, :] = Ter * numpy.matrix(
[[velocity], [self._path.dtheta_dt(distance, velocity)]])
return goal
@@ -618,25 +627,23 @@
# Compute theta and the two derivatives
theta = spline_theta(alphas, control_points, dspline_points=dspline_points)
- dtheta = dspline_theta(
- alphas, control_points, dspline_points=dspline_points)
- ddtheta = ddspline_theta(
- alphas,
- control_points,
- dspline_points=dspline_points,
- dddspline_points=dddspline_points)
+ dtheta = dspline_theta(alphas,
+ control_points,
+ dspline_points=dspline_points)
+ ddtheta = ddspline_theta(alphas,
+ control_points,
+ dspline_points=dspline_points,
+ dddspline_points=dddspline_points)
# Plot the control points and the spline.
pylab.figure()
- pylab.plot(
- numpy.array(control_points)[0, :],
- numpy.array(control_points)[1, :],
- '-o',
- label='control')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(numpy.array(control_points)[0, :],
+ numpy.array(control_points)[1, :],
+ '-o',
+ label='control')
+ pylab.plot(numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
# For grins, confirm that the double integral of the acceleration (with
@@ -657,9 +664,9 @@
# Integrate up the spline velocity and heading to confirm that given a
# velocity (as a function of the spline parameter) and angle, we will move
# from the starting point to the ending point.
- thetaint_plot = numpy.zeros((len(alphas),))
+ thetaint_plot = numpy.zeros((len(alphas), ))
thetaint = theta[0]
- dthetaint_plot = numpy.zeros((len(alphas),))
+ dthetaint_plot = numpy.zeros((len(alphas), ))
dthetaint = dtheta[0]
thetaint_plot[0] = thetaint
dthetaint_plot[0] = dthetaint
@@ -670,15 +677,14 @@
for i in range(len(alphas) - 1):
dalpha = alphas[i + 1] - alphas[i]
txint += dalpha * numpy.linalg.norm(
- dspline_points[:, i]) * numpy.matrix(
- [[numpy.cos(theta[i])], [numpy.sin(theta[i])]])
+ dspline_points[:, i]) * numpy.matrix([[numpy.cos(theta[i])],
+ [numpy.sin(theta[i])]])
txint_plot[:, i + 1] = txint
thetaint += dalpha * dtheta[i]
dthetaint += dalpha * ddtheta[i]
thetaint_plot[i + 1] = thetaint
dthetaint_plot[i + 1] = dthetaint
-
# Now plot x, dx/dalpha, ddx/ddalpha, dddx/dddalpha, and integrals thereof
# to perform consistency checks.
pylab.figure()
@@ -709,11 +715,9 @@
pylab.plot(alphas, ddtheta, label='ddtheta')
pylab.plot(alphas, thetaint_plot, label='thetai')
pylab.plot(alphas, dthetaint_plot, label='dthetai')
- pylab.plot(
- alphas,
- numpy.linalg.norm(
- numpy.array(dspline_points), axis=0),
- label='velocity')
+ pylab.plot(alphas,
+ numpy.linalg.norm(numpy.array(dspline_points), axis=0),
+ label='velocity')
# Now, repeat as a function of path length as opposed to alpha
path = Path(control_points)
@@ -780,12 +784,11 @@
longitudal_accel = 3.0
lateral_accel = 2.0
- trajectory = Trajectory(
- path,
- drivetrain=velocity_drivetrain,
- longitudal_accel=longitudal_accel,
- lateral_accel=lateral_accel,
- distance_count=500)
+ trajectory = Trajectory(path,
+ drivetrain=velocity_drivetrain,
+ longitudal_accel=longitudal_accel,
+ lateral_accel=lateral_accel,
+ distance_count=500)
vmax = numpy.inf
vmax = 10.0
@@ -827,14 +830,16 @@
while spline_state[0, 0] < path.length():
t += dt
+
def along_spline_diffeq(t, x):
_, v, a = trajectory.ff_accel(backward_accel_plan, x[0, 0])
return numpy.matrix([[x[1, 0]], [a]])
- spline_state = RungeKutta(along_spline_diffeq,
- spline_state.copy(), t, dt)
+ spline_state = RungeKutta(along_spline_diffeq, spline_state.copy(), t,
+ dt)
- d, v, a = trajectory.ff_accel(backward_accel_plan, length_plan_x[-1][0, 0])
+ d, v, a = trajectory.ff_accel(backward_accel_plan,
+ length_plan_x[-1][0, 0])
length_plan_v.append(v)
length_plan_a.append(a)
@@ -877,9 +882,10 @@
[path.dtheta_dt(xva_plan[0, i], xva_plan[1, i])[0]]])
vel_lr = numpy.linalg.inv(Ter) * vel_omega_r
- state_plan[:, i] = numpy.matrix(
- [[x_r], [y_r], [theta_r], [vel_lr[0, 0]], [vel_lr[1, 0]]])
- u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i])
+ state_plan[:, i] = numpy.matrix([[x_r], [y_r], [theta_r],
+ [vel_lr[0, 0]], [vel_lr[1, 0]]])
+ u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0,
+ i])
Q = numpy.matrix(
numpy.diag([
@@ -906,16 +912,17 @@
linear_velocity = -kMinVelocity
width = (velocity_drivetrain.robot_radius_l +
- velocity_drivetrain.robot_radius_r)
- A_linearized_continuous = numpy.matrix([[
- 0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta, 0.5 *
- costheta
- ], [
- 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta, 0.5 *
- sintheta
- ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0]])
+ velocity_drivetrain.robot_radius_r)
+ A_linearized_continuous = numpy.matrix(
+ [[
+ 0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta,
+ 0.5 * costheta
+ ],
+ [
+ 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta,
+ 0.5 * sintheta
+ ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
+ [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]])
A_linearized_continuous[3:5, 3:5] = velocity_drivetrain.A_continuous
B_linearized_continuous = numpy.matrix(numpy.zeros((5, 2)))
B_linearized_continuous[3:5, :] = velocity_drivetrain.B_continuous
@@ -942,15 +949,15 @@
velocity_drivetrain.robot_radius_r)
accel = (velocity_drivetrain.A_continuous * velocity +
velocity_drivetrain.B_continuous * U)
- return numpy.matrix(
- [[numpy.cos(theta) * linear_velocity],
- [numpy.sin(theta) * linear_velocity], [angular_velocity],
- [accel[0, 0]], [accel[1, 0]]])
+ return numpy.matrix([[numpy.cos(theta) * linear_velocity],
+ [numpy.sin(theta) * linear_velocity],
+ [angular_velocity], [accel[0, 0]],
+ [accel[1, 0]]])
full_us[:, i] = U
- state = RungeKutta(lambda t, x: spline_diffeq(U, t, x),
- state, i * dt, dt)
+ state = RungeKutta(lambda t, x: spline_diffeq(U, t, x), state, i * dt,
+ dt)
pylab.figure()
pylab.plot(length_plan_t, numpy.array(xva_plan)[0, :], label='x')
@@ -963,17 +970,14 @@
pylab.legend()
pylab.figure()
- pylab.plot(
- numpy.array(states)[0, :],
- numpy.array(states)[1, :],
- label='robot')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(numpy.array(states)[0, :],
+ numpy.array(states)[1, :],
+ label='robot')
+ pylab.plot(numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
-
def a(_, x):
return 2.0
return 2.0 + 0.0001 * x
diff --git a/frc971/control_loops/python/spline_drawing.py b/frc971/control_loops/python/spline_drawing.py
new file mode 100644
index 0000000..4e8b04b
--- /dev/null
+++ b/frc971/control_loops/python/spline_drawing.py
@@ -0,0 +1,292 @@
+from color import Color, palette
+import cairo
+
+import numpy as np
+from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+PIXELS_ON_SCREEN = 800
+
+X_TRANSLATE = 0
+Y_TRANSLATE = 0
+
+
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
+
+
+def mToPx(m):
+ return (m * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
+
+
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+ """Draws a cross with fixed dimensions in pixel space."""
+ set_color(cr, color)
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+ set_color(cr, palette["LIGHT_GREY"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+ """Draws a x with fixed dimensions in pixel space."""
+ length_px = length_px1 / np.sqrt(2)
+ set_color(cr, color)
+ cr.move_to(x - length_px, y - length_px)
+ cr.line_to(x + length_px, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y + length_px)
+ cr.line_to(x + length_px, y - length_px)
+ cr.stroke()
+ set_color(cr, palette["LIGHT_GREY"])
+
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+ cr.scale(widtha, -heighta)
+ cr.show_text(text)
+ cr.scale(widthb, -heightb)
+
+
+def redraw(needs_redraw, window):
+ print("Redrew")
+ if not needs_redraw:
+ window.queue_draw()
+
+
+def draw_HAB(cr):
+ print("WENT IN")
+ # BASE Constants
+ X_BASE = 0 + X_TRANSLATE #(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+ BACKWALL_X = X_TRANSLATE
+ LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+ # HAB Levels 2 and 3 called in variables backhab
+ # draw loading stations
+ cr.move_to(0, LOADING_Y)
+ cr.line_to(mToPx(0.6), LOADING_Y)
+ cr.move_to(0, -1.0 * LOADING_Y)
+ cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+
+ BACKWALL_X = X_TRANSLATE
+
+ # HAB Levels 2 and 3 called in variables backhab
+
+ WIDTH_BACKHAB = mToPx(1.2192)
+
+ Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+ BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+ BACKHAB_LV3_LENGTH = mToPx(1.2192)
+ Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+ Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+ # HAB LEVEL 1
+ X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+ WIDTH_LV1_BOX = mToPx(0.90805)
+ LENGTH_LV1_BOX = mToPx(1.6256)
+
+ Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+ # Ramp off Level 1
+ X_RAMP = X_LV1_BOX
+
+ Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+ WIDTH_TOP_RAMP = mToPx(1.20015)
+ LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+ X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+ Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+ LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+ WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+ Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+ # Side Bars to Hold in balls
+ X_BARS = BACKWALL_X
+ WIDTH_BARS = WIDTH_BACKHAB
+ LENGTH_BARS = mToPx(0.574675)
+
+ Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+ Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+ set_color(cr, palette["BLACK"])
+ cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+ LENGTH_MIDDLE_RAMP)
+ cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.stroke()
+
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+ cr.stroke()
+
+ #draw 0, 0
+ set_color(cr, palette["BLACK"])
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(8.2296 / 2.0))
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(-8.2296 / 2.0))
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE + mToPx(8.2296), Y_TRANSLATE)
+
+ cr.stroke()
+
+
+def draw_rockets(cr):
+ # BASE Constants
+ X_BASE = 0 + X_TRANSLATE + mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+ near_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.4))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+ far_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+ print("FAR SIDE ROCKET")
+ #print(far_side_rocket_center)
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+ near_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.4))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+ far_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+ X_BASE = 0 + X_TRANSLATE #mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ cr.stroke()
+
+
+def draw_cargo_ship(cr):
+ # BASE Constants
+
+ X_BASE = X_TRANSLATE + mToPx(5.59435)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ FRONT_PEG_DELTA_Y = mToPx(0.276352)
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.4), Y_BASE + FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.4), Y_BASE - FRONT_PEG_DELTA_Y)
+
+ SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+ SIDE_PEG_X = X_BASE + mToPx(1.148842)
+ SIDE_PEG_DX = mToPx(0.55245)
+
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+ mToPx(1.41605))
+
+ X_BASE = X_TRANSLATE + mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
+ mToPx(2.43205), mToPx(1.41605))
+
+ cr.stroke()
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
new file mode 100755
index 0000000..2fd7964
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph.py
@@ -0,0 +1,100 @@
+#!/usr/bin/python3
+import gi
+from path_edit import *
+import numpy as np
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk, Gtk, GLib
+import cairo
+
+class GridWindow(Gtk.Window):
+ def method_connect(self, event, cb):
+ def handler(obj, *args):
+ cb(*args)
+
+ print("Method_connect ran")
+ self.connect(event, handler)
+
+ def mouse_move(self, event):
+ #Changes event.x and event.y to be relative to the center.
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.mouse_move(event)
+ self.queue_draw()
+
+ def button_press(self, event):
+ print("button press activated")
+ o_x = event.x
+ o_y = event.y
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = 2 * self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.do_button_press(event)
+ event.x = o_x
+ event.y = o_y
+
+ def key_press(self, event):
+ print("key press activated")
+ self.drawing_area.do_key_press(event, self.file_name_box.get_text())
+ self.queue_draw()
+
+ def configure(self, event):
+ print("configure activated")
+ self.drawing_area.window_shape = (event.width, event.height)
+
+ # handle submitting a constraint
+ def on_submit_click(self, widget):
+ self.drawing_area.inConstraint = int(self.constraint_box.get_text())
+ self.drawing_area.inValue = int(self.value_box.get_text())
+
+ def __init__(self):
+ Gtk.Window.__init__(self)
+
+ self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
+
+ flowBox = Gtk.FlowBox()
+ flowBox.set_valign(Gtk.Align.START)
+ flowBox.set_selection_mode(Gtk.SelectionMode.NONE)
+
+ flowBox.set_valign(Gtk.Align.START)
+
+ self.add(flowBox)
+
+ container = Gtk.Fixed()
+ flowBox.add(container)
+
+ self.eventBox = Gtk.EventBox()
+ container.add(self.eventBox)
+
+ self.eventBox.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+ | Gdk.EventMask.BUTTON_RELEASE_MASK
+ | Gdk.EventMask.POINTER_MOTION_MASK
+ | Gdk.EventMask.SCROLL_MASK
+ | Gdk.EventMask.KEY_PRESS_MASK)
+
+ #add the graph box
+ self.drawing_area = GTK_Widget()
+ self.eventBox.add(self.drawing_area)
+
+ self.method_connect("key-release-event", self.key_press)
+ self.method_connect("button-release-event", self.button_press)
+ self.method_connect("configure-event", self.configure)
+ self.method_connect("motion_notify_event", self.mouse_move)
+
+ self.file_name_box = Gtk.Entry()
+ self.file_name_box.set_size_request(200, 40)
+
+ self.file_name_box.set_text("File")
+ self.file_name_box.set_editable(True)
+
+ container.put(self.file_name_box, 0, 0)
+
+ self.show_all()
+
+
+window = GridWindow()
+RunApp()
diff --git a/frc971/control_loops/python/spline_graph_usage.txt b/frc971/control_loops/python/spline_graph_usage.txt
new file mode 100644
index 0000000..a698298
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph_usage.txt
@@ -0,0 +1,22 @@
+Spline Graph Usage
+
+Run with: bazel run spline_graph -c opt "YOUR SCREEN SIZE"
+
+On Start Place down first 6 points
+Then you go into editing mode, where you can drag your points around
+Then to create another spline, press p and the first three points will be autogenerated
+
+To export type in your file in the top right textfield ex.test.json
+It will be exported to spline_jsons/YOURFILE
+
+Notes:
+If importing gi, gi requires #!/usr/bin/python3 be at the top of the file
+
+This was built on top of BaseWindow
+
+Known Bugs:
+libspline throws errors about viable friction limits and viable voltage limits not overlapping
+bazel run lib_spline_test -c opt
+John commented those lines out
+
+Upon closing, reopens window
diff --git a/frc971/control_loops/python/spline_jsons/test.json b/frc971/control_loops/python/spline_jsons/test.json
new file mode 100644
index 0000000..09737e0
--- /dev/null
+++ b/frc971/control_loops/python/spline_jsons/test.json
@@ -0,0 +1 @@
+[[[1.6680953371273437, -3.4989921261100347], [1.8841223685193285, -3.3960504631707944], [2.0281410171518672, -3.272708402385707], [2.192647314562578, -3.1286520914802223], [2.3984302077132025, -3.0360669258966317], [2.5936689198479943, -2.912724865111545]], [[2.5936689198479943, -2.912724865111545], [2.788907631982786, -2.789382804326458], [2.973602163101745, -2.635283848339876], [3.190230535451773, -2.5732992795250538], [3.396013428602397, -2.511627935280236], [3.6117404172199103, -2.4705139150185405]]]
\ No newline at end of file
diff --git a/frc971/control_loops/python/spline_writer.py b/frc971/control_loops/python/spline_writer.py
new file mode 100644
index 0000000..df622f3
--- /dev/null
+++ b/frc971/control_loops/python/spline_writer.py
@@ -0,0 +1,72 @@
+import os
+import numpy as np
+
+
+class SplineWriter(object):
+ def __init__(self, namespaces=None, filename="auto_splines.cc"):
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'autonomous']
+
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
+
+ self._namespace_end = '\n'.join([
+ '} // namespace %s' % name for name in reversed(self._namespaces)
+ ])
+
+ self.filename_ = filename
+
+ def make_string_list(self, list):
+ string = "{{"
+ for i in range(0, len(list)):
+ if i == len(list) - 1:
+ string += str(list[i])
+ else:
+ string += str(list[i]) + ", "
+ return string + "}}"
+
+ def Write(self, spline_name, spline_idx, control_points):
+ """Writes the cc file to the file named cc_file."""
+ xs = control_points[:, 0]
+ ys = control_points[:, 1]
+ spline_count = (len(xs) - 6) / 5 + 1
+ with open(self.filename_, 'a') as fd:
+ fd.write(self._namespace_start)
+ #write the name
+ fd.write("\n\n::frc971::MultiSpline " + spline_name + "() {\n")
+ # write the objs, at the moment assumes a single constraint, needs fixing
+ fd.write(
+ "\t::frc971::MultiSpline spline;\n\t::frc971::Constraint constraints;\n"
+ )
+ fd.write(
+ "\tconstraints.constraint_type = 0;\n\tconstraints.value = 0;\n"
+ )
+ fd.write(
+ "\tconstraints.start_distance = 0;\n\tconstraints.end_distance = 0;\n"
+ )
+ fd.write('\n')
+ fd.write("\tspline.spline_idx = " + str(spline_idx) +
+ ";\n\tspline.spline_count = " + str(spline_count) + ";\n")
+ fd.write("\tspline.spline_x = " + self.make_string_list(xs) +
+ ";\n\tspline.spline_y = " + self.make_string_list(ys) +
+ ";\n")
+ fd.write(
+ "\tspline.constraints = {{constraints}};\n\treturn spline;\n")
+ fd.write("}")
+ fd.write("\n\n")
+ fd.write(self._namespace_end)
+ fd.write("\n\n")
+
+
+def main():
+ writer = SplineWriter()
+ points = np.array([[1.0, 2.0], [3.0, 4.0], [5.0, 6.0], [7.0, 8.0],
+ [9.0, 10.0], [11.0, 12.0]])
+ spline_name = "test_spline"
+ spline_idx = 1
+ writer.Write(spline_name, spline_idx, points)
+
+
+main()