Pass voltage through to the trajectory python wrapper.
Add dependencies on python_init at suggestion of Brian.
Change-Id: I4fc7394bbe891115cf05f8604f2dde398f40e75f
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 0d14f66..f931e59 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -149,6 +149,12 @@
void TrajectoryPlan(Trajectory *t) { t->Plan(); }
+ void TrajectoryVoltage(Trajectory *t, double distance, double* res) {
+ double *val = t->FFVoltage(distance).data();
+ res[0] = val[0];
+ res[1] = val[1];
+ }
+
double TrajectoryLength(Trajectory *t) { return t->length(); }
int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 705aa38..9080e9a 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -84,6 +84,9 @@
data = [
"//frc971/control_loops/drivetrain:spline.so",
],
+ deps = [
+ ":python_init",
+ ],
)
py_test(
@@ -93,6 +96,7 @@
],
deps = [
":libspline",
+ ":python_init",
]
)
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index fd44cb4..7a77637 100644
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -193,6 +193,16 @@
"""
libSpline.TrajectoryPlan(self.__trajectory)
+ def Voltage(self, distance):
+ """
+ Returns a pair of voltages for a given distance.
+ Order is left-right.
+ """
+ result = np.zeros(2)
+ libSpline.TrajectoryVoltage(self.__trajectory, ct.c_double(distance),
+ np.ctypeslib.as_ctypes(result))
+ return result
+
def Length(self):
return libSpline.TrajectoryLength(self.__trajectory)