Merge changes I3f444cd5,If3815bdc,Iac39d224
* changes:
Hide control_loop() inside ControlLoop better
Remove unused queuegroup hash
Remove unused SerializableControlLoop class
diff --git a/aos/containers/sized_array.h b/aos/containers/sized_array.h
index 264d466..0b8b447 100644
--- a/aos/containers/sized_array.h
+++ b/aos/containers/sized_array.h
@@ -30,6 +30,8 @@
using const_reverse_iterator = typename array::const_reverse_iterator;
constexpr SizedArray() = default;
+ SizedArray(const SizedArray &) = default;
+ SizedArray(SizedArray &&) = default;
SizedArray &operator=(const SizedArray &) = default;
SizedArray &operator=(SizedArray &&) = default;
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 8f98953..aee3a37 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -274,7 +274,11 @@
deps = [
":distance_spline",
":spline",
+ ":trajectory",
+ "//aos/logging:implementations",
+ "//aos/network:team_number",
"//third_party/eigen",
+ "//y2019/control_loops/drivetrain:drivetrain_base",
],
linkshared=True,
)
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 117290d..0d14f66 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -1,62 +1,66 @@
#include <vector>
+#include <string>
#include "Eigen/Dense"
+#include "aos/logging/implementations.h"
+#include "aos/network/team_number.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
extern "C" {
- NSpline<6>* NewSpline(double x[6], double y[6]) {
+ // Based on spline.h
+ NSpline<6> *NewSpline(double x[6], double y[6]) {
return new NSpline<6>((::Eigen::Matrix<double, 2, 6>() << x[0], x[1], x[2],
x[3], x[4], x[5], y[0], y[1], y[2], y[3], y[4],
y[5]).finished());
}
- void deleteSpline(NSpline<6>* spline) {
- delete spline;
- }
+ void deleteSpline(NSpline<6> *spline) { delete spline; }
- void SplinePoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->Point(alpha).data();
+ void SplinePoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->Point(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void SplineDPoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->DPoint(alpha).data();
+ void SplineDPoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->DPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void SplineDDPoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->DDPoint(alpha).data();
+ void SplineDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->DDPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void SplineDDDPoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->DDDPoint(alpha).data();
+ void SplineDDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->DDDPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- double SplineTheta(NSpline<6>* spline, double alpha) {
+ double SplineTheta(NSpline<6> *spline, double alpha) {
return spline->Theta(alpha);
}
- double SplineDTheta(NSpline<6>* spline, double alpha) {
+ double SplineDTheta(NSpline<6> *spline, double alpha) {
return spline->DTheta(alpha);
}
- double SplineDDTheta(NSpline<6>* spline, double alpha) {
+ double SplineDDTheta(NSpline<6> *spline, double alpha) {
return spline->DDTheta(alpha);
}
- void SplineControlPoints(NSpline<6>* spline, double* x, double* y) {
+ void SplineControlPoints(NSpline<6> *spline, double *x, double *y) {
auto points = spline->control_points();
// Deal with incorrectly strided matrix.
for (int i = 0; i < 6; ++i) {
@@ -65,7 +69,8 @@
}
}
- DistanceSpline* NewDistanceSpline(Spline** splines, int count) {
+ // Based on distance_spline.h
+ DistanceSpline *NewDistanceSpline(Spline **splines, int count) {
::std::vector<Spline> splines_;
for (int i = 0; i < count; ++i) {
splines_.push_back(*splines[i]);
@@ -73,9 +78,7 @@
return new DistanceSpline(::std::vector<Spline>(splines_));
}
- void deleteDistanceSpline(DistanceSpline* spline) {
- delete spline;
- }
+ void deleteDistanceSpline(DistanceSpline *spline) { delete spline; }
void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
double *val = spline->XY(distance).data();
@@ -116,6 +119,89 @@
double DistanceSplineLength(DistanceSpline *spline) {
return spline->length();
}
+
+ // Based on trajectory.h
+ Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
+ int num_distance) {
+ return new Trajectory(
+ spline, ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
+ num_distance);
+ }
+
+ void deleteTrajectory(Trajectory *t) { delete t; }
+
+ void TrajectorySetLongitudalAcceleration(Trajectory *t, double accel) {
+ t->set_longitudal_acceleration(accel);
+ }
+
+ void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
+ t->set_lateral_acceleration(accel);
+ }
+
+ void TrajectorySetVoltageLimit(Trajectory *t, double limit) {
+ t->set_voltage_limit(limit);
+ }
+
+ void TrajectoryLimitVelocity(Trajectory *t, double start, double end,
+ double max) {
+ t->LimitVelocity(start, end, max);
+ }
+
+ void TrajectoryPlan(Trajectory *t) { t->Plan(); }
+
+ double TrajectoryLength(Trajectory *t) { return t->length(); }
+
+ int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
+
+ // This assumes that res is created in python to be getPathLength() long.
+ // Likely to SEGFAULT otherwise.
+ void Distances(Trajectory *t, double *res) {
+ const ::std::vector<double> &distances = t->Distances();
+ ::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
+ }
+
+ double TrajectoryDistance(Trajectory *t, int index) {
+ return t->Distance(index);
+ }
+
+ // This assumes that res is created in python to be getPathLength() long.
+ // Likely to SEGFAULT otherwise.
+ void TrajectoryGetPlan(Trajectory *t, double *res) {
+ const ::std::vector<double> &velocities = t->plan();
+ ::std::memcpy(res, velocities.data(), sizeof(double) * velocities.size());
+ }
+
+ // Time in in nanoseconds.
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *TrajectoryGetPlanXVAPtr(
+ Trajectory *t, int dt) {
+ return new ::std::vector<::Eigen::Matrix<double, 3, 1>>(
+ t->PlanXVA(::std::chrono::nanoseconds(dt)));
+ }
+
+ void TrajectoryDeleteVector(
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ delete vec;
+ }
+
+ int TrajectoryGetVectorLength(
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ return vec->size();
+ }
+
+ void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
+ double *X, double *V, double *A) {
+ for (size_t i = 0; i < vec->size(); ++i) {
+ X[i] = (*vec)[0][0];
+ V[i] = (*vec)[0][1];
+ A[i] = (*vec)[0][2];
+ }
+ }
+
+ // Util
+ void SetUpLogging() {
+ ::aos::logging::Init();
+ ::aos::network::OverrideTeamNumber(971);
+ }
}
} // namespace drivetrain
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index f2eb9e9..2f72c8f 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -158,3 +158,31 @@
"@matplotlib",
],
)
+
+py_binary(
+ name = "path_edit",
+ srcs = [
+ "path_edit.py",
+ ],
+ visibility = ["//visibility:public"],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ ":libspline",
+ "@python_gtk",
+ ":basic_window",
+ ],
+)
+
+py_library(
+ name = "basic_window",
+ srcs = [
+ "basic_window.py",
+ "color.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ "@python_gtk",
+ ],
+)
diff --git a/y2018/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
similarity index 100%
rename from y2018/control_loops/python/basic_window.py
rename to frc971/control_loops/python/basic_window.py
diff --git a/y2018/control_loops/python/color.py b/frc971/control_loops/python/color.py
similarity index 100%
rename from y2018/control_loops/python/color.py
rename to frc971/control_loops/python/color.py
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index 2c77d62..4e725a0 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -5,7 +5,7 @@
from numpy.testing import *
import unittest
-from libspline import Spline, DistanceSpline
+from libspline import Spline, DistanceSpline, Trajectory
class TestSpline(unittest.TestCase):
def testSimpleSpline(self):
@@ -23,5 +23,29 @@
dSpline = DistanceSpline([spline])
assert_almost_equal(dSpline.Length(), 5 * math.sqrt(2))
+class TestTrajectory(unittest.TestCase):
+ def testTrajectory(self):
+ points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 5.0],
+ [2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
+ spline = Spline(points)
+ dSpline = DistanceSpline([spline])
+ trajectory = Trajectory(dSpline)
+ trajectory.Plan()
+ plan = trajectory.GetPlanXVA(5.05*1e-3)
+ assert(plan.shape == (3, 611))
+
+ def testLimits(self):
+ """ A plan with a lower limit should take longer. """
+ points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 5.0],
+ [2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
+ spline = Spline(points)
+ dSpline = DistanceSpline([spline])
+ trajectory = Trajectory(dSpline)
+ trajectory.LimitVelocity(0, trajectory.Length(), 3)
+ trajectory.Plan()
+ plan = trajectory.GetPlanXVA(5.05*1e-3)
+ self.assertEqual(plan.shape, (3, 644))
+
+
if __name__ == '__main__':
unittest.main()
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index ca20f61..fd44cb4 100644
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -24,10 +24,18 @@
libSpline.DistanceSplineDThetaDt.restype = ct.c_double
libSpline.DistanceSplineDDTheta.restype = ct.c_double
libSpline.DistanceSplineLength.restype = ct.c_double
+libSpline.TrajectoryLength.restype = ct.c_double
+libSpline.TrajectoryDistance.resType = ct.c_double
+# Required for trajectory
+libSpline.SetUpLogging()
class Spline:
- """A wrapper around spline.h/cc through libspline.cc."""
+ """
+ A wrapper around spline.h/cc through libspline.cc.
+ The functions return values parameterized by alpha, a number that varies
+ between 0 and 1 along the length of the spline.
+ """
def __init__(self, points):
assert points.shape == (2, 6)
@@ -83,18 +91,22 @@
def ControlPoints(self):
return self.__points
- def Spline(self):
+ def GetSplinePtr(self):
return self.__spline
class DistanceSpline:
- """A wrapper around distance_spline.h/cc through libdistancespline.cc."""
+ """
+ A wrapper around distance_spline.h/cc through libspline.cc.
+ The functions return values parameterized by the distance along the spline,
+ starting at 0 and and ending at the value returned by Length().
+ """
def __init__(self, splines):
self.__spline = None
spline_ptrs = []
for spline in splines:
- spline_ptrs.append(spline.Spline())
+ spline_ptrs.append(spline.GetSplinePtr())
spline_ptrs = np.array(spline_ptrs)
spline_array = np.ctypeslib.as_ctypes(spline_ptrs)
@@ -141,3 +153,89 @@
def Length(self):
return libSpline.DistanceSplineLength(self.__spline)
+
+ def GetSplinePtr(self):
+ return self.__spline
+
+
+class Trajectory:
+ """A wrapper around trajectory.h/cc through libspline.cc."""
+
+ def __init__(self, distance_spline, vmax=10, num_distance=0):
+ self.__trajectory = libSpline.NewTrajectory(
+ distance_spline.GetSplinePtr(), ct.c_double(vmax), num_distance)
+
+ def __del__(self):
+ libSpline.deleteTrajectory(self.__trajectory)
+
+ def SetLongitudalAcceleration(self, accel):
+ libSpline.TrajectorySetLongitualAcceleration(self.__trajectory,
+ ct.c_double(accel))
+
+ def SetLateralAcceleration(self, accel):
+ libSpline.TrajectorySetLateralAcceleration(self.__trajectory,
+ ct.c_double(accel))
+
+ def SetVoltageLimit(self, limit):
+ libSpline.TrajectorySetVoltageLimit(self.__trajectory,
+ ct.c_double(limit))
+
+ def LimitVelocity(self, start_distance, end_distance, max_vel):
+ libSpline.TrajectoryLimitVelocity(self.__trajectory,
+ ct.c_double(start_distance),
+ ct.c_double(end_distance),
+ ct.c_double(max_vel))
+
+ def Plan(self):
+ """
+ Call this to compute the plan, if any of the limits change, a new plan
+ must be generated.
+ """
+ libSpline.TrajectoryPlan(self.__trajectory)
+
+ def Length(self):
+ return libSpline.TrajectoryLength(self.__trajectory)
+
+ def Distance(self, index):
+ return libSpline.TrajectoryDistance(self.__trajectory, index)
+
+ def Distances(self):
+ """
+ Returns an array of distances used to compute the plan. The linear
+ distance between each distance is equal.
+ """
+ path_length = libSpline.TrajectoryGetPathLength(self.__trajectory)
+ distances = numpy.zeros(path_length)
+ libSpline.TrajectoryDistances(self.__trajectory,
+ np.ctypeslib.as_ctypes(distances))
+ return distances
+
+ def GetPlan(self):
+ """
+ Returns the plan as an array of velocities matched to each distance
+ from Distances.
+ """
+ path_length = libSpline.TrajectoryGetPathLength(self.__trajectory)
+ velocities = numpy.zeros(path_length)
+ libSpline.TrajectoryGetPlan(self.__trajectory,
+ np.ctypeslib.as_ctypes(velocities))
+ return velocities
+
+ def GetPlanXVA(self, dt):
+ """
+ dt is in seconds
+ Returns the position, velocity, and acceleration as a function of time.
+ This is returned as a 3xN numpy array where N is proportional to how
+ long it takes to run the path.
+ This is slow so don't call more than once with the same data.
+ """
+ XVAPtr = libSpline.TrajectoryGetPlanXVAPtr(self.__trajectory, int(dt*1e9))
+ XVALength = libSpline.TrajectoryGetVectorLength(XVAPtr)
+ X = np.zeros(XVALength)
+ V = np.zeros(XVALength)
+ A = np.zeros(XVALength)
+ libSpline.TrajectoryGetPlanXVA(XVAPtr, np.ctypeslib.as_ctypes(X),
+ np.ctypeslib.as_ctypes(V),
+ np.ctypeslib.as_ctypes(A))
+ libSpline.TrajectoryDeleteVector(XVAPtr)
+ return np.vstack([X, V, A])
diff --git a/y2018/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
similarity index 100%
rename from y2018/control_loops/python/path_edit.py
rename to frc971/control_loops/python/path_edit.py
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index 43fa3a8..14f8290 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -163,18 +163,6 @@
],
)
-py_library(
- name = "basic_window",
- srcs = [
- "basic_window.py",
- "color.py",
- ],
- restricted_to = ["//tools:k8"],
- deps = [
- ":python_init",
- "@python_gtk",
- ],
-)
py_binary(
name = "graph_edit",
@@ -187,7 +175,7 @@
restricted_to = ["//tools:k8"],
srcs_version = "PY3",
deps = [
- ":basic_window",
+ "//frc971/control_loops/python:basic_window",
":python_init",
"@python_gtk",
],
@@ -207,22 +195,6 @@
],
)
-py_binary(
- name = "path_edit",
- srcs = [
- "path_edit.py",
- ],
- default_python_version = "PY3",
- legacy_create_init = False,
- restricted_to = ["//tools:k8"],
- srcs_version = "PY3",
- deps = [
- ":basic_window",
- ":python_init",
- "@python_gtk",
- ],
-)
-
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
new file mode 100644
index 0000000..8a8bca3
--- /dev/null
+++ b/y2019/jevois/BUILD
@@ -0,0 +1,11 @@
+cc_library(
+ name = "structures",
+ hdrs = [
+ "structures.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/containers:sized_array",
+ "//third_party/eigen",
+ ],
+)
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
new file mode 100644
index 0000000..d2ab3e5
--- /dev/null
+++ b/y2019/jevois/structures.h
@@ -0,0 +1,103 @@
+#ifndef Y2019_JEVOIS_STRUCTURES_H_
+#define Y2019_JEVOIS_STRUCTURES_H_
+
+#include <stdint.h>
+
+#include <array>
+#include <bitset>
+#include <chrono>
+
+#include "Eigen/Dense"
+
+#include "aos/containers/sized_array.h"
+
+namespace frc971 {
+namespace jevois {
+
+// The overall flow to get data to the roboRIO consists of:
+// 1. Camera captures a frame and grabs an absolute timestamp.
+// 2. Camera processes the frame.
+// 3. Camera grabs another absolute timestamp and subtracts to get a
+// camera_duration.
+// 4. Camera sends the frame via UART to the Teensy.
+// 5. Teensy grabs an absolute timestamp for the first character received.
+// 6. Teensy buffers at most one frame from each camera.
+// 7. roboRIO toggles the CS line.
+// 8. Teensy grabs an absolute timestamp for CS being asserted.
+// 9. Teensy pulls together up to three frames and adds the time each one
+// spent in its queue to the timestamps, and queues them in its SPI
+// peripheral. This all happens before the roboRIO has enough time to start
+// actually moving data.
+// 10. roboRIO transfers the frames, and sends back light/status commands.
+
+using camera_duration = std::chrono::duration<uint8_t, std::milli>;
+
+// This file declares the shared datastructures for the JeVois-based image
+// system.
+//
+// Note that floating-point numbers are represented with floats. This is because
+// a float has more bits than we need for anything, and the Teensy can't handle
+// doubles very quickly. It would probably be quick enough, but it's easier to
+// just use floats and not worry about it.
+
+struct Target {
+ // Distance to the target in meters. Specifically, the distance from the
+ // center of the camera's image plane to the center of the target.
+ float distance;
+
+ // Height of the target in meters. Specifically, the distance from the floor
+ // to the center of the target.
+ float height;
+
+ // Heading of the center of the target in radians. Zero is straight out
+ // perpendicular to the camera's image plane. Images to the left (looking at a
+ // camera image) are at a positive angle.
+ float heading;
+
+ // The angle between the target and the camera's image plane. This is
+ // projected so both are assumed to be perpendicular to the floor. Parallel
+ // targets have a skew of zero. Targets rotated such that their left edge
+ // (looking at a camera image) is closer are at a positive angle.
+ float skew;
+};
+
+// The information extracted from a single camera frame.
+//
+// This is all the information sent from each camera to the Teensy.
+struct Frame {
+ // The top most interesting targets found in this frame.
+ aos::SizedArray<Target, 3> targets;
+
+ // How long ago from the current time this frame was captured.
+ camera_duration age;
+};
+
+// This is all the information sent from the Teensy to each camera.
+struct CameraCalibration {
+ // The calibration matrix. This defines where the camera is pointing.
+ //
+ // TODO(Parker): What are the details on how this is defined.
+ Eigen::Matrix<float, 3, 4> calibration;
+};
+
+// This is all the information the Teensy sends to the RoboRIO.
+struct TeensyToRoborio {
+ // The newest frames received from up to three cameras. These will be the
+ // three earliest-received of all buffered frames.
+ aos::SizedArray<Frame, 3> frames;
+};
+
+// This is all the information the RoboRIO sends to the Teensy.
+struct RoborioToTeensy {
+ // Brightnesses for each of the beacon light channels. 0 is off, 255 is fully
+ // on.
+ std::array<uint8_t, 3> beacon_brightness;
+
+ // Whether the light ring for each camera should be on.
+ std::bitset<5> light_rings;
+};
+
+} // namespace jevois
+} // namespace frc971
+
+#endif // Y2019_JEVOIS_STRUCTURES_H_