Merge "Convert joystick_reader over to event loops."
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index acb472c..a0ee825 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -167,6 +167,8 @@
name = "path_edit",
srcs = [
"path_edit.py",
+ "basic_window.py",
+ "color.py",
],
visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
@@ -174,7 +176,6 @@
":python_init",
":libspline",
"@python_gtk",
- ":basic_window",
],
)
@@ -182,11 +183,24 @@
name = "basic_window",
srcs = [
"basic_window.py",
- "color.py",
+ "color.py"
],
+ visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
deps = [
":python_init",
"@python_gtk",
],
)
+
+py_library(
+ name = "color",
+ srcs = [
+ "color.py",
+ ],
+ visibility = ["//visibility:public"],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ ],
+)
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index 7a34b3a..78324a3 100644
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -4,7 +4,6 @@
from gi.repository import GLib
from gi.repository import Gdk
from gi.repository import GdkX11
-from color import Color, palette
import cairo
identity = cairo.Matrix()
@@ -46,31 +45,13 @@
# Create a GTK+ widget on which we will draw using Cairo
class BaseWindow(Gtk.DrawingArea):
- def method_connect(self, event, cb):
- def handler(obj, *args):
- cb(*args)
-
- self.window.connect(event, handler)
# Draw in response to an expose-event
def __init__(self):
super(BaseWindow, self).__init__()
- self.window = Gtk.Window()
- self.window.set_title("DrawingArea")
- self.window.connect("destroy", quit_main_loop)
- self.window.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)
- self.method_connect("key-press-event", self.do_key_press)
- self.method_connect("button-press-event",
- self._do_button_press_internal)
- self.method_connect("configure-event", self._do_configure)
+ #self.window.connect("destroy", quit_main_loop)
- self.set_size_request(640, 400)
- self.window.add(self)
- self.window.show_all()
+ self.set_size_request(640, 600)
self.center = (0, 0)
self.shape = (640, 400)
self.needs_redraw = False
@@ -100,29 +81,3 @@
# Handle the expose-event by drawing
def handle_draw(self, cr):
pass
-
- def do_key_press(self, event):
- pass
-
- def _do_button_press_internal(self, event):
- o_x = event.x
- o_y = event.y
- x = event.x - self.window_shape[0] / 2
- y = self.window_shape[1] / 2 - event.y
- scale = self.get_current_scale()
- event.x = x / scale + self.center[0]
- event.y = y / scale + self.center[1]
- self.do_button_press(event)
- event.x = o_x
- event.y = o_y
-
- def do_button_press(self, event):
- pass
-
- def _do_configure(self, event):
- self.window_shape = (event.width, event.height)
-
- def redraw(self):
- if not self.needs_redraw:
- self.needs_redraw = True
- self.window.queue_draw()
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 7a77637..e9e6c5f 100644
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -12,7 +12,7 @@
try:
libSpline = ct.cdll.LoadLibrary(
os.path.join(path, 'frc971/control_loops/drivetrain/spline.so'))
- except OSError, e:
+ except (OSError):
pass
# Define required output types.
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index a0aa621..3a0a323 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -1,34 +1,38 @@
#!/usr/bin/python3
-
from __future__ import print_function
import os
+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')
-from gi.repository import Gdk
+from gi.repository import Gdk, Gtk, GLib
import cairo
-
import enum
-import csv # For writing to csv files
+import csv # For writing to csv files
from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
LENGTH_OF_FIELD = 323.65
PIXELS_ON_SCREEN = 300
+
def pxToIn(p):
- return p*LENGTH_OF_FIELD/PIXELS_ON_SCREEN
+ return p * LENGTH_OF_FIELD / PIXELS_ON_SCREEN
+
def inToPx(i):
- return i*PIXELS_ON_SCREEN/LENGTH_OF_FIELD
+ return (i * PIXELS_ON_SCREEN / LENGTH_OF_FIELD)
+
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)
@@ -41,6 +45,7 @@
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)
@@ -54,9 +59,12 @@
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))
+ draw_px_cross(cr, p[i][0], p[i][1], size, Color(
+ 0, np.sqrt(0.2 * i), 0))
+
class Mode(enum.Enum):
kViewing = 0
@@ -64,14 +72,48 @@
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)
-# Create a GTK+ widget on which we will draw using Cairo
+
+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):
+ """Create a GTK+ widget on which we will draw using Cairo"""
+
def __init__(self):
super(GTK_Widget, self).__init__()
@@ -82,8 +124,6 @@
self.x = 0
self.y = 0
- self.switch = True
-
# update list of control points
self.point_selected = False
# self.adding_spline = False
@@ -91,20 +131,39 @@
self.new_point = []
# For the editing mode
- self.index_of_edit = -1 # Can't be zero beause array starts at 0
+ self.index_of_edit = -1 # Can't be zero beause array starts at 0
self.held_x = 0
+ self.spline_edit = -1
- # Theo take them from here?
+ self.curves = []
+
+ self.colors = []
+
+ for c in palette:
+ self.colors.append(palette[c])
+
self.selected_points = []
-
+ self.splines = []
+ self.spline = []
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)<4):
- self.selected_points.append([x,y])
+ if (len(self.selected_points) < 6):
+ self.selected_points.append([x, 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, this needs to be redone with proper distances"""
+ """set extents on images"""
+
def reinit_extents(self):
self.extents_x_min = -800
self.extents_x_max = 800
@@ -123,10 +182,55 @@
def get_nearest_point(self):
return self.all_controls[self.get_index_of_nearest_point()]
- # Handle the expose-event by updating the Window and drawing
+ 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, i in enumerate(points.curve):
+ # pythagorean
+ distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[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.spline[self.spline_edit].addConstraint(
+ self.inStart, self.inEnd, self.inConstraint, self.inValue)
+ 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 pxToIn(distance)
+
+ # Handle the expose-event by updating the Window and drawing
def handle_draw(self, cr):
- print(self.new_point)
- print("SELF.POINT_SELECTED: " + str(self.point_selected))
+ # print(self.new_point)
+ # print("SELF.POINT_SELECTED: " + str(self.point_selected))
# begin drawing
# Fill the background color of the window with grey
@@ -146,25 +250,40 @@
cr.show_text('Press "i" to import')
set_color(cr, Color(0.3, 0.3, 0.3))
- cr.rectangle(-150,-150,300,300)
+ cr.rectangle(-450, -150, 300, 300)
cr.fill()
set_color(cr, palette["BLACK"])
- cr.rectangle(-150,-150,300,300)
+ cr.rectangle(-450, -150, 300, 300)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
- cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+ cr.rectangle((inToPx(140 - 161.825) - 300), inToPx(76.575), inToPx(56),
+ inToPx(-153.15))
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
- cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+ cr.rectangle((inToPx(161.825 - 24) - 300), inToPx(90), inToPx(24),
+ inToPx(-180))
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
set_color(cr, Color(0.2, 0.2, 0.2))
- cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+ cr.rectangle(
+ inToPx(140 - 161.825) - 300, inToPx(76.575), inToPx(56),
+ inToPx(-153.15))
cr.fill()
- cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+ cr.rectangle(
+ inToPx(161.825 - 24) - 300, inToPx(90), inToPx(24), inToPx(-180))
cr.fill()
+ y = 0
+ for x, i in enumerate(self.spline):
+ for j in i.constraints:
+ cr.move_to(-650, -y * 10 + 320)
+ set_color(cr, palette["BLACK"])
+ display_text(
+ cr, str("Spline " + str(x) + ": " + str(j.toString())),
+ 0.5, 0.5, 2, 2)
+ y += 1
+
# update all the things
if self.mode == Mode.kViewing:
@@ -172,32 +291,24 @@
cr.move_to(-300, 170)
cr.show_text("VIEWING")
set_color(cr, palette["GREY"])
- # its gonna check for points_added from button_press_action
- # The behavior of the click is such that it runs twice
- # This is consistant with graph_edit.py which someone smart wrote
- # So I'm just going to delete the last element in order to not get
- # repeating points
+
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 item in self.selected_points:
+ # print(str(item))
for i, point in enumerate(self.selected_points):
- print("I: " + str(i))
+ # print("I: " + str(i))
draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1]-15)
+ cr.move_to(point[0], point[1] - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
- if self.mode == Mode.kPlacing:
+ 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"])
- # its gonna check for points_added from button_press_action
- # The behavior of the click is such that it runs twice
- # This is consistant with graph_edit.py which someone smart wrote
- # So I'm just going to delete the last element in order to not get
- # repeating points
+
if len(self.selected_points) > 0:
print("SELECTED_POINTS: " + str(len(self.selected_points)))
print("ITEMS:")
@@ -206,59 +317,76 @@
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)
+ cr.move_to(point[0], point[1] - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
- if(i==3):
- self.mode = Mode.kEditing
elif self.mode == Mode.kEditing:
set_color(cr, palette["BLACK"])
cr.move_to(-300, 170)
display_text(cr, "EDITING", 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))
- 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)
+ if len(self.splines) > 0:
+ # print("Splines: " + str(len(self.splines)))
+ # print("ITEMS:")
+ 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] = 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(
+ 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)
+
+ 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, point[0], 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, point[0], point[1], 10,
+ self.colors[spline])
+ cr.move_to(point[0], point[1] - 15)
+ display_text(cr, str(i), 0.5, 0.5, 2, 2)
elif self.mode == Mode.kExporting:
set_color(cr, palette["BLACK"])
cr.move_to(-300, 170)
display_text(cr, "VIEWING", 1, 1, 1, 1)
set_color(cr, palette["GREY"])
- #its gonna check for points_added from button_press_action
- # The behavior of the click is such that it runs twice
- # This is consistant with graph_edit.py which someone smart wrote
- # So I'm just going to delete the last element in order to not get
- # repeating points
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 item in self.selected_points:
+ # print(str(item))
for i, point in enumerate(self.selected_points):
- print("I: " + str(i))
+ # print("I: " + str(i))
draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1]-15)
+ cr.move_to(point[0], point[1] - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
elif self.mode == Mode.kImporting:
set_color(cr, palette["BLACK"])
cr.move_to(-300, 170)
display_text(cr, "VIEWING", 1, 1, 1, 1)
set_color(cr, palette["GREY"])
- # its gonna check for points_added from button_press_action
- # The behavior of the click is such that it runs twice
- # This is consistant with graph_edit.py which someone smart wrote
- # So I'm just going to delete the last element in order to not get
- # repeating points
if len(self.selected_points) > 0:
print("SELECTED_POINTS: " + str(len(self.selected_points)))
print("ITEMS:")
@@ -267,9 +395,25 @@
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)
+ cr.move_to(point[0], point[1] - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ 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)
cr.paint_with_alpha(.65)
@@ -277,7 +421,7 @@
def do_key_press(self, event):
keyval = Gdk.keyval_to_lower(event.keyval)
- print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+ # print("Gdk.KEY_" + Gdk.keyval_name(keyval))
if keyval == Gdk.KEY_q:
print("Found q key and exiting.")
quit_main_loop()
@@ -285,11 +429,14 @@
self.mode = Mode.kExporting
# Will export to csv file
with open('points_for_pathedit.csv', mode='w') as points_file:
- writer = csv.writer(points_file, delimiter=',', quotechar='"',
- quoting=csv.QUOTE_MINIMAL)
+ writer = csv.writer(
+ points_file,
+ delimiter=',',
+ quotechar='"',
+ quoting=csv.QUOTE_MINIMAL)
for item in self.selected_points:
writer.writerow([str(item[0]), str(item[1])])
- print("Wrote: " + str(item[0]) + " " + str(item[1]))
+ print("Wrote: " + str(item[0]) + " " + str(item[1]))
if keyval == Gdk.KEY_i:
self.mode = Mode.kImporting
# import from csv file
@@ -298,53 +445,95 @@
reader = csv.reader(points_file, delimiter=',')
for row in reader:
self.add_point(float(row[0]), float(row[1]))
- print("Added: " + row[0] + " " + row[1])
+ print("Added: " + row[0] + " " + row[1])
+ 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)
- self.redraw()
+ if keyval == Gdk.KEY_c:
+ self.mode = Mode.kConstraint
def button_press_action(self):
- if self.switch:
- self.switch = False
- if self.mode == Mode.kPlacing:
- #Check that the point clicked is on the field
- if(self.x<150 and self.x>-150 and self.y <150 and self.y >-150):
- self.add_point(self.x, self.y)
- if self.mode == Mode.kEditing:
- # Now after index_of_edit is not -1, the point is selected, so
- # user can click for new point
- print("INDEX OF EDIT: " + str(self.index_of_edit))
+ 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)
+ 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] = [
+ self.x, self.y
+ ]
- if self.index_of_edit > -1 and self.held_x != self.x:
- print("INDEX OF EDIT: " + str(self.index_of_edit))
- self.selected_points[self.index_of_edit] = [self.x, self.y]
- self.index_of_edit = -1
- else:
- print("mode == 2")
- # Get clicked point
- # Find nearest
- # Move nearest to clicked
- cur_p = [self.x, self.y]
- print("CUR_P: " + str(self.x) + " " + str(self.y))
- # What I wanna do is get each point
- # Get the distance between each for x and y
- # Save the index of the point closest
- nearest = 1000
- index = 0
- for ind, i in enumerate(self.selected_points):
- # pythagorean
- distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
- if distance < nearest:
- nearest = distance
- index = ind
- print("Nearest: " + str(nearest))
- print("Index: " + str(index))
- self.index_of_edit = index
- self.held_x = self.x
- else:
- self.switch = True
+ 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
- self.redraw()
+ self.spline[spline_edit].point = self.splines[spline_edit]
+ self.spline[spline_edit].math()
+ 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[spline_edit].point = self.splines[spline_edit]
+ self.spline[spline_edit].math()
+
+ self.spline[self.spline_edit].edit(self.index_of_edit,
+ [self.x, self.y])
+ self.index_of_edit = -1
+ self.spline_edit = -1
+ else:
+ print("mode == 2")
+ # Get clicked point
+ # Find nearest
+ # Move nearest to clicked
+ cur_p = [self.x, 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 = 50
+ index_of_closest = 0
+ for index_splines, points in enumerate(self.splines):
+ 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:
+ nearest = distance
+ index_of_closest = index_points
+ print("Nearest: " + str(nearest))
+ print("Index: " + str(index_of_closest))
+ 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")
@@ -353,6 +542,100 @@
self.button_press_action()
+class GridWindow(Gtk.Window):
+ def method_connect(self, event, cb):
+ def handler(obj, *args):
+ cb(*args)
-silly = GTK_Widget()
+ print("Method_connect ran")
+ self.connect(event, handler)
+
+ 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()
+ 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.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)
+
+ 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)
+
+ 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)
+
+ # Constraint Boxes
+
+ self.start_box = Gtk.Entry()
+ self.start_box.set_size_request(100, 20)
+
+ 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)
+
+ self.show_all()
+
+
+window = GridWindow()
basic_window.RunApp()
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index bc3ac04..00cd77d 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -276,3 +276,22 @@
"//aos/logging",
],
)
+
+cc_library(
+ name = "sensor_reader",
+ srcs = [
+ "sensor_reader.cc",
+ ],
+ hdrs = [
+ "sensor_reader.h"
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ "//aos/stl_mutex",
+ "//aos/time:time",
+ "//aos:init",
+ "//third_party:wpilib",
+ ":dma",
+ ":dma_edge_counting",
+ ],
+)
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
new file mode 100644
index 0000000..24d33f8
--- /dev/null
+++ b/frc971/wpilib/sensor_reader.cc
@@ -0,0 +1,106 @@
+#include "frc971/wpilib/sensor_reader.h"
+
+#include "aos/init.h"
+#include "aos/util/compiler_memory_barrier.h"
+#include "frc971/wpilib/ahal/DigitalInput.h"
+#include "frc971/wpilib/ahal/Utility.h"
+
+namespace frc971 {
+namespace wpilib {
+
+SensorReader::SensorReader() {}
+
+void SensorReader::set_drivetrain_left_encoder(
+ ::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_left_encoder_ = ::std::move(encoder);
+}
+
+void SensorReader::set_drivetrain_right_encoder(
+ ::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_right_encoder_ = ::std::move(encoder);
+}
+
+// All of the DMA-related set_* calls must be made before this, and it
+// doesn't hurt to do all of them.
+void SensorReader::set_dma(::std::unique_ptr<DMA> dma) {
+ dma_synchronizer_.reset(
+ new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+}
+
+void SensorReader::set_pwm_trigger(
+ ::std::unique_ptr<frc::DigitalInput> pwm_trigger) {
+ medium_encoder_filter_.Add(pwm_trigger.get());
+ pwm_trigger_ = ::std::move(pwm_trigger);
+}
+
+void SensorReader::RunPWMDetecter() {
+ ::aos::SetCurrentThreadRealtimePriority(41);
+
+ pwm_trigger_->RequestInterrupts();
+ // Rising edge only.
+ pwm_trigger_->SetUpSourceEdge(true, false);
+
+ monotonic_clock::time_point last_posedge_monotonic =
+ monotonic_clock::min_time;
+
+ while (run_) {
+ auto ret = pwm_trigger_->WaitForInterrupt(1.0, true);
+ if (ret == frc::InterruptableSensorBase::WaitResult::kRisingEdge) {
+ // Grab all the clocks.
+ const double pwm_fpga_time = pwm_trigger_->ReadRisingTimestamp();
+
+ aos_compiler_memory_barrier();
+ const double fpga_time_before = frc::GetFPGATime() * 1e-6;
+ aos_compiler_memory_barrier();
+ const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ aos_compiler_memory_barrier();
+ const double fpga_time_after = frc::GetFPGATime() * 1e-6;
+ aos_compiler_memory_barrier();
+
+ const double fpga_offset =
+ (fpga_time_after + fpga_time_before) / 2.0 - pwm_fpga_time;
+
+ // Compute when the edge was.
+ const monotonic_clock::time_point monotonic_edge =
+ monotonic_now - chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(fpga_offset));
+
+ LOG(DEBUG, "Got PWM pulse %f spread, %f offset, %lld trigger\n",
+ fpga_time_after - fpga_time_before, fpga_offset,
+ monotonic_edge.time_since_epoch().count());
+
+ // Compute bounds on the timestep and sampling times.
+ const double fpga_sample_length = fpga_time_after - fpga_time_before;
+ const chrono::nanoseconds elapsed_time =
+ monotonic_edge - last_posedge_monotonic;
+
+ last_posedge_monotonic = monotonic_edge;
+
+ // Verify that the values are sane.
+ if (fpga_sample_length > 2e-5 || fpga_sample_length < 0) {
+ continue;
+ }
+ if (fpga_offset < 0 || fpga_offset > 0.00015) {
+ continue;
+ }
+ if (elapsed_time > chrono::microseconds(5050) + chrono::microseconds(4) ||
+ elapsed_time < chrono::microseconds(5050) - chrono::microseconds(4)) {
+ continue;
+ }
+ // Good edge!
+ {
+ ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
+ last_tick_time_monotonic_timepoint_ = last_posedge_monotonic;
+ last_period_ = elapsed_time;
+ }
+ } else {
+ LOG(INFO, "PWM triggered %d\n", ret);
+ }
+ }
+ pwm_trigger_->CancelInterrupts();
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
new file mode 100644
index 0000000..3abe1b0
--- /dev/null
+++ b/frc971/wpilib/sensor_reader.h
@@ -0,0 +1,66 @@
+#ifndef FRC971_WPILIB_SENSOR_READER_H_
+#define FRC971_WPILIB_SENSOR_READER_H_
+
+#include <atomic>
+#include <chrono>
+
+#include "aos/stl_mutex/stl_mutex.h"
+#include "aos/time/time.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DigitalInput.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/dma_edge_counting.h"
+
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+
+namespace frc971 {
+namespace wpilib {
+
+class SensorReader {
+ public:
+ SensorReader();
+
+ // Sets the left drivetrain encoder.
+ void set_drivetrain_left_encoder(::std::unique_ptr<frc::Encoder> encoder);
+
+ // Sets the right drivetrain encoder.
+ void set_drivetrain_right_encoder(::std::unique_ptr<frc::Encoder> encoder);
+
+ // Sets the dma.
+ void set_dma(::std::unique_ptr<DMA> dma);
+
+ // Sets the pwm trigger.
+ void set_pwm_trigger(::std::unique_ptr<frc::DigitalInput> pwm_trigger);
+
+ // Stops the pwm trigger on the next iteration.
+ void Quit() { run_ = false; }
+
+ protected:
+ // Uses the pwm trigger to find the pwm cycle width and offset for that
+ // iteration.
+ void RunPWMDetecter();
+
+ ::std::unique_ptr<frc::DigitalInput> pwm_trigger_;
+
+ frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
+ hall_filter_;
+
+ // Mutex to manage access to the period and tick time variables.
+ ::aos::stl_mutex tick_time_mutex_;
+ monotonic_clock::time_point last_tick_time_monotonic_timepoint_ =
+ monotonic_clock::min_time;
+ chrono::nanoseconds last_period_ = chrono::microseconds(5050);
+
+ ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
+
+ ::std::atomic<bool> run_{true};
+
+ ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_SENSOR_READER_H_
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index 14f8290..af158de 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -176,6 +176,7 @@
srcs_version = "PY3",
deps = [
"//frc971/control_loops/python:basic_window",
+ "//frc971/control_loops/python:color",
":python_init",
"@python_gtk",
],
diff --git a/y2018/control_loops/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index a2cf414..fa9c9e5 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -2,20 +2,20 @@
from __future__ import print_function
import os
-import basic_window
-from color import Color, palette
+from frc971.control_loops.python import basic_window
+from frc971.control_loops.python.color import Color, palette
import random
import gi
import numpy
gi.require_version('Gtk', '3.0')
-from gi.repository import Gdk
+from gi.repository import Gdk, Gtk
import cairo
import graph_generate
from graph_generate import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend
from graph_generate import back_to_xy_loop, subdivide_theta, to_theta_loop
from graph_generate import l1, l2, joint_center
-from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+from frc971.control_loops.python.basic_window import OverrideMatrix, identity, quit_main_loop, set_color
import shapely
from shapely.geometry import Polygon
@@ -157,7 +157,7 @@
abs(dpx * pdx + dpy * pdy)
-#
+
def closest_segment(lines, pt):
c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
for i in range(1, len(lines)):
@@ -175,6 +175,21 @@
def __init__(self):
super(Silly, self).__init__()
+ self.window = Gtk.Window()
+ self.window.set_title("DrawingArea")
+
+ self.window.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)
+ self.method_connect("key-press-event", self.do_key_press)
+ self.method_connect("button-press-event",
+ self._do_button_press_internal)
+ self.method_connect("configure-event", self._do_configure)
+ self.window.add(self)
+ self.window.show_all()
+
self.theta_version = False
self.reinit_extents()
@@ -188,6 +203,38 @@
self.spline_edit = 0
self.edit_control1 = True
+ def do_key_press(self, event):
+ pass
+
+ def _do_button_press_internal(self, event):
+ o_x = event.x
+ o_y = event.y
+ x = event.x - self.window_shape[0] / 2
+ y = self.window_shape[1] / 2 - event.y
+ scale = self.get_current_scale()
+ event.x = x / scale + self.center[0]
+ event.y = y / scale + self.center[1]
+ self.do_button_press(event)
+ event.x = o_x
+ event.y = o_y
+
+ def do_button_press(self, event):
+ pass
+
+ def _do_configure(self, event):
+ self.window_shape = (event.width, event.height)
+
+ def redraw(self):
+ if not self.needs_redraw:
+ self.needs_redraw = True
+ self.window.queue_draw()
+
+ def method_connect(self, event, cb):
+ def handler(obj, *args):
+ cb(*args)
+
+ self.window.connect(event, handler)
+
def reinit_extents(self):
if self.theta_version:
self.extents_x_min = -numpy.pi * 2
diff --git a/y2019/BUILD b/y2019/BUILD
index 8705da0..7c987f6 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -46,6 +46,7 @@
"//frc971/wpilib:logging_queue",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
+ "//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 5d78649..dcdf00b 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -23,9 +23,7 @@
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
-#include "aos/util/compiler_memory_barrier.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
@@ -33,16 +31,13 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
-#include "frc971/wpilib/buffered_pcm.h"
-#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
-#include "frc971/wpilib/dma_edge_counting.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
-#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_interface.h"
#include "frc971/wpilib/wpilib_robot_base.h"
@@ -105,7 +100,7 @@
"medium encoders are too fast");
// Class to send position messages with sensor readings to our loops.
-class SensorReader {
+class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader() {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
@@ -121,100 +116,6 @@
hall_filter_.SetPeriodNanoSeconds(100000);
}
- // Left drivetrain side.
- void set_drivetrain_left_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- drivetrain_left_encoder_ = ::std::move(encoder);
- }
-
- // Right drivetrain side.
- void set_drivetrain_right_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- drivetrain_right_encoder_ = ::std::move(encoder);
- }
-
- void set_pwm_trigger(::std::unique_ptr<frc::DigitalInput> pwm_trigger) {
- medium_encoder_filter_.Add(pwm_trigger.get());
- pwm_trigger_ = ::std::move(pwm_trigger);
- }
-
- // All of the DMA-related set_* calls must be made before this, and it
- // doesn't hurt to do all of them.
- void set_dma(::std::unique_ptr<DMA> dma) {
- dma_synchronizer_.reset(
- new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
- }
-
- void RunPWMDetecter() {
- ::aos::SetCurrentThreadRealtimePriority(41);
-
- pwm_trigger_->RequestInterrupts();
- // Rising edge only.
- pwm_trigger_->SetUpSourceEdge(true, false);
-
- monotonic_clock::time_point last_posedge_monotonic =
- monotonic_clock::min_time;
-
- while (run_) {
- auto ret = pwm_trigger_->WaitForInterrupt(1.0, true);
- if (ret == frc::InterruptableSensorBase::WaitResult::kRisingEdge) {
- // Grab all the clocks.
- const double pwm_fpga_time = pwm_trigger_->ReadRisingTimestamp();
-
- aos_compiler_memory_barrier();
- const double fpga_time_before = frc::GetFPGATime() * 1e-6;
- aos_compiler_memory_barrier();
- const monotonic_clock::time_point monotonic_now =
- monotonic_clock::now();
- aos_compiler_memory_barrier();
- const double fpga_time_after = frc::GetFPGATime() * 1e-6;
- aos_compiler_memory_barrier();
-
- const double fpga_offset =
- (fpga_time_after + fpga_time_before) / 2.0 - pwm_fpga_time;
-
- // Compute when the edge was.
- const monotonic_clock::time_point monotonic_edge =
- monotonic_now - chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(fpga_offset));
-
- LOG(DEBUG, "Got PWM pulse %f spread, %f offset, %lld trigger\n",
- fpga_time_after - fpga_time_before, fpga_offset,
- monotonic_edge.time_since_epoch().count());
-
- // Compute bounds on the timestep and sampling times.
- const double fpga_sample_length = fpga_time_after - fpga_time_before;
- const chrono::nanoseconds elapsed_time =
- monotonic_edge - last_posedge_monotonic;
-
- last_posedge_monotonic = monotonic_edge;
-
- // Verify that the values are sane.
- if (fpga_sample_length > 2e-5 || fpga_sample_length < 0) {
- continue;
- }
- if (fpga_offset < 0 || fpga_offset > 0.00015) {
- continue;
- }
- if (elapsed_time >
- chrono::microseconds(5050) + chrono::microseconds(4) ||
- elapsed_time <
- chrono::microseconds(5050) - chrono::microseconds(4)) {
- continue;
- }
- // Good edge!
- {
- ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
- last_tick_time_monotonic_timepoint_ = last_posedge_monotonic;
- last_period_ = elapsed_time;
- }
- } else {
- LOG(INFO, "PWM triggered %d\n", ret);
- }
- }
- pwm_trigger_->CancelInterrupts();
- }
-
void operator()() {
::aos::SetCurrentThreadName("SensorReader");
@@ -282,34 +183,12 @@
dma_synchronizer_->RunIteration();
}
- void Quit() { run_ = false; }
private:
- double encoder_translate(int32_t value, double counts_per_revolution,
- double ratio) {
- return static_cast<double>(value) / counts_per_revolution * ratio *
- (2.0 * M_PI);
- }
-
int32_t my_pid_;
- // Mutex to manage access to the period and tick time variables.
- ::aos::stl_mutex tick_time_mutex_;
- monotonic_clock::time_point last_tick_time_monotonic_timepoint_ =
- monotonic_clock::min_time;
- chrono::nanoseconds last_period_ = chrono::microseconds(5050);
-
- ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
-
- frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
- hall_filter_;
-
::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
drivetrain_right_encoder_;
-
- ::std::unique_ptr<frc::DigitalInput> pwm_trigger_;
-
- ::std::atomic<bool> run_{true};
};
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {