Merge "Moving of basic_window and path_edit to frc971"
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index ff0b600..c139195 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -35,3 +35,21 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "sized_array",
+ hdrs = [
+ "sized_array.h",
+ ],
+)
+
+cc_test(
+ name = "sized_array_test",
+ srcs = [
+ "sized_array_test.cc",
+ ],
+ deps = [
+ ":sized_array",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/aos/containers/sized_array.h b/aos/containers/sized_array.h
new file mode 100644
index 0000000..0b8b447
--- /dev/null
+++ b/aos/containers/sized_array.h
@@ -0,0 +1,110 @@
+#ifndef AOS_CONTAINERS_SIZED_ARRAY_H_
+#define AOS_CONTAINERS_SIZED_ARRAY_H_
+
+#include <array>
+
+namespace aos {
+
+// An array along with a variable size. This is a simple variable-size container
+// with a fixed maximum size.
+//
+// Note that it default-constructs N T instances at construction time. This
+// simplifies the internal bookkeeping a lot (I believe this can be
+// all-constexpr in C++17), but makes it a poor choice for complex T.
+template <typename T, size_t N>
+class SizedArray {
+ private:
+ using array = std::array<T, N>;
+
+ public:
+ using value_type = typename array::value_type;
+ using size_type = typename array::size_type;
+ using difference_type = typename array::difference_type;
+ using reference = typename array::reference;
+ using const_reference = typename array::const_reference;
+ using pointer = typename array::pointer;
+ using const_pointer = typename array::const_pointer;
+ using iterator = typename array::iterator;
+ using const_iterator = typename array::const_iterator;
+ using reverse_iterator = typename array::reverse_iterator;
+ 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;
+
+ reference at(size_t i) {
+ check_index(i);
+ return array_.at(i);
+ }
+ const_reference at(size_t i) const {
+ check_index(i);
+ return array_.at(i);
+ }
+
+ reference operator[](size_t i) { return array_[i]; }
+ const_reference operator[](size_t i) const { return array_[i]; }
+
+ reference front() { return array_.front(); }
+ const_reference front() const { return array_.front(); }
+
+ reference back() { return array_[size_ - 1]; }
+ const_reference back() const { return array_[size_ - 1]; }
+
+ T *data() { return array_.data(); }
+ const T *data() const { return array_.data(); }
+
+ iterator begin() { return array_.begin(); }
+ const_iterator begin() const { return array_.begin(); }
+ const_iterator cbegin() const { return array_.cbegin(); }
+
+ iterator end() { return array_.begin() + size_; }
+ const_iterator end() const { return array_.begin() + size_; }
+ const_iterator cend() const { return array_.cbegin() + size_; }
+
+ reverse_iterator rbegin() { return array_.rend() - size_; }
+ const_reverse_iterator rbegin() const { return array_.rend() - size_; }
+ const_reverse_iterator crbegin() const { return array_.crend() - size_; }
+
+ reverse_iterator rend() { return array_.rend(); }
+ const_reverse_iterator rend() const { return array_.rend(); }
+ const_reverse_iterator crend() const { return array_.crend(); }
+
+ bool empty() const { return size_ == 0; }
+ bool full() const { return size() == max_size(); }
+
+ size_t size() const { return size_; }
+ constexpr size_t max_size() const { return array_.max_size(); }
+
+ void push_back(const T &t) {
+ array_.at(size_) = t;
+ ++size_;
+ }
+ void push_back(T &&t) {
+ array_.at(size_) = std::move(t);
+ ++size_;
+ }
+
+ void pop_back() {
+ if (empty()) {
+ __builtin_trap();
+ }
+ --size_;
+ }
+
+ private:
+ void check_index(size_t i) const {
+ if (i >= size_) {
+ __builtin_trap();
+ }
+ }
+
+ array array_;
+ size_t size_ = 0;
+};
+
+} // namespace aos
+
+#endif // AOS_CONTAINERS_SIZED_ARRAY_H_
diff --git a/aos/containers/sized_array_test.cc b/aos/containers/sized_array_test.cc
new file mode 100644
index 0000000..04a8e41
--- /dev/null
+++ b/aos/containers/sized_array_test.cc
@@ -0,0 +1,139 @@
+#include "aos/containers/sized_array.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+// Tests the various ways of accessing elements.
+TEST(SizedArrayTest, ElementAccess) {
+ SizedArray<int, 5> a;
+ a.push_back(9);
+ a.push_back(7);
+ a.push_back(1);
+ EXPECT_EQ(9, a[0]);
+ EXPECT_EQ(7, a[1]);
+ EXPECT_EQ(1, a[2]);
+ EXPECT_EQ(9, a.data()[0]);
+ EXPECT_EQ(7, a.data()[1]);
+ EXPECT_EQ(1, a.data()[2]);
+ EXPECT_EQ(9, a.at(0));
+ EXPECT_EQ(7, a.at(1));
+ EXPECT_EQ(1, a.at(2));
+ EXPECT_EQ(9, a.front());
+ EXPECT_EQ(1, a.back());
+
+ a.pop_back();
+ EXPECT_EQ(9, a.front());
+ EXPECT_EQ(7, a.back());
+}
+
+// Tests the accessors that don't access data.
+TEST(SizedArrayTest, Accessors) {
+ SizedArray<int, 5> a;
+ EXPECT_TRUE(a.empty());
+ EXPECT_FALSE(a.full());
+ EXPECT_EQ(0u, a.size());
+ EXPECT_EQ(5u, a.max_size());
+
+ a.push_back(9);
+ EXPECT_FALSE(a.empty());
+ EXPECT_FALSE(a.full());
+ EXPECT_EQ(1u, a.size());
+ EXPECT_EQ(5u, a.max_size());
+
+ a.push_back(9);
+ EXPECT_FALSE(a.empty());
+ EXPECT_FALSE(a.full());
+ EXPECT_EQ(2u, a.size());
+ EXPECT_EQ(5u, a.max_size());
+
+ a.push_back(9);
+ EXPECT_FALSE(a.empty());
+ EXPECT_FALSE(a.full());
+ EXPECT_EQ(3u, a.size());
+ EXPECT_EQ(5u, a.max_size());
+
+ a.push_back(9);
+ EXPECT_FALSE(a.empty());
+ EXPECT_FALSE(a.full());
+ EXPECT_EQ(4u, a.size());
+ EXPECT_EQ(5u, a.max_size());
+
+ a.push_back(9);
+ EXPECT_FALSE(a.empty());
+ EXPECT_TRUE(a.full());
+ EXPECT_EQ(5u, a.size());
+ EXPECT_EQ(5u, a.max_size());
+}
+
+// Tests the various kinds of iterator.
+TEST(SizedArrayTest, Iterators) {
+ SizedArray<int, 5> a;
+ EXPECT_EQ(a.begin(), a.end());
+ EXPECT_EQ(a.cbegin(), a.cend());
+ EXPECT_EQ(a.rbegin(), a.rend());
+ EXPECT_EQ(a.crbegin(), a.crend());
+ a.push_back(9);
+ a.push_back(7);
+ a.push_back(1);
+
+ {
+ auto iterator = a.begin();
+ ASSERT_NE(iterator, a.end());
+ EXPECT_EQ(9, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.end());
+ EXPECT_EQ(7, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.end());
+ EXPECT_EQ(1, *iterator);
+ ++iterator;
+ EXPECT_EQ(iterator, a.end());
+ }
+
+ {
+ auto iterator = a.cbegin();
+ ASSERT_NE(iterator, a.cend());
+ EXPECT_EQ(9, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.cend());
+ EXPECT_EQ(7, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.cend());
+ EXPECT_EQ(1, *iterator);
+ ++iterator;
+ EXPECT_EQ(iterator, a.cend());
+ }
+
+ {
+ auto iterator = a.rbegin();
+ ASSERT_NE(iterator, a.rend());
+ EXPECT_EQ(1, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.rend());
+ EXPECT_EQ(7, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.rend());
+ EXPECT_EQ(9, *iterator);
+ ++iterator;
+ EXPECT_EQ(iterator, a.rend());
+ }
+
+ {
+ auto iterator = a.crbegin();
+ ASSERT_NE(iterator, a.crend());
+ EXPECT_EQ(1, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.crend());
+ EXPECT_EQ(7, *iterator);
+ ++iterator;
+ ASSERT_NE(iterator, a.crend());
+ EXPECT_EQ(9, *iterator);
+ ++iterator;
+ EXPECT_EQ(iterator, a.crend());
+ }
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 03e93fd..bebecd0 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -65,7 +65,7 @@
};
// The internal state of a zeroing estimator.
-struct AbsoluteEstimatorState {
+struct PotAndAbsoluteEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
bool error;
// If the joint has seen an index pulse and is zeroed.
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/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 3bcd6c2..d1356da 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -113,7 +113,7 @@
float feedforwards_power;
// State of the estimator.
- .frc971.AbsoluteEstimatorState estimator_state;
+ .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
};
struct IndexProfiledJointStatus {
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index e5dd199..8d375f8 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -164,7 +164,9 @@
duration=1.0,
use_profile=True,
kick_time=0.5,
- kick_magnitude=0.0):
+ kick_magnitude=0.0,
+ max_velocity=10.0,
+ max_acceleration=70.0):
"""Runs the plant with an initial condition and goal.
Args:
@@ -177,6 +179,8 @@
duration: float, time in seconds to run the simulation for.
kick_time: float, time in seconds to kick the robot.
kick_magnitude: float, disturbance in volts to apply.
+ max_velocity: float, The maximum velocity for the profile.
+ max_acceleration: float, The maximum acceleration for the profile.
"""
t_plot = []
x_plot = []
@@ -197,8 +201,8 @@
(plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
profile = TrapezoidProfile(plant.dt)
- profile.set_maximum_acceleration(70.0)
- profile.set_maximum_velocity(10.0)
+ profile.set_maximum_acceleration(max_acceleration)
+ profile.set_maximum_velocity(max_velocity)
profile.SetGoal(goal[0, 0])
U_last = numpy.matrix(numpy.zeros((1, 1)))
@@ -326,11 +330,13 @@
kick_magnitude=2.0)
-def PlotMotion(params, R):
+def PlotMotion(params, R, max_velocity=10.0, max_acceleration=70.0):
"""Plots a trapezoidal motion.
Args:
R: numpy.matrix(2, 1), the goal,
+ max_velocity: float, The max velocity of the profile.
+ max_acceleration: float, The max acceleration of the profile.
"""
plant = AngularSystem(params, params.name)
controller = IntegralAngularSystem(params, params.name)
@@ -346,7 +352,9 @@
controller=controller,
observer=observer,
duration=2.0,
- use_profile=True)
+ use_profile=True,
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteAngularSystem(params, plant_files, controller_files, year_namespaces):
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/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 7129e32..322e41a 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -171,7 +171,8 @@
use_profile=True,
kick_time=0.5,
kick_magnitude=0.0,
- max_velocity=0.3):
+ max_velocity=0.3,
+ max_acceleration=10.0):
"""Runs the plant with an initial condition and goal.
Args:
@@ -185,6 +186,7 @@
kick_time: float, time in seconds to kick the robot.
kick_magnitude: float, disturbance in volts to apply.
max_velocity: float, the max speed in m/s to profile.
+ max_acceleration: float, the max acceleration in m/s/s to profile.
"""
t_plot = []
x_plot = []
@@ -205,7 +207,7 @@
(plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
profile = TrapezoidProfile(plant.dt)
- profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_acceleration(max_acceleration)
profile.set_maximum_velocity(max_velocity)
profile.SetGoal(goal[0, 0])
@@ -334,12 +336,13 @@
kick_magnitude=2.0)
-def PlotMotion(params, R, max_velocity=0.3):
+def PlotMotion(params, R, max_velocity=0.3, max_acceleration=10.0):
"""Plots a trapezoidal motion.
Args:
R: numpy.matrix(2, 1), the goal,
max_velocity: float, The max velocity of the profile.
+ max_acceleration: float, The max acceleration of the profile.
"""
plant = LinearSystem(params, params.name)
controller = IntegralLinearSystem(params, params.name)
@@ -356,7 +359,8 @@
observer=observer,
duration=2.0,
use_profile=True,
- max_velocity=max_velocity)
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteLinearSystem(params, plant_files, controller_files, year_namespaces):
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 14d26c9..7bf2b27 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -3,20 +3,13 @@
#include <algorithm>
#include <cmath>
#include <limits>
+#include <numeric>
#include <vector>
#include "frc971/zeroing/wrap.h"
namespace frc971 {
namespace zeroing {
-namespace {
-
-bool compare_encoder(const PotAndAbsolutePosition &left,
- const PotAndAbsolutePosition &right) {
- return left.encoder < right.encoder;
-}
-
-} // namespace
PotAndIndexPulseZeroingEstimator::PotAndIndexPulseZeroingEstimator(
const constants::PotAndIndexPulseZeroingConstants &constants)
@@ -253,15 +246,16 @@
return r;
}
-PotAndAbsEncoderZeroingEstimator::PotAndAbsEncoderZeroingEstimator(
+PotAndAbsoluteEncoderZeroingEstimator::PotAndAbsoluteEncoderZeroingEstimator(
const constants::PotAndAbsoluteEncoderZeroingConstants &constants)
: constants_(constants) {
relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
offset_samples_.reserve(constants_.average_filter_size);
+ buffered_samples_.reserve(constants_.moving_buffer_size);
Reset();
}
-void PotAndAbsEncoderZeroingEstimator::Reset() {
+void PotAndAbsoluteEncoderZeroingEstimator::Reset() {
first_offset_ = 0.0;
pot_relative_encoder_offset_ = 0.0;
offset_ = 0.0;
@@ -293,7 +287,7 @@
// of samples and check that the buffered samples are not different than the
// zeroing threshold. At any point that the samples differ too much, do not
// update estimates based on those samples.
-void PotAndAbsEncoderZeroingEstimator::UpdateEstimate(
+void PotAndAbsoluteEncoderZeroingEstimator::UpdateEstimate(
const PotAndAbsolutePosition &info) {
// Check for Abs Encoder NaN value that would mess up the rest of the zeroing
// code below. NaN values are given when the Absolute Encoder is disconnected.
@@ -325,12 +319,15 @@
} else {
// Have enough samples to start determining if the robot is moving or not.
buffered_samples_[buffered_samples_idx_] = info;
- auto max_value =
- ::std::max_element(buffered_samples_.begin(), buffered_samples_.end(),
- compare_encoder)->encoder;
- auto min_value =
- ::std::min_element(buffered_samples_.begin(), buffered_samples_.end(),
- compare_encoder)->encoder;
+ const auto minmax_value = ::std::minmax_element(
+ buffered_samples_.begin(), buffered_samples_.end(),
+ [](const PotAndAbsolutePosition &left,
+ const PotAndAbsolutePosition &right) {
+ return left.encoder < right.encoder;
+ });
+ const double min_value = minmax_value.first->encoder;
+ const double max_value = minmax_value.second->encoder;
+
if (::std::abs(max_value - min_value) < constants_.zeroing_threshold) {
// Robot isn't moving, use middle sample to determine offsets.
moving = false;
@@ -344,47 +341,39 @@
const int middle_index =
(buffered_samples_idx_ + (constants_.moving_buffer_size - 1) / 2) %
constants_.moving_buffer_size;
-
- // Compute the sum of all the offset samples.
- double relative_to_absolute_offset_sum = 0.0;
- for (size_t i = 0; i < relative_to_absolute_offset_samples_.size(); ++i) {
- relative_to_absolute_offset_sum +=
- relative_to_absolute_offset_samples_[i];
- }
+ const PotAndAbsolutePosition &sample = buffered_samples_[middle_index];
// Compute the average offset between the absolute encoder and relative
// encoder. If we have 0 samples, assume it is 0.
double average_relative_to_absolute_offset =
relative_to_absolute_offset_samples_.size() == 0
? 0.0
- : relative_to_absolute_offset_sum /
+ : ::std::accumulate(relative_to_absolute_offset_samples_.begin(),
+ relative_to_absolute_offset_samples_.end(),
+ 0.0) /
relative_to_absolute_offset_samples_.size();
const double adjusted_incremental_encoder =
- buffered_samples_[middle_index].encoder +
- average_relative_to_absolute_offset;
+ sample.encoder + average_relative_to_absolute_offset;
// Now, compute the nearest absolute encoder value to the offset relative
// encoder position.
const double adjusted_absolute_encoder =
Wrap(adjusted_incremental_encoder,
- buffered_samples_[middle_index].absolute_encoder -
- constants_.measured_absolute_position,
+ sample.absolute_encoder - constants_.measured_absolute_position,
constants_.one_revolution_distance);
// Reverse the math on the previous line to compute the absolute encoder.
// Do this by taking the adjusted encoder, and then subtracting off the
// second argument above, and the value that was added by Wrap.
filtered_absolute_encoder_ =
- ((buffered_samples_[middle_index].encoder +
- average_relative_to_absolute_offset) -
+ ((sample.encoder + average_relative_to_absolute_offset) -
(-constants_.measured_absolute_position +
(adjusted_absolute_encoder -
- (buffered_samples_[middle_index].absolute_encoder -
- constants_.measured_absolute_position))));
+ (sample.absolute_encoder - constants_.measured_absolute_position))));
const double relative_to_absolute_offset =
- adjusted_absolute_encoder - buffered_samples_[middle_index].encoder;
+ adjusted_absolute_encoder - sample.encoder;
// Add the sample and update the average with the new reading.
const size_t relative_to_absolute_offset_samples_size =
@@ -412,29 +401,22 @@
// Now compute the offset between the pot and relative encoder.
if (offset_samples_.size() < constants_.average_filter_size) {
- offset_samples_.push_back(buffered_samples_[middle_index].pot -
- buffered_samples_[middle_index].encoder);
+ offset_samples_.push_back(sample.pot - sample.encoder);
} else {
- offset_samples_[samples_idx_] = buffered_samples_[middle_index].pot -
- buffered_samples_[middle_index].encoder;
+ offset_samples_[samples_idx_] = sample.pot - sample.encoder;
}
// Drop the oldest sample when we run this function the next time around.
samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
- double pot_relative_encoder_offset_sum = 0.0;
- for (size_t i = 0; i < offset_samples_.size(); ++i) {
- pot_relative_encoder_offset_sum += offset_samples_[i];
- }
pot_relative_encoder_offset_ =
- pot_relative_encoder_offset_sum / offset_samples_.size();
+ ::std::accumulate(offset_samples_.begin(), offset_samples_.end(), 0.0) /
+ offset_samples_.size();
- offset_ = Wrap(buffered_samples_[middle_index].encoder +
- pot_relative_encoder_offset_,
- average_relative_to_absolute_offset +
- buffered_samples_[middle_index].encoder,
+ offset_ = Wrap(sample.encoder + pot_relative_encoder_offset_,
+ average_relative_to_absolute_offset + sample.encoder,
constants_.one_revolution_distance) -
- buffered_samples_[middle_index].encoder;
+ sample.encoder;
if (offset_ready()) {
if (!zeroed_) {
first_offset_ = offset_;
@@ -460,8 +442,8 @@
position_ = offset_ + info.encoder;
}
-PotAndAbsEncoderZeroingEstimator::State
-PotAndAbsEncoderZeroingEstimator::GetEstimatorState() const {
+PotAndAbsoluteEncoderZeroingEstimator::State
+PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState() const {
State r;
r.error = error_;
r.zeroed = zeroed_;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 07cc43a..4d4f13d 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -215,12 +215,12 @@
// Estimates the position with an absolute encoder which also reports
// incremental counts, and a potentiometer.
-class PotAndAbsEncoderZeroingEstimator
+class PotAndAbsoluteEncoderZeroingEstimator
: public ZeroingEstimator<PotAndAbsolutePosition,
- constants::PotAndAbsoluteEncoderZeroingConstants,
- AbsoluteEstimatorState> {
+ constants::PotAndAbsoluteEncoderZeroingConstants,
+ PotAndAbsoluteEncoderEstimatorState> {
public:
- explicit PotAndAbsEncoderZeroingEstimator(
+ explicit PotAndAbsoluteEncoderZeroingEstimator(
const constants::PotAndAbsoluteEncoderZeroingConstants &constants);
// Resets the internal logic so it needs to be re-zeroed.
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 7948bf4..2a35764 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -39,7 +39,7 @@
}
void MoveTo(PositionSensorSimulator *simulator,
- PotAndAbsEncoderZeroingEstimator *estimator,
+ PotAndAbsoluteEncoderZeroingEstimator *estimator,
double new_position) {
PotAndAbsolutePosition sensor_values_;
simulator->MoveTo(new_position);
@@ -338,7 +338,7 @@
sim.Initialize(start_pos, index_diff / 3.0, 0.0,
constants.measured_absolute_position);
- PotAndAbsEncoderZeroingEstimator estimator(constants);
+ PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
MoveTo(&sim, &estimator, start_pos);
@@ -366,7 +366,7 @@
sim.Initialize(start_pos, index_diff / 3.0, 0.0,
constants.measured_absolute_position);
- PotAndAbsEncoderZeroingEstimator estimator(constants);
+ PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
PotAndAbsolutePosition sensor_values;
@@ -402,7 +402,7 @@
sim.Initialize(start_pos, index_diff / 3.0, 0.0,
constants.measured_absolute_position);
- PotAndAbsEncoderZeroingEstimator estimator(constants);
+ PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
MoveTo(&sim, &estimator, start_pos + i * index_diff);
@@ -419,7 +419,7 @@
PotAndAbsoluteEncoderZeroingConstants constants{
kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
- PotAndAbsEncoderZeroingEstimator estimator(constants);
+ PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
PotAndAbsolutePosition sensor_values;
sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index 941b4ce..40f49fc 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -56,7 +56,8 @@
int disable_count_ = 0;
::frc971::control_loops::SingleDOFProfiledSubsystem<
- ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator> profiled_subsystem_;
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ profiled_subsystem_;
};
} // namespace intake
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index a9b3590..0a4666a 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -91,8 +91,10 @@
::aos::monotonic_clock::time_point claw_close_start_time_ =
::aos::monotonic_clock::min_time;
- ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator proximal_zeroing_estimator_;
- ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator distal_zeroing_estimator_;
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
+ proximal_zeroing_estimator_;
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
+ distal_zeroing_estimator_;
double proximal_offset_ = 0.0;
double distal_offset_ = 0.0;
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index dff0adc..552086a 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -104,7 +104,7 @@
State state_ = State::UNINITIALIZED;
- ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator zeroing_estimator_;
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator zeroing_estimator_;
double intake_last_position_ = 0.0;
};
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index ee38539..19fbc05 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -35,7 +35,7 @@
float delayed_voltage;
// State of the estimator.
- .frc971.AbsoluteEstimatorState estimator_state;
+ .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
};
struct IntakeGoal {
@@ -62,8 +62,8 @@
struct ArmStatus {
// State of the estimators.
- .frc971.AbsoluteEstimatorState proximal_estimator_state;
- .frc971.AbsoluteEstimatorState distal_estimator_state;
+ .frc971.PotAndAbsoluteEncoderEstimatorState proximal_estimator_state;
+ .frc971.PotAndAbsoluteEncoderEstimatorState distal_estimator_state;
// The node we are currently going to.
uint32_t current_node;
diff --git a/y2018/vision/exposure_loop.sh b/y2018/vision/exposure_loop.sh
new file mode 100755
index 0000000..5fd2da5
--- /dev/null
+++ b/y2018/vision/exposure_loop.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+while true
+do
+ v4l2-ctl --set-ctrl="exposure_absolute=300" -d /dev/video0
+ v4l2-ctl --set-ctrl="exposure_absolute=300" -d /dev/video1
+ sleep 10
+done
diff --git a/y2019/BUILD b/y2019/BUILD
index 49ae5a5..52c0264 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,5 +1,23 @@
+cc_library(
+ name = "constants",
+ srcs = [
+ "constants.cc",
+ ],
+ hdrs = [
+ "constants.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:once",
+ "//aos/logging",
+ "//aos/mutex",
+ "//aos/network:team_number",
+ "//frc971:constants",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
visibility = ["//visibility:public"],
-)
+)
\ No newline at end of file
diff --git a/y2019/constants.cc b/y2019/constants.cc
new file mode 100644
index 0000000..0851577
--- /dev/null
+++ b/y2019/constants.cc
@@ -0,0 +1,83 @@
+#include "y2019/constants.h"
+
+#include <inttypes.h>
+
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "aos/logging/logging.h"
+#include "aos/mutex/mutex.h"
+#include "aos/network/team_number.h"
+#include "aos/once.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace y2019 {
+namespace constants {
+
+const int Values::kZeroingSampleSize;
+
+namespace {
+
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 9971;
+
+const Values *DoGetValuesForTeam(uint16_t team) {
+ Values *const r = new Values();
+
+ switch (team) {
+ // A set of constants for tests.
+ case 1:
+ break;
+
+ case kCompTeamNumber:
+ break;
+
+ case kPracticeTeamNumber:
+ break;
+
+ default:
+ LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
+ }
+
+ return r;
+}
+
+const Values *DoGetValues() {
+ uint16_t team = ::aos::network::GetTeamNumber();
+ LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ return DoGetValuesForTeam(team);
+}
+
+} // namespace
+
+const Values &GetValues() {
+ static ::aos::Once<const Values> once(DoGetValues);
+ return *once.Get();
+}
+
+const Values &GetValuesForTeam(uint16_t team_number) {
+ static ::aos::Mutex mutex;
+ ::aos::MutexLocker locker(&mutex);
+
+ // IMPORTANT: This declaration has to stay after the mutex is locked to avoid
+ // race conditions.
+ static ::std::map<uint16_t, const Values *> values;
+
+ if (values.count(team_number) == 0) {
+ values[team_number] = DoGetValuesForTeam(team_number);
+#if __has_feature(address_sanitizer)
+ __lsan_ignore_object(values[team_number]);
+#endif
+ }
+ return *values[team_number];
+}
+
+
+} // namespace constants
+} // namespace y2019
diff --git a/y2019/constants.h b/y2019/constants.h
new file mode 100644
index 0000000..e4dda86
--- /dev/null
+++ b/y2019/constants.h
@@ -0,0 +1,37 @@
+#ifndef Y2019_CONSTANTS_H_
+#define Y2019_CONSTANTS_H_
+
+#include <stdint.h>
+#include <math.h>
+
+#include "frc971/constants.h"
+
+namespace y2019 {
+namespace constants {
+
+// Has all of our "constants", except the ones that come from other places. The
+// ones which change between robots are put together with a workable way to
+// retrieve the values for the current robot.
+
+// Everything is in SI units (volts, radians, meters, seconds, etc).
+// Some of these values are related to the conversion between raw values
+// (encoder counts, voltage, etc) to scaled units (radians, meters, etc).
+//
+// All ratios are from the encoder shaft to the output units.
+
+struct Values {
+ static const int kZeroingSampleSize = 200;
+};
+
+// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
+// returns a reference to it.
+const Values &GetValues();
+
+// Creates Values instances for each team number it is called with and returns
+// them.
+const Values &GetValuesForTeam(uint16_t team_number);
+
+} // namespace constants
+} // namespace y2019
+
+#endif // Y2019_CONSTANTS_H_
diff --git a/y2019/control_loops/python/BUILD b/y2019/control_loops/python/BUILD
index dbf0dc7..f3e4ca1 100644
--- a/y2019/control_loops/python/BUILD
+++ b/y2019/control_loops/python/BUILD
@@ -87,3 +87,35 @@
"//frc971/control_loops/python:controls",
],
)
+
+py_binary(
+ name = "intake",
+ srcs = [
+ "intake.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ ],
+)
+
+py_binary(
+ name = "lift",
+ srcs = [
+ "lift.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:linear_system",
+ "//frc971/control_loops/python:controls",
+ ],
+)
diff --git a/y2019/control_loops/python/intake.py b/y2019/control_loops/python/intake.py
new file mode 100755
index 0000000..97f8880
--- /dev/null
+++ b/y2019/control_loops/python/intake.py
@@ -0,0 +1,54 @@
+#!/usr/bin/python
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kIntake = angular_system.AngularSystemParams(
+ name='Intake',
+ motor=control_loop.BAG(),
+ G=(1.0 / 9.0) * (1.0 / 9.0) * (16.0 / 38.0),
+ # TODO(austin): Pull moments of inertia from CAD when it's done.
+ J=0.8,
+ q_pos=0.20,
+ q_vel=5.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotMotion(kIntake, R)
+ angular_system.PlotKick(kIntake, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the intake and integral intake.'
+ )
+ else:
+ namespaces = ['y2019', 'control_loops', 'superstructure', 'intake']
+ angular_system.WriteAngularSystem(kIntake, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2019/control_loops/python/lift.py b/y2019/control_loops/python/lift.py
new file mode 100755
index 0000000..dab2754
--- /dev/null
+++ b/y2019/control_loops/python/lift.py
@@ -0,0 +1,54 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import linear_system
+import numpy
+import sys
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kLift = linear_system.LinearSystemParams(
+ name='Lift',
+ motor=control_loop.Vex775Pro(),
+ G=(12.0 / 100.0) * (14.0 / 52.0),
+ # 5mm pitch, 18 tooth
+ radius=0.005 * 18.0 / (2.0 * numpy.pi),
+ # Or, 2.34 lb * 2 (2.1 kg) when lifting back up
+ mass=40.0,
+ q_pos=0.070,
+ q_vel=1.2,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.00,
+ kalman_q_voltage=35.0,
+ kalman_r_position=0.05,
+ dt=0.00505)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[1.5], [0.0]])
+ linear_system.PlotKick(kLift, R)
+ linear_system.PlotMotion(kLift, R, max_velocity=5.0)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the elevator and integral elevator.'
+ )
+ else:
+ namespaces = ['y2019', 'control_loops', 'superstructure', 'lift']
+ linear_system.WriteLinearSystem(kLift, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..79d851a
--- /dev/null
+++ b/y2019/control_loops/superstructure/BUILD
@@ -0,0 +1,40 @@
+package(default_visibility = ["//visibility:public"])
+
+load("//aos/build:queues.bzl", "queue_library")
+
+queue_library(
+ name = "superstructure_queue",
+ srcs = [
+ "superstructure.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:profiled_subsystem_queue",
+ "//frc971/control_loops:queues",
+ ],
+)
+
+cc_library(
+ name = 'superstructure_lib',
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/controls:control_loop",
+ ]
+)
+
+cc_binary(
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ "//aos:init",
+ ]
+)
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..2f0832c
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,32 @@
+#include "y2019/control_loops/superstructure/superstructure.h"
+
+#include "aos/controls/control_loops.q.h"
+#include "frc971/control_loops/control_loops.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+Superstructure::Superstructure(
+ SuperstructureQueue *superstructure_queue)
+ : aos::controls::ControlLoop<SuperstructureQueue>(
+ superstructure_queue) {}
+
+void Superstructure::RunIteration(
+ const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) {
+ (void)unsafe_goal;
+ (void)position;
+ (void)output;
+ (void)status;
+
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..1faa6b4
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -0,0 +1,34 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/controls/control_loop.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<SuperstructureQueue> {
+ public:
+ explicit Superstructure(
+ SuperstructureQueue *my_superstructure =
+ &superstructure_queue);
+
+ protected:
+ virtual void RunIteration(
+ const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) override;
+
+ private:
+
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
new file mode 100644
index 0000000..2cd7238
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -0,0 +1,127 @@
+package y2019.control_loops.superstructure;
+
+import "aos/controls/control_loops.q";
+import "frc971/control_loops/profiled_subsystem.q";
+
+struct ElevatorGoal {
+ // Meters, 0 = lowest position - mechanical hard stop,
+ // positive = upward
+ double height;
+
+ .frc971.ProfileParameters profile_params;
+};
+
+struct IntakeGoal {
+ // Positive is rollers intaking inward.
+ double roller_voltage;
+
+ // 0 = linkage on the sprocket is pointing straight up,
+ // positive = forward
+ double joint_angle;
+
+ .frc971.ProfileParameters profile_params;
+};
+
+struct SuctionGoal {
+ // True = open solenoid (apply suction)
+ // Top/bottom are when wrist is forward
+ bool top;
+ bool bottom;
+};
+
+struct StiltsGoal {
+ // Distance stilts extended out of the bottom of the robot. Positive = down.
+ // 0 is the height such that the bottom of the stilts is tangent to the bottom
+ // of the middle wheels.
+ double height;
+
+ .frc971.ProfileParameters profile_params;
+};
+
+struct WristGoal {
+ // 0 = Straight up parallel to elevator
+ // Positive rotates toward intake from 0
+ double angle;
+ .frc971.ProfileParameters profile_params;
+};
+
+queue_group SuperstructureQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ ElevatorGoal elevator;
+ IntakeGoal intake;
+ SuctionGoal suction;
+ StiltsGoal stilts;
+ WristGoal wrist;
+ };
+
+ message Status {
+ // All subsystems know their location.
+ bool zeroed;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ bool estopped;
+
+ // Whether suction_pressure indicates cargo is held
+ bool has_piece;
+
+ // Status of each subsystem.
+ .frc971.control_loops.AbsoluteProfiledJointStatus elevator;
+ .frc971.control_loops.AbsoluteProfiledJointStatus wrist;
+ .frc971.control_loops.AbsoluteProfiledJointStatus intake;
+ .frc971.control_loops.AbsoluteProfiledJointStatus stilts;
+ };
+
+ message Position {
+ // Input from pressure sensor in psi
+ // 0 = current atmospheric pressure, negative = suction.
+ double suction_pressure;
+
+ // Position of the elevator, 0 at lowest position, positive when up.
+ .frc971.PotAndAbsolutePosition elevator;
+
+ // Position of wrist, 0 when up, positive is rotating toward the front,
+ // over the top.
+ .frc971.PotAndAbsolutePosition wrist;
+
+ // Position of the intake. 0 when rollers are retracted, positive extended.
+ .frc971.PotAndAbsolutePosition intake_joint;
+
+ // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
+ .frc971.PotAndAbsolutePosition stilts;
+ };
+
+ message Output {
+ // Voltage sent to motors moving elevator up/down. Positive is up.
+ double elevator_voltage;
+
+ // Voltage sent to wrist motors on elevator to rotate.
+ // Positive rotates over the top towards the front of the robot.
+ double wrist_voltage;
+
+ // Voltage sent to motors on intake joint. Positive extends rollers.
+ double intake_joint_voltage;
+
+ // Voltage sent to rollers on intake. Positive rolls inward.
+ double intake_roller_voltage;
+
+ // Voltage sent to motors to move stilts height. Positive moves robot upward.
+ double stilts_voltage;
+
+ // True opens solenoid (applies suction)
+ // Top/bottom are when wrist is toward the front of the robot
+ bool intake_suction_top;
+ bool intake_suction_bottom;
+
+ // Voltage sent to the vacuum pump motors.
+ double pump_voltage;
+ };
+
+ queue Goal goal;
+ queue Output output;
+ queue Status status;
+ queue Position position;
+};
+
+queue_group SuperstructureQueue superstructure_queue;
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..f3dd1ad
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,11 @@
+#include "y2019/control_loops/superstructure/superstructure.h"
+
+#include "aos/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2019::control_loops::superstructure::Superstructure superstructure;
+ superstructure.Run();
+ ::aos::Cleanup();
+ return 0;
+}
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_