Spline UI: improve json exports
Change-Id: I2aac5c6eda3122a627bb99185d0db2b17b3bcd16
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
index 8f94e3f..4444a72 100644
--- a/frc971/control_loops/python/points.py
+++ b/frc971/control_loops/python/points.py
@@ -8,6 +8,18 @@
self.points = [] # Holds all points not yet in spline
self.libsplines = [] # Formatted for libspline library usage
self.splines = [] # Formatted for drawing
+ self.constraints = [ # default constraints
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ }, {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ }, {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
def getPoints(self):
return self.points
@@ -18,6 +30,21 @@
def getLibsplines(self):
return self.libsplines
+ def setConstraint(self, id, value):
+ for constraint in self.constraints:
+ if constraint["constraint_type"] == id:
+ constraint["value"] = value
+ break
+
+ def addConstraintsToTrajectory(self, trajectory):
+ for constraint in self.constraints:
+ if constraint["constraint_type"] == "VOLTAGE":
+ trajectory.SetVoltageLimit(constraint["value"])
+ elif constraint["constraint_type"] == "LATERAL_ACCELERATION":
+ trajectory.SetLateralAcceleration(constraint["value"])
+ elif constraint["constraint_type"] == "LONGITUDINAL_ACCELERATION":
+ trajectory.SetLongitudinalAcceleration(constraint["value"])
+
def splineExtrapolate(self, o_spline_edit):
spline_edit = o_spline_edit
if not spline_edit == len(self.splines) - 1:
@@ -82,15 +109,41 @@
spline = Spline(np.ascontiguousarray(np.transpose(array)))
self.libsplines.append(spline)
+ def toMultiSpline(self):
+ multi_spline = {
+ "spline_count": 0,
+ "spline_x": [],
+ "spline_y": [],
+ "constraints": self.constraints,
+ }
+ for points in self.splines:
+ multi_spline["spline_count"] += 1
+ for j, point in enumerate(points):
+ if j == 0 and multi_spline["spline_count"] > 1:
+ continue # skip overlapping points
+ multi_spline["spline_x"].append(point[0])
+ multi_spline["spline_y"].append(point[1])
+ return multi_spline
+
+ def fromMultiSpline(self, multi_spline):
+ self.constraints = multi_spline["constraints"]
+ self.splines = []
+ self.points = []
+
+ i = 0
+ for j in range(multi_spline["spline_count"]):
+ # get the last point of the last spline
+ # and read in another 6 points
+ for i in range(i, i + 6):
+ self.points.append(
+ [multi_spline["spline_x"][i], multi_spline["spline_y"][i]])
+ self.splines.append(np.array(self.points))
+ self.points = []
+ self.update_lib_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]