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/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()