Merge "Add a consistent overhead byte stuffing implementation"
diff --git a/.bazelrc b/.bazelrc
index c80339f..57f01ea 100644
--- a/.bazelrc
+++ b/.bazelrc
@@ -50,6 +50,10 @@
build:msan --test_env MSAN_SYMBOLIZER_PATH=/usr/bin/llvm-symbolizer-3.6
build:msan --define have_msan=true
+# Sometime, we want to be able to have eigen assertions run so that we can
+# catch potential issues (e.g., accessing invalid indices).
+build:eigen --copt -UNDEBUG
+
# Show paths to a few more than just 1 target.
build --show_result 5
# Dump the output of the failing test to stdout.
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
index a187981..0590d7b 100644
--- a/aos/events/event-loop.h
+++ b/aos/events/event-loop.h
@@ -59,11 +59,13 @@
const T &operator*() const { return *get(); }
const T *operator->() const { return get(); }
- // Sends the message to the queue. Should only be called once.
- void Send() {
+ // Sends the message to the queue. Should only be called once. Returns true
+ // if the message was successfully sent, and false otherwise.
+ bool Send() {
RawSender *sender = &msg_.get_deleter();
msg_->SetTimeToNow();
- sender->Send(reinterpret_cast<RawSender::SendContext *>(msg_.release()));
+ return sender->Send(
+ reinterpret_cast<RawSender::SendContext *>(msg_.release()));
}
private:
@@ -91,6 +93,10 @@
// Current time.
virtual monotonic_clock::time_point monotonic_now() = 0;
+ // Note, it is supported to create:
+ // multiple fetchers, and (one sender or one watcher) per <path, type>
+ // tuple.
+
// Makes a class that will always fetch the most recent value
// sent to path.
template <typename T>
diff --git a/aos/events/event-loop_param_test.cc b/aos/events/event-loop_param_test.cc
index 78e744a..efac8f9 100644
--- a/aos/events/event-loop_param_test.cc
+++ b/aos/events/event-loop_param_test.cc
@@ -55,44 +55,54 @@
EXPECT_TRUE(happened);
}
-// Verify that making a fetcher and handler for "/test" dies.
-TEST_P(AbstractEventLoopTest, FetcherAndHandler) {
+// Verify that making a fetcher and watcher for "/test" succeeds.
+TEST_P(AbstractEventLoopTest, FetcherAndWatcher) {
auto loop = Make();
auto fetcher = loop->MakeFetcher<TestMessage>("/test");
- EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}), "/test");
+ loop->MakeWatcher("/test", [&](const TestMessage &) {});
}
-// Verify that making 2 fetchers for "/test" fails.
+// Verify that making 2 fetchers for "/test" succeeds.
TEST_P(AbstractEventLoopTest, TwoFetcher) {
auto loop = Make();
auto fetcher = loop->MakeFetcher<TestMessage>("/test");
- EXPECT_DEATH(loop->MakeFetcher<TestMessage>("/test"), "/test");
+ auto fetcher2 = loop->MakeFetcher<TestMessage>("/test");
}
-// Verify that registering a handler twice for "/test" fails.
-TEST_P(AbstractEventLoopTest, TwoHandler) {
+// Verify that registering a watcher twice for "/test" fails.
+TEST_P(AbstractEventLoopTest, TwoWatcher) {
auto loop = Make();
loop->MakeWatcher("/test", [&](const TestMessage &) {});
- EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}), "/test");
+ EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}),
+ "/test");
+}
+
+// Verify that registering a watcher and a sender for "/test" fails.
+TEST_P(AbstractEventLoopTest, WatcherAndSender) {
+ auto loop = Make();
+ auto sender = loop->MakeSender<TestMessage>("/test");
+ EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}),
+ "/test");
}
// Verify that Quit() works when there are multiple watchers.
-TEST_P(AbstractEventLoopTest, MultipleFetcherQuit) {
- auto loop = Make();
+TEST_P(AbstractEventLoopTest, MultipleWatcherQuit) {
+ auto loop1 = Make();
+ auto loop2 = Make();
- auto sender = loop->MakeSender<TestMessage>("/test2");
+ auto sender = loop1->MakeSender<TestMessage>("/test2");
{
auto msg = sender.MakeMessage();
msg->msg_value = 200;
msg.Send();
}
- loop->MakeWatcher("/test1", [&](const TestMessage &) {});
- loop->MakeWatcher("/test2", [&](const TestMessage &message) {
+ loop2->MakeWatcher("/test1", [&](const TestMessage &) {});
+ loop2->MakeWatcher("/test2", [&](const TestMessage &message) {
EXPECT_EQ(message.msg_value, 200);
- loop->Exit();
+ loop2->Exit();
});
- loop->Run();
+ loop2->Run();
}
// Verify that timer intervals and duration function properly.
diff --git a/aos/events/shm-event-loop.cc b/aos/events/shm-event-loop.cc
index 945ebc0..781c963 100644
--- a/aos/events/shm-event-loop.cc
+++ b/aos/events/shm-event-loop.cc
@@ -1,12 +1,14 @@
#include "aos/events/shm-event-loop.h"
-#include "aos/logging/logging.h"
-#include "aos/queue.h"
#include <sys/timerfd.h>
+#include <algorithm>
#include <atomic>
#include <chrono>
#include <stdexcept>
+#include "aos/logging/logging.h"
+#include "aos/queue.h"
+
namespace aos {
ShmEventLoop::ShmEventLoop() : thread_state_(std::make_shared<ThreadState>()) {}
@@ -173,13 +175,13 @@
std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
const std::string &path, const QueueTypeInfo &type) {
- Take(path);
return std::unique_ptr<RawFetcher>(new ShmFetcher(
RawQueue::Fetch(path.c_str(), type.size, type.hash, type.queue_length)));
}
std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
const std::string &path, const QueueTypeInfo &type) {
+ Take(path);
return std::unique_ptr<RawSender>(new ShmSender(
RawQueue::Fetch(path.c_str(), type.size, type.hash, type.queue_length)));
}
@@ -269,8 +271,12 @@
if (is_running()) {
::aos::Die("Cannot add new objects while running.\n");
}
- if (!taken_.emplace(path).second) {
- ::aos::Die("%s already has a listener / watcher.", path.c_str());
+
+ const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
+ if (prior != taken_.end()) {
+ ::aos::Die("%s is already being used.", path.c_str());
+ } else {
+ taken_.emplace_back(path);
}
}
diff --git a/aos/events/shm-event-loop.h b/aos/events/shm-event-loop.h
index 49d65f7..e8ff267 100644
--- a/aos/events/shm-event-loop.h
+++ b/aos/events/shm-event-loop.h
@@ -17,6 +17,10 @@
// Specialization of EventLoop that is built from queues running out of shared
// memory. See more details at aos/queue.h
+//
+// This object must be interacted with from one thread, but the Senders and
+// Fetchers may be used from multiple threads afterwords (as long as their
+// destructors are called back in one thread again)
class ShmEventLoop : public EventLoop {
public:
ShmEventLoop();
@@ -73,12 +77,13 @@
};
// Exclude multiple of the same type for path.
- void Take(const std::string &path);
std::vector<std::function<void()>> on_run_;
std::shared_ptr<ThreadState> thread_state_;
- std::unordered_set<std::string> taken_;
+ void Take(const std::string &path);
+
+ std::vector<::std::string> taken_;
};
} // namespace aos
diff --git a/aos/events/simulated-event-loop.cc b/aos/events/simulated-event-loop.cc
index e82c381..a23b18c 100644
--- a/aos/events/simulated-event-loop.cc
+++ b/aos/events/simulated-event-loop.cc
@@ -1,4 +1,7 @@
#include "aos/events/simulated-event-loop.h"
+
+#include <algorithm>
+
#include "aos/queue.h"
namespace aos {
@@ -80,13 +83,13 @@
std::unique_ptr<RawSender> SimulatedEventLoop::MakeRawSender(
const std::string &path, const QueueTypeInfo &type) {
+ Take(path);
::std::pair<::std::string, QueueTypeInfo> key(path, type);
return GetSimulatedQueue(key)->MakeRawSender();
}
std::unique_ptr<RawFetcher> SimulatedEventLoop::MakeRawFetcher(
const std::string &path, const QueueTypeInfo &type) {
- Take(path);
::std::pair<::std::string, QueueTypeInfo> key(path, type);
return GetSimulatedQueue(key)->MakeRawFetcher();
}
@@ -116,12 +119,15 @@
return std::unique_ptr<RawFetcher>(new SimulatedFetcher(this));
}
-void SimulatedEventLoop::Take(const std::string &path) {
+void SimulatedEventLoop::Take(const ::std::string &path) {
if (is_running()) {
::aos::Die("Cannot add new objects while running.\n");
}
- if (!taken_.emplace(path).second) {
- ::aos::Die("%s already has a listener / watcher.", path.c_str());
+ const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
+ if (prior != taken_.end()) {
+ ::aos::Die("%s is already being used.", path.c_str());
+ } else {
+ taken_.emplace_back(path);
}
}
} // namespace aos
diff --git a/aos/events/simulated-event-loop.h b/aos/events/simulated-event-loop.h
index 79b48b8..b5c94bf 100644
--- a/aos/events/simulated-event-loop.h
+++ b/aos/events/simulated-event-loop.h
@@ -212,8 +212,7 @@
std::function<void(const aos::Message *message)> watcher) override;
TimerHandler *AddTimer(::std::function<void()> callback) override {
- timers_.emplace_back(
- new SimulatedTimerHandler(scheduler_, callback));
+ timers_.emplace_back(new SimulatedTimerHandler(scheduler_, callback));
return timers_.back().get();
}
@@ -232,19 +231,20 @@
SimulatedQueue *GetSimulatedQueue(
const ::std::pair<::std::string, QueueTypeInfo> &);
- void Take(const std::string &path);
+ void Take(const ::std::string &path);
private:
EventScheduler *scheduler_;
- std::map<::std::pair<::std::string, QueueTypeInfo>, SimulatedQueue> *queues_;
- std::unordered_set<std::string> taken_;
- std::vector<std::unique_ptr<TimerHandler>> timers_;
+ ::std::map<::std::pair<::std::string, QueueTypeInfo>, SimulatedQueue>
+ *queues_;
+ ::std::vector<std::string> taken_;
+ ::std::vector<std::unique_ptr<TimerHandler>> timers_;
};
class SimulatedEventLoopFactory {
public:
- std::unique_ptr<EventLoop> CreateEventLoop() {
- return std::unique_ptr<EventLoop>(
+ ::std::unique_ptr<EventLoop> CreateEventLoop() {
+ return ::std::unique_ptr<EventLoop>(
new SimulatedEventLoop(&scheduler_, &queues_));
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a06bc3b..a50b429 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -207,7 +207,7 @@
}
LOG(DEBUG,
- "New IMU value from ADIS16448, rate is %f, angle %f, fused %f, bias "
+ "New IMU value, rate is %f, angle %f, fused %f, bias "
"%f\n",
rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
down_U_(0, 0) = rate;
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 784b92f..1b6b965 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -78,7 +78,7 @@
.frc971.HallEffectAndPositionEstimatorState estimator_state;
};
-struct AbsoluteProfiledJointStatus {
+struct PotAndAbsoluteEncoderProfiledJointStatus {
// Is the subsystem zeroed?
bool zeroed;
@@ -154,6 +154,44 @@
.frc971.IndexEstimatorState estimator_state;
};
+struct AbsoluteEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ .frc971.AbsoluteEncoderEstimatorState estimator_state;
+};
+
struct StaticZeroingSingleDOFProfiledSubsystemGoal {
double unsafe_goal;
.frc971.ProfileParameters profile_params;
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 322e41a..e1f55cc 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -379,10 +379,12 @@
linear_system.name, [linear_system], namespaces=year_namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f', linear_system.motor.
- free_speed / (2.0 * numpy.pi)))
+ free_speed))
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', linear_system.G *
linear_system.radius))
+ loop_writer.AddConstant(
+ control_loop.Constant('kRadius', '%f', linear_system.radius))
loop_writer.Write(plant_files[0], plant_files[1])
integral_linear_system = IntegralLinearSystem(params,
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 52bd8d1..9748a41 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -17,16 +17,16 @@
from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
-LENGTH_OF_FIELD = 323.65
+WIDTH_OF_FIELD_IN_METERS = 8.258302
PIXELS_ON_SCREEN = 300
-def pxToIn(p):
- return p * LENGTH_OF_FIELD / PIXELS_ON_SCREEN
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-def inToPx(i):
- return (i * PIXELS_ON_SCREEN / LENGTH_OF_FIELD)
+def mToPx(i):
+ return (i * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
def px(cr):
@@ -155,7 +155,7 @@
#John also wrote this
def add_point(self, x, y):
if (len(self.selected_points) < 6):
- self.selected_points.append([x, y])
+ self.selected_points.append([pxToM(x), pxToM(y)])
if (len(self.selected_points) == 6):
self.mode = Mode.kEditing
self.splines.append(np.array(self.selected_points))
@@ -188,9 +188,10 @@
cur_p = [self.x, self.y]
for index_splines, points in enumerate(self.spline):
- for index_points, i in enumerate(points.curve):
+ for index_points, point in enumerate(points.curve):
# pythagorean
- distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
+ distance = np.sqrt((cur_p[0] - mToPx(point[0]))**2 +
+ (cur_p[1] - mToPx(point[1])**2))
if distance < nearest:
nearest = distance
print("DISTANCE: ", distance, " | INDEX: ", index_points)
@@ -222,7 +223,7 @@
if index > 0 and index <= self.index_of_edit:
distance += np.sqrt((points[index - 1][0] - point[0])**2 +
(points[index - 1][1] - point[1])**2)
- return pxToIn(distance)
+ return distance
# Handle the expose-event by updating the Window and drawing
def handle_draw(self, cr):
@@ -255,23 +256,6 @@
cr.rectangle(-450, -150, 300, 300)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
- cr.rectangle((inToPx(140 - 161.825) - 300), inToPx(76.575), inToPx(56),
- inToPx(-153.15))
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
- cr.stroke()
- cr.rectangle((inToPx(161.825 - 24) - 300), inToPx(90), inToPx(24),
- inToPx(-180))
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
- cr.stroke()
-
- set_color(cr, Color(0.2, 0.2, 0.2))
- cr.rectangle(
- inToPx(140 - 161.825) - 300, inToPx(76.575), inToPx(56),
- inToPx(-153.15))
- cr.fill()
- cr.rectangle(
- inToPx(161.825 - 24) - 300, inToPx(90), inToPx(24), inToPx(-180))
- cr.fill()
y = 0
@@ -307,8 +291,8 @@
print(str(item))
for i, point in enumerate(self.selected_points):
print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1] - 15)
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
+ cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
elif self.mode == Mode.kEditing:
@@ -316,17 +300,14 @@
cr.move_to(-300, 170)
display_text(cr, "EDITING", 1, 1, 1, 1)
if len(self.splines) > 0:
- # print("Splines: " + str(len(self.splines)))
- # print("ITEMS:")
holder_spline = []
for i, points in enumerate(self.splines):
array = np.zeros(shape=(6, 2), dtype=float)
for j, point in enumerate(points):
- array[j, 0] = point[0]
- array[j, 1] = point[1]
+ array[j, 0] = mToPx(point[0])
+ array[j, 1] = mToPx(point[1])
spline = Spline(np.ascontiguousarray(np.transpose(array)))
for k in np.linspace(0.01, 1, 100):
-
cr.move_to(
spline.Point(k - 0.01)[0],
spline.Point(k - 0.01)[1])
@@ -336,7 +317,6 @@
spline.Point(k - 0.01)[0],
spline.Point(k - 0.01)[1]
]
-
holder_spline.append(holding)
self.curves.append(holder_spline)
@@ -346,13 +326,13 @@
for i, point in enumerate(points):
# print("I: " + str(i))
if spline == self.spline_edit and i == self.index_of_edit:
- draw_px_x(cr, point[0], point[1], 15,
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 15,
self.colors[spline])
elif (spline == 0 and not i == 5) or (not i == 0
and not i == 5):
- draw_px_x(cr, point[0], point[1], 10,
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10,
self.colors[spline])
- cr.move_to(point[0], point[1] - 15)
+ cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
elif self.mode == Mode.kExporting:
@@ -466,7 +446,7 @@
if self.index_of_edit > -1 and self.held_x != self.x:
print("INDEX OF EDIT: " + str(self.index_of_edit))
self.splines[self.spline_edit][self.index_of_edit] = [
- self.x, self.y
+ pxToM(self.x), pxToM(self.y)
]
if not self.spline_edit == len(self.splines) - 1:
@@ -494,11 +474,11 @@
# Get clicked point
# Find nearest
# Move nearest to clicked
- cur_p = [self.x, self.y]
+ cur_p = [pxToM(self.x), pxToM(self.y)]
print("CUR_P: " + str(self.x) + " " + str(self.y))
# Get the distance between each for x and y
# Save the index of the point closest
- nearest = 50
+ nearest = 1 # Max distance away a the selected point can be in meters
index_of_closest = 0
for index_splines, points in enumerate(self.splines):
for index_points, val in enumerate(points):
@@ -520,7 +500,7 @@
def do_button_press(self, event):
print("button press activated")
- #Be consistent with the scaling in the drawing_area
+ # Be consistent with the scaling in the drawing_area
self.x = event.x * 2
self.y = event.y * 2
self.button_press_action()
@@ -622,4 +602,4 @@
window = GridWindow()
-basic_window.RunApp()
\ No newline at end of file
+basic_window.RunApp()
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 9ad7439..7b7b370 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -22,10 +22,10 @@
::frc971::ProfileParameters default_profile_params;
// Maximum range of the subsystem in meters
- const ::frc971::constants::Range range;
+ ::frc971::constants::Range range;
// Zeroing constants for PotAndABsoluteEncoder estimator
- const typename ZeroingEstimator::ZeroingConstants zeroing_constants;
+ typename ZeroingEstimator::ZeroingConstants zeroing_constants;
// Function that makes the integral loop for the subsystem
::std::function<StateFeedbackLoop<3, 1, 1>()> make_integral_loop;
@@ -33,13 +33,16 @@
// Class for controlling and motion profiling a single degree of freedom
// subsystem with a zeroing strategy of not moving.
-template <typename ZeroingEstimator>
+template <typename TZeroingEstimator, typename TProfiledJointStatus>
class StaticZeroingSingleDOFProfiledSubsystem {
public:
StaticZeroingSingleDOFProfiledSubsystem(
- const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+ const StaticZeroingSingleDOFProfiledSubsystemParams<TZeroingEstimator>
¶ms);
+ using ZeroingEstimator = TZeroingEstimator;
+ using ProfiledJointStatus = TProfiledJointStatus;
+
// Returns the filtered goal of the profiled subsystem (R)
double goal(int row) const { return profiled_subsystem_.goal(row, 0); }
@@ -64,8 +67,7 @@
void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position,
- double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+ double *output, ProfiledJointStatus *status);
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -90,8 +92,8 @@
profiled_subsystem_;
};
-template <typename ZeroingEstimator>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::
StaticZeroingSingleDOFProfiledSubsystem(
const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
¶ms)
@@ -100,26 +102,26 @@
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- 3, 1, 1>(params.make_integral_loop())),
+ 3, 1, 1>(params_.make_integral_loop())),
params.zeroing_constants, params.range,
params.default_profile_params.max_velocity,
params.default_profile_params.max_acceleration) {
Reset();
};
-template <typename ZeroingEstimator>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Reset() {
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Reset() {
state_ = State::UNINITIALIZED;
clear_min_position();
clear_max_position();
profiled_subsystem_.Reset();
}
-template <typename ZeroingEstimator>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Iterate(
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ ProfiledJointStatus *status) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -213,4 +215,4 @@
} // namespace control_loops
} // namespace frc971
-#endif // FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
\ No newline at end of file
+#endif // FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 8805842..9a9c092 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -17,85 +17,102 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+using SZSDPS_PotAndAbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+using SZSDPS_AbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+typedef ::testing::Types<SZSDPS_AbsEncoder, SZSDPS_PotAndAbsEncoder> TestTypes;
+
+constexpr ::frc971::constants::Range kRange{
+ .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
+
+constexpr double kZeroingVoltage = 2.5;
+constexpr double kOperatingVoltage = 12.0;
+const int kZeroingSampleSize = 200;
+
+constexpr double kEncoderIndexDifference = 1.0;
+
+template <typename ZeroingEstimator>
struct TestIntakeSystemValues {
- static const int kZeroingSampleSize = 200;
+ static const typename ZeroingEstimator::ZeroingConstants kZeroing;
- static constexpr double kEncoderRatio =
- (16.0 * 0.25) * (20.0 / 40.0) / (2.0 * M_PI) * 0.0254;
- static constexpr double kEncoderIndexDifference = 2.0 * M_PI * kEncoderRatio;
- static constexpr ::frc971::constants::Range kRange{
- .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
-
- static constexpr double kZeroingVoltage = 2.5;
- static constexpr double kOperatingVoltage = 12.0;
-
- static const ::frc971::ProfileParameters kDefaultParams;
- static const ::frc971::ProfileParameters kZeroingParams;
-
- static constexpr ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
- kZeroing{kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
-
- static const StaticZeroingSingleDOFProfiledSubsystemParams<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- kParams;
+ static const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+ make_params();
};
-constexpr ::frc971::constants::Range TestIntakeSystemValues::kRange;
-constexpr ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
- TestIntakeSystemValues::kZeroing;
-const ::frc971::ProfileParameters TestIntakeSystemValues::kDefaultParams{0.3,
- 5.0};
-const ::frc971::ProfileParameters TestIntakeSystemValues::kZeroingParams{0.1,
- 1.0};
+template <>
+const zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
+ TestIntakeSystemValues<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
+ kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
-const StaticZeroingSingleDOFProfiledSubsystemParams<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- TestIntakeSystemValues::kParams(
- {.zeroing_voltage = TestIntakeSystemValues::kZeroingVoltage,
- .operating_voltage = TestIntakeSystemValues::kOperatingVoltage,
- .zeroing_profile_params = TestIntakeSystemValues::kZeroingParams,
- .default_profile_params = TestIntakeSystemValues::kDefaultParams,
- .range = TestIntakeSystemValues::kRange,
- .zeroing_constants = TestIntakeSystemValues::kZeroing,
- .make_integral_loop = MakeIntegralTestIntakeSystemLoop});
+template <>
+const zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
+ TestIntakeSystemValues<zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
+ kZeroingSampleSize,
+ kEncoderIndexDifference,
+ 0.0,
+ 0.2,
+ 0.0005,
+ 20,
+ 1.9};
+template <typename ZeroingEstimator>
+const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+TestIntakeSystemValues<ZeroingEstimator>::make_params() {
+ StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator> params(
+ {kZeroingVoltage,
+ kOperatingVoltage,
+ {0.1, 1.0},
+ {0.3, 5.0},
+ kRange,
+ TestIntakeSystemValues::kZeroing,
+ &MakeIntegralTestIntakeSystemLoop});
+ return params;
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus>
struct TestIntakeSystemData {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
- ::frc971::control_loops::AbsoluteProfiledJointStatus status;
+ ProfiledJointStatus status;
- ::frc971::PotAndAbsolutePosition position;
+ typename ZeroingEstimator::Position position;
double output;
};
} // namespace
+template <typename SZSDPS>
class TestIntakeSystemSimulation {
public:
TestIntakeSystemSimulation()
: subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
- subsystem_pot_encoder_(
- TestIntakeSystemValues::kEncoderIndexDifference) {
+ subsystem_sensor_sim_(
+ kEncoderIndexDifference) {
// Start the subsystem out in the middle by default.
- InitializeSubsystemPosition((TestIntakeSystemValues::kRange.lower +
- TestIntakeSystemValues::kRange.upper) /
- 2.0);
+ InitializeSubsystemPosition((kRange.lower + kRange.upper) / 2.0);
}
void InitializeSubsystemPosition(double start_pos) {
subsystem_plant_->mutable_X(0, 0) = start_pos;
subsystem_plant_->mutable_X(1, 0) = 0.0;
- subsystem_pot_encoder_.Initialize(
- start_pos, kNoiseScalar, 0.0,
- TestIntakeSystemValues::kZeroing.measured_absolute_position);
+ InitializeSensorSim(start_pos);
}
+ void InitializeSensorSim(double start_pos);
+
// Updates the position message with the position of the subsystem.
- void UpdatePositionMessage(::frc971::PotAndAbsolutePosition *position) {
- subsystem_pot_encoder_.GetSensorValues(position);
+ void UpdatePositionMessage(
+ typename SZSDPS::ZeroingEstimator::Position *position) {
+ subsystem_sensor_sim_.GetSensorValues(position);
}
double subsystem_position() const { return subsystem_plant_->X(0, 0); }
@@ -110,12 +127,9 @@
// Simulates the subsystem for a single timestep.
void Simulate(const double output_voltage, const int32_t state) {
const double voltage_check_subsystem =
- (static_cast<StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State>(state) ==
- StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING)
- ? TestIntakeSystemValues::kOperatingVoltage
- : TestIntakeSystemValues::kZeroingVoltage;
+ (static_cast<typename SZSDPS::State>(state) == SZSDPS::State::RUNNING)
+ ? kOperatingVoltage
+ : kZeroingVoltage;
EXPECT_LE(::std::abs(output_voltage), voltage_check_subsystem);
@@ -125,21 +139,48 @@
const double position_subsystem = subsystem_plant_->Y(0, 0);
- subsystem_pot_encoder_.MoveTo(position_subsystem);
+ subsystem_sensor_sim_.MoveTo(position_subsystem);
- EXPECT_GE(position_subsystem, TestIntakeSystemValues::kRange.lower_hard);
- EXPECT_LE(position_subsystem, TestIntakeSystemValues::kRange.upper_hard);
+ EXPECT_GE(position_subsystem, kRange.lower_hard);
+ EXPECT_LE(position_subsystem, kRange.upper_hard);
}
private:
::std::unique_ptr<CappedTestPlant> subsystem_plant_;
- PositionSensorSimulator subsystem_pot_encoder_;
+ PositionSensorSimulator subsystem_sensor_sim_;
};
+template <>
+void TestIntakeSystemSimulation<SZSDPS_PotAndAbsEncoder>::InitializeSensorSim(
+ double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+template <>
+void TestIntakeSystemSimulation<SZSDPS_AbsEncoder>::InitializeSensorSim(
+ double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+template <typename TSZSDPS>
class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
protected:
+ using SZSDPS = TSZSDPS;
+ using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
+ using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
+
IntakeSystemTest()
- : subsystem_(TestIntakeSystemValues::kParams), subsystem_plant_() {}
+ : subsystem_(TestIntakeSystemValues<
+ typename SZSDPS::ZeroingEstimator>::make_params()),
+ subsystem_plant_() {}
void VerifyNearGoal() {
EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
@@ -212,14 +253,14 @@
// TestIntakeSystemData subsystem_data_;
// Create a control loop and simulation.
- StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- subsystem_;
- TestIntakeSystemSimulation subsystem_plant_;
+ SZSDPS subsystem_;
+ TestIntakeSystemSimulation<SZSDPS> subsystem_plant_;
StaticZeroingSingleDOFProfiledSubsystemGoal subsystem_goal_;
- TestIntakeSystemData subsystem_data_;
+ TestIntakeSystemData<typename SZSDPS::ZeroingEstimator,
+ typename SZSDPS::ProfiledJointStatus>
+ subsystem_data_;
private:
// The acceleration limits to check for while moving.
@@ -230,80 +271,81 @@
int32_t sensor_reader_pid_ = 0;
};
-// Tests that the subsystem does nothing when the goal is zero.
-TEST_F(IntakeSystemTest, DoesNothing) {
- // Intake system uses 0.05 to test for 0.
- subsystem_data_.goal.unsafe_goal = 0.05;
- RunForTime(chrono::seconds(5));
+TYPED_TEST_CASE_P(IntakeSystemTest);
- VerifyNearGoal();
+// Tests that the subsystem does nothing when the goal is zero.
+TYPED_TEST_P(IntakeSystemTest, DoesNothing) {
+ // Intake system uses 0.05 to test for 0.
+ this->subsystem_data_.goal.unsafe_goal = 0.05;
+ this->RunForTime(chrono::seconds(5));
+
+ this->VerifyNearGoal();
}
// Tests that the subsystem loop can reach a goal.
-TEST_F(IntakeSystemTest, ReachesGoal) {
+TYPED_TEST_P(IntakeSystemTest, ReachesGoal) {
// Set a reasonable goal.
- auto &goal = subsystem_data_.goal;
+ auto &goal = this->subsystem_data_.goal;
goal.unsafe_goal = 0.1;
goal.profile_params.max_velocity = 1;
goal.profile_params.max_acceleration = 0.5;
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(8));
+ this->RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
// Makes sure that the voltage on a motor is properly pulled back after
// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
-TEST_F(IntakeSystemTest, SaturationTest) {
+TYPED_TEST_P(IntakeSystemTest, SaturationTest) {
// Zero it before we move.
- auto &goal = subsystem_data_.goal;
- goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
- RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ auto &goal = this->subsystem_data_.goal;
+ goal.unsafe_goal = kRange.upper;
+ this->RunForTime(chrono::seconds(8));
+ this->VerifyNearGoal();
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- goal.unsafe_goal = TestIntakeSystemValues::kRange.lower;
+ goal.unsafe_goal = kRange.lower;
goal.profile_params.max_velocity = 20.0;
goal.profile_params.max_acceleration = 0.1;
}
- set_peak_subsystem_velocity(23.0);
- set_peak_subsystem_acceleration(0.2);
+ this->set_peak_subsystem_velocity(23.0);
+ this->set_peak_subsystem_acceleration(0.2);
- RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ this->RunForTime(chrono::seconds(8));
+ this->VerifyNearGoal();
// Now do a high acceleration move with a low velocity limit.
{
- goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.unsafe_goal = kRange.upper;
goal.profile_params.max_velocity = 0.1;
goal.profile_params.max_acceleration = 100;
}
- set_peak_subsystem_velocity(0.2);
- set_peak_subsystem_acceleration(103);
- RunForTime(chrono::seconds(8));
+ this->set_peak_subsystem_velocity(0.2);
+ this->set_peak_subsystem_acceleration(103);
+ this->RunForTime(chrono::seconds(8));
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
// Tests that the subsystem loop doesn't try and go beyond it's physical range
// of the mechanisms.
-TEST_F(IntakeSystemTest, RespectsRange) {
- auto &goal = subsystem_data_.goal;
+TYPED_TEST_P(IntakeSystemTest, RespectsRange) {
+ auto &goal = this->subsystem_data_.goal;
// Set some ridiculous goals to test upper limits.
{
goal.unsafe_goal = 100.0;
goal.profile_params.max_velocity = 1;
goal.profile_params.max_acceleration = 0.5;
}
- RunForTime(chrono::seconds(10));
+ this->RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(TestIntakeSystemValues::kRange.upper,
- subsystem_data_.status.position, 0.001);
+ EXPECT_NEAR(kRange.upper, this->subsystem_data_.status.position, 0.001);
// Set some ridiculous goals to test lower limits.
{
@@ -312,181 +354,163 @@
goal.profile_params.max_acceleration = 0.5;
}
- RunForTime(chrono::seconds(10));
+ this->RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(TestIntakeSystemValues::kRange.lower,
- subsystem_data_.status.position, 0.001);
+ EXPECT_NEAR(kRange.lower, this->subsystem_data_.status.position, 0.001);
}
// Tests that the subsystem loop zeroes when run for a while.
-TEST_F(IntakeSystemTest, ZeroTest) {
- auto &goal = subsystem_data_.goal;
+TYPED_TEST_P(IntakeSystemTest, ZeroTest) {
+ auto &goal = this->subsystem_data_.goal;
{
- goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.unsafe_goal = kRange.upper;
goal.profile_params.max_velocity = 1;
goal.profile_params.max_acceleration = 0.5;
}
- RunForTime(chrono::seconds(10));
+ this->RunForTime(chrono::seconds(10));
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
-TEST_F(IntakeSystemTest, ZeroNoGoal) {
- RunForTime(chrono::seconds(5));
+TYPED_TEST_P(IntakeSystemTest, ZeroNoGoal) {
+ this->RunForTime(chrono::seconds(5));
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
}
-TEST_F(IntakeSystemTest, LowerHardstopStartup) {
- subsystem_plant_.InitializeSubsystemPosition(
- TestIntakeSystemValues::kRange.lower_hard);
- { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper; }
- RunForTime(chrono::seconds(10));
+TYPED_TEST_P(IntakeSystemTest, LowerHardstopStartup) {
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper;
+ this->RunForTime(chrono::seconds(10));
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
// Tests that starting at the upper hardstops doesn't cause an abort.
-TEST_F(IntakeSystemTest, UpperHardstopStartup) {
- subsystem_plant_.InitializeSubsystemPosition(
- TestIntakeSystemValues::kRange.upper_hard);
- { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper; }
- RunForTime(chrono::seconds(10));
+TYPED_TEST_P(IntakeSystemTest, UpperHardstopStartup) {
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper;
+ this->RunForTime(chrono::seconds(10));
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
// Tests that resetting WPILib results in a rezero.
-TEST_F(IntakeSystemTest, ResetTest) {
- subsystem_plant_.InitializeSubsystemPosition(
- TestIntakeSystemValues::kRange.upper);
- {
- subsystem_data_.goal.unsafe_goal =
- TestIntakeSystemValues::kRange.upper - 0.1;
- }
- RunForTime(chrono::seconds(10));
+TYPED_TEST_P(IntakeSystemTest, ResetTest) {
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper - 0.1;
+ this->RunForTime(chrono::seconds(10));
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
- VerifyNearGoal();
- SimulateSensorReset();
- RunForTime(chrono::milliseconds(100));
+ this->VerifyNearGoal();
+ this->SimulateSensorReset();
+ this->RunForTime(chrono::milliseconds(100));
- EXPECT_EQ(
- StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::UNINITIALIZED,
- subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::UNINITIALIZED,
+ this->subsystem_.state());
- RunForTime(chrono::seconds(10));
+ this->RunForTime(chrono::seconds(10));
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
- VerifyNearGoal();
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ this->VerifyNearGoal();
}
// Tests that the internal goals don't change while disabled.
-TEST_F(IntakeSystemTest, DisabledGoalTest) {
- {
- subsystem_data_.goal.unsafe_goal =
- TestIntakeSystemValues::kRange.lower + 0.03;
- }
+TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.03;
// Checks that the subsystem has not moved from its starting position at 0
- RunForTime(chrono::milliseconds(100), false);
- EXPECT_EQ(0.0, subsystem_.goal(0));
+ this->RunForTime(chrono::milliseconds(100), false);
+ EXPECT_EQ(0.0, this->subsystem_.goal(0));
// Now make sure they move correctly
- RunForTime(chrono::seconds(4), true);
- EXPECT_NE(0.0, subsystem_.goal(0));
+ this->RunForTime(chrono::seconds(4), true);
+ EXPECT_NE(0.0, this->subsystem_.goal(0));
}
// Tests that zeroing while disabled works.
-TEST_F(IntakeSystemTest, DisabledZeroTest) {
- { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.lower; }
+TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.lower;
// Run disabled for 2 seconds
- RunForTime(chrono::seconds(2), false);
- EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
- subsystem_.state());
+ this->RunForTime(chrono::seconds(2), false);
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
- RunForTime(chrono::seconds(4), true);
+ this->RunForTime(chrono::seconds(4), true);
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
// Tests that set_min_position limits range properly
-TEST_F(IntakeSystemTest, MinPositionTest) {
- subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.lower_hard;
- RunForTime(chrono::seconds(2), true);
+TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.lower_hard;
+ this->RunForTime(chrono::seconds(2), true);
// Check that kRange.lower is used as the default min position
- EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower);
- EXPECT_NEAR(subsystem_data_.status.position,
- TestIntakeSystemValues::kRange.lower, 0.001);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
+ EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
// Set min position and check that the subsystem increases to that position
- subsystem_.set_min_position(TestIntakeSystemValues::kRange.lower + 0.05);
- RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower + 0.05);
- EXPECT_NEAR(subsystem_data_.status.position,
- TestIntakeSystemValues::kRange.lower + 0.05, 0.001);
+ this->subsystem_.set_min_position(kRange.lower + 0.05);
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.lower + 0.05);
+ EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower + 0.05,
+ 0.001);
// Clear min position and check that the subsystem returns to kRange.lower
- subsystem_.clear_min_position();
- RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower);
- EXPECT_NEAR(subsystem_data_.status.position,
- TestIntakeSystemValues::kRange.lower, 0.001);
+ this->subsystem_.clear_min_position();
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
+ EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
}
// Tests that set_max_position limits range properly
-TEST_F(IntakeSystemTest, MaxPositionTest) {
- subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper_hard;
- RunForTime(chrono::seconds(2), true);
+TYPED_TEST_P(IntakeSystemTest, MaxPositionTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.upper_hard;
+ this->RunForTime(chrono::seconds(2), true);
// Check that kRange.upper is used as the default max position
- EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper);
- EXPECT_NEAR(subsystem_data_.status.position,
- TestIntakeSystemValues::kRange.upper, 0.001);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
+ EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
// Set max position and check that the subsystem lowers to that position
- subsystem_.set_max_position(TestIntakeSystemValues::kRange.upper - 0.05);
- RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper - 0.05);
- EXPECT_NEAR(subsystem_data_.status.position,
- TestIntakeSystemValues::kRange.upper - 0.05, 0.001);
+ this->subsystem_.set_max_position(kRange.upper - 0.05);
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.upper - 0.05);
+ EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper - 0.05,
+ 0.001);
// Clear max position and check that the subsystem returns to kRange.upper
- subsystem_.clear_max_position();
- RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper);
- EXPECT_NEAR(subsystem_data_.status.position,
- TestIntakeSystemValues::kRange.upper, 0.001);
+ this->subsystem_.clear_max_position();
+ this->RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
+ EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
}
// Tests that the subsystem maintains its current position when sent a null goal
-TEST_F(IntakeSystemTest, NullGoalTest) {
- subsystem_data_.goal.unsafe_goal =
- TestIntakeSystemValues::kRange.lower + 0.05;
- RunForTime(chrono::seconds(2), true);
+TYPED_TEST_P(IntakeSystemTest, NullGoalTest) {
+ this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.05;
+ this->RunForTime(chrono::seconds(2), true);
- VerifyNearGoal();
+ this->VerifyNearGoal();
// Run with a null goal
- RunForTime(chrono::seconds(2), true, true);
+ this->RunForTime(chrono::seconds(2), true, true);
// Check that the subsystem has not moved
- VerifyNearGoal();
+ this->VerifyNearGoal();
}
+REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
+ SaturationTest, RespectsRange, ZeroTest, ZeroNoGoal,
+ LowerHardstopStartup, UpperHardstopStartup,
+ ResetTest, DisabledGoalTest, DisabledZeroTest,
+ MinPositionTest, MaxPositionTest, NullGoalTest);
+INSTANTIATE_TYPED_TEST_CASE_P(My, IntakeSystemTest, TestTypes);
+
} // namespace control_loops
} // namespace frc971
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index e0e7bb6..91fbb2d 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -4,7 +4,9 @@
TARGETS='//... @com_github_google_glog//... @com_google_ceres_solver//...'
-bazel test -c opt --curses=no --color=no ${TARGETS}
+# Include --config=eigen to enable Eigen assertions so that we catch potential
+# bugs with Eigen.
+bazel test -c opt --config=eigen --curses=no --color=no ${TARGETS}
bazel build -c opt --curses=no --color=no ${TARGETS} --cpu=roborio
bazel build --curses=no --color=no ${TARGETS} --cpu=armhf-debian
bazel build -c opt --curses=no --color=no //motors/... --cpu=cortex-m4f
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index 89145a4..76c44a4 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -32,7 +32,7 @@
void Intake::Iterate(
const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index 40f49fc..db1442a 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -34,7 +34,8 @@
void Iterate(const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *status);
void Reset();
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 60c5e16..5c0771c 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -200,7 +200,7 @@
bool estopped;
// Each subsystems status.
- .frc971.control_loops.AbsoluteProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
.frc971.control_loops.IndexProfiledJointStatus hood;
ShooterStatus shooter;
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index efe0c1d..a646ab6 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -36,6 +36,7 @@
cc_test(
name = "superstructure_lib_test",
+ timeout = "long",
srcs = [
"superstructure_lib_test.cc",
],
@@ -44,9 +45,9 @@
":superstructure_queue",
"//aos:math",
"//aos:queues",
- "//aos/time:time",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
+ "//aos/time",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
"//y2018/control_loops/superstructure/intake:intake_plants",
@@ -67,12 +68,12 @@
cc_library(
name = "debouncer",
- hdrs = [
- "debouncer.h",
- ],
srcs = [
"debouncer.cc",
],
+ hdrs = [
+ "debouncer.h",
+ ],
)
cc_test(
diff --git a/y2019/BUILD b/y2019/BUILD
index 2eb5e24..576cf6c 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -24,6 +24,10 @@
"//aos/network:team_number",
"//frc971:constants",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2019/control_loops/superstructure/elevator:elevator_plants",
+ "//y2019/control_loops/superstructure/intake:intake_plants",
+ "//y2019/control_loops/superstructure/stilts:stilts_plants",
+ "//y2019/control_loops/superstructure/wrist:wrist_plants",
],
)
@@ -63,6 +67,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
"//third_party/Phoenix-frc-lib:phoenix",
+ "//y2019/control_loops/superstructure:superstructure_queue",
],
)
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 0851577..e91a49c 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -26,19 +26,99 @@
const uint16_t kCompTeamNumber = 971;
const uint16_t kPracticeTeamNumber = 9971;
+const uint16_t kCodingRobotTeamNumber = 7971;
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
+ Values::PotAndAbsConstants *const elevator = &r->elevator;
+ Values::Intake *const intake = &r->intake;
+ Values::PotAndAbsConstants *const stilts = &r->stilts;
+ Values::PotAndAbsConstants *const wrist = &r->wrist;
+
+ elevator->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ elevator->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kElevatorEncoderRatio();
+ elevator->zeroing.zeroing_threshold = 0.0005;
+ elevator->zeroing.moving_buffer_size = 20;
+ elevator->zeroing.allowable_encoder_error = 0.9;
+
+ intake->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ intake->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake->zeroing.zeroing_threshold = 0.0005;
+ intake->zeroing.moving_buffer_size = 20;
+ intake->zeroing.allowable_encoder_error = 0.9;
+
+ stilts->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ stilts->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kStiltsEncoderRatio();
+ stilts->zeroing.zeroing_threshold = 0.0005;
+ stilts->zeroing.moving_buffer_size = 20;
+ stilts->zeroing.allowable_encoder_error = 0.9;
+
+ wrist->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ wrist->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kWristEncoderRatio();
+ wrist->zeroing.zeroing_threshold = 0.0005;
+ wrist->zeroing.moving_buffer_size = 20;
+ wrist->zeroing.allowable_encoder_error = 0.9;
switch (team) {
// A set of constants for tests.
case 1:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
+ break;
+
+ case kCodingRobotTeamNumber:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
break;
default:
@@ -78,6 +158,5 @@
return *values[team_number];
}
-
} // namespace constants
} // namespace y2019
diff --git a/y2019/constants.h b/y2019/constants.h
index e68143c..21c061c 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -6,6 +6,10 @@
#include "frc971/constants.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2019/control_loops/superstructure/intake/intake_plant.h"
+#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
+#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
+#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
namespace y2019 {
namespace constants {
@@ -27,15 +31,130 @@
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
}
- static constexpr double kDrivetrainEncoderRatio() {
- return (24.0 / 52.0);
- }
+ static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
control_loops::drivetrain::kHighOutputRatio /
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
+
+ // Elevator
+ static constexpr double kElevatorEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kElevatorEncoderRatio() {
+ return (1.0) * control_loops::superstructure::elevator::kRadius;
+ }
+
+ static constexpr double kMaxElevatorEncoderPulsesPerSecond() {
+ return control_loops::superstructure::elevator::kFreeSpeed *
+ control_loops::superstructure::elevator::kOutputRatio /
+ kElevatorEncoderRatio() / (2.0 * M_PI) *
+ kElevatorEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kElevatorPotRatio() {
+ return (1.0) * control_loops::superstructure::elevator::kRadius;
+ }
+
+ static constexpr ::frc971::constants::Range kElevatorRange() {
+ return ::frc971::constants::Range{
+ 0.0, // Bottom Hard
+ 1.44, // Top Hard
+ 0.025, // Bottom Soft
+ 1.415 // Top Soft
+ };
+ }
+
+ // Intake
+ static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kIntakeEncoderRatio() { return (18.0 / 38.0); }
+
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake::kFreeSpeed *
+ control_loops::superstructure::intake::kOutputRatio /
+ kIntakeEncoderRatio() / (2.0 * M_PI) *
+ kIntakeEncoderCountsPerRevolution();
+ }
+
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ -1.15, // Back Hard
+ 1.36, // Front Hard
+ -1.14, // Back Soft
+ 1.22 // Front Soft
+ };
+ }
+
+ // Wrist
+ static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kWristEncoderRatio() {
+ return (20.0 / 100.0) * (24.0 / 84.0);
+ }
+
+ static constexpr double kMaxWristEncoderPulsesPerSecond() {
+ return control_loops::superstructure::wrist::kFreeSpeed *
+ control_loops::superstructure::wrist::kOutputRatio /
+ kWristEncoderRatio() / (2.0 * M_PI) *
+ kWristEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kWristPotRatio() { return (24.0) / (84.0); }
+
+ static constexpr ::frc971::constants::Range kWristRange() {
+ return ::frc971::constants::Range{
+ -3.14, // Back Hard
+ 2.58, // Front Hard
+ -2.97, // Back Soft
+ 2.41 // Front Soft
+ };
+ }
+
+ // Stilts
+ static constexpr double kStiltsEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kStiltsEncoderRatio() {
+ return (1.0 /* Gear ratio */) *
+ control_loops::superstructure::stilts::kRadius;
+ }
+
+ static constexpr double kMaxStiltsEncoderPulsesPerSecond() {
+ return control_loops::superstructure::stilts::kFreeSpeed *
+ control_loops::superstructure::stilts::kOutputRatio /
+ kStiltsEncoderRatio() / (2.0 * M_PI) *
+ kStiltsEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kStiltsPotRatio() {
+ return (1.0 /* Gear ratio */) *
+ control_loops::superstructure::stilts::kRadius;
+ }
+
+ static constexpr ::frc971::constants::Range kStiltsRange() {
+ return ::frc971::constants::Range{
+ -0.026, // Top Hard
+ 0.693, // Bottom Hard
+ -0.02, // Top Soft
+ 0.673 // Bottom Soft
+ };
+ }
+
+ struct PotAndAbsConstants {
+ ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+ double potentiometer_offset;
+ };
+ PotAndAbsConstants elevator;
+ PotAndAbsConstants wrist;
+ PotAndAbsConstants stilts;
+
+ struct Intake {
+ ::frc971::constants::AbsoluteEncoderZeroingConstants zeroing;
+ };
+ Intake intake;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2019/control_loops/python/elevator.py b/y2019/control_loops/python/elevator.py
index dd8b412..41c05c6 100755
--- a/y2019/control_loops/python/elevator.py
+++ b/y2019/control_loops/python/elevator.py
@@ -14,12 +14,15 @@
except gflags.DuplicateFlagError:
pass
+first_stage_mass = 0.7957
+carriage_mass = 2.754
+
kElevator = linear_system.LinearSystemParams(
name='Elevator',
motor=control_loop.Vex775Pro(),
G=(8.0 / 82.0),
radius=2.25 * 0.0254 / 2.0,
- mass=4.0,
+ mass=first_stage_mass + carriage_mass,
q_pos=0.070,
q_vel=1.2,
kalman_q_pos=0.12,
diff --git a/y2019/control_loops/python/intake.py b/y2019/control_loops/python/intake.py
index 97f8880..2ac139b 100755
--- a/y2019/control_loops/python/intake.py
+++ b/y2019/control_loops/python/intake.py
@@ -20,9 +20,16 @@
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,
+ G=(1.0 / 7.0) * (1.0 / 4.0) * (1.0 / 4.0)* (18.0 / 38.0),
+ # Suneel: Sampled moment of inertia at 6 different positions
+ # J = the average of the six.
+ # 1. 0.686
+ # 2. 0.637
+ # 3. 0.514
+ # 4. 0.332
+ # 5. 0.183
+ # 6. 0.149
+ J=0.3,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
diff --git a/y2019/control_loops/python/stilts.py b/y2019/control_loops/python/stilts.py
index 8faa5d4..b07321c 100755
--- a/y2019/control_loops/python/stilts.py
+++ b/y2019/control_loops/python/stilts.py
@@ -21,7 +21,7 @@
# 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,
+ mass=1.175 * 2, # 1 stilt foot = 1.175 kg
q_pos=0.070,
q_vel=1.2,
kalman_q_pos=0.12,
diff --git a/y2019/control_loops/python/wrist.py b/y2019/control_loops/python/wrist.py
index 0229f6e..2e292a2 100755
--- a/y2019/control_loops/python/wrist.py
+++ b/y2019/control_loops/python/wrist.py
@@ -17,12 +17,18 @@
except gflags.DuplicateFlagError:
pass
+# Wrist alone
+# 0.1348
+# Wrist with ball
+# 0.3007
+# Wrist with hatch
+# 0.446
+
kWrist = angular_system.AngularSystemParams(
name='Wrist',
motor=control_loop.BAG(),
G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
- # TODO(austin): Pull moments of inertia from CAD when it's done.
- J=0.34,
+ J=0.27,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 5d07e17..453c680 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -39,3 +39,31 @@
"//aos/events:shm-event-loop",
],
)
+
+cc_library(
+ name = "collision_avoidance",
+ srcs = [
+ "collision_avoidance.cc",
+ ],
+ hdrs = [
+ "collision_avoidance.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/controls:control_loop_queues",
+ "//frc971:constants",
+ ],
+)
+
+cc_test(
+ name = "collision_avoidance_tests",
+ srcs = [
+ "collision_avoidance_tests.cc",
+ ],
+ deps = [
+ ":collision_avoidance",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
new file mode 100644
index 0000000..7a26b90
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -0,0 +1,162 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include <cmath>
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double CollisionAvoidance::kElevatorClearHeight;
+constexpr double CollisionAvoidance::kElevatorClearWristDownHeight;
+constexpr double CollisionAvoidance::kElevatorClearIntakeHeight;
+constexpr double CollisionAvoidance::kWristMaxAngle;
+constexpr double CollisionAvoidance::kWristMinAngle;
+constexpr double CollisionAvoidance::kIntakeOutAngle;
+constexpr double CollisionAvoidance::kIntakeInAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMinAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMaxAngle;
+constexpr double CollisionAvoidance::kEps;
+constexpr double CollisionAvoidance::kEpsIntake;
+constexpr double CollisionAvoidance::kEpsWrist;
+
+CollisionAvoidance::CollisionAvoidance() {
+ clear_min_wrist_goal();
+ clear_max_wrist_goal();
+ clear_min_elevator_goal();
+ clear_min_intake_goal();
+ clear_max_intake_goal();
+}
+
+bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Elevator is down, so the wrist can't be close to vertical.
+ if (elevator_position < kElevatorClearHeight) {
+ if (wrist_position < kWristElevatorCollisionMaxAngle &&
+ wrist_position > kWristElevatorCollisionMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so wrist can't go below horizontal in either direction.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ if (wrist_position > kWristMaxAngle) {
+ return true;
+ }
+ if (wrist_position < kWristMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so the intake has to be at either extreme.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
+ return true;
+ }
+ }
+
+ // Nothing is hitting, we must be good.
+ return false;
+}
+
+void CollisionAvoidance::UpdateGoal(
+ const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Start with our constraints being wide open.
+ clear_max_wrist_goal();
+ clear_min_wrist_goal();
+ clear_max_intake_goal();
+ clear_min_intake_goal();
+
+ // If the elevator is low enough, we also can't transition the wrist.
+ if (elevator_position < kElevatorClearHeight) {
+ // Figure out which side the wrist is on and stay there.
+ if (wrist_position < 0.0) {
+ update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
+ } else {
+ update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
+ }
+ }
+
+ // If the elevator is too low, the wrist needs to be above the clearance
+ // angles to avoid crashing the frame.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ update_min_wrist_goal(kWristMinAngle + kEpsWrist);
+ update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+ }
+
+ constexpr double kIntakeMiddleAngle =
+ (kIntakeOutAngle + kIntakeInAngle) / 2.0;
+
+ // If the elevator is too low, the intake can't transition from in to out or
+ // back.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ // Figure out if the intake is in our out and keep it there.
+ if (intake_position < kIntakeMiddleAngle) {
+ update_max_intake_goal(kIntakeInAngle - kEpsIntake);
+ } else {
+ update_min_intake_goal(kIntakeOutAngle + kEpsIntake);
+ }
+ }
+
+ // Start with an unconstrained elevator.
+ clear_min_elevator_goal();
+
+ // If the intake is within the collision range, don't let the elevator down.
+ if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+
+ // If the wrist is within the elevator collision range, don't let the elevator
+ // go down.
+ if (wrist_position > kWristElevatorCollisionMinAngle &&
+ wrist_position < kWristElevatorCollisionMaxAngle) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // If the wrist is far enough down that we are going to hit the frame, don't
+ // let the elevator go too far down.
+ if (wrist_position > kWristMaxAngle || wrist_position < kWristMinAngle) {
+ update_min_elevator_goal(kElevatorClearWristDownHeight + kEps);
+ }
+
+ if (unsafe_goal) {
+ const double wrist_goal = unsafe_goal->wrist.angle;
+ const double intake_goal = unsafe_goal->intake.joint_angle;
+
+ // Compute if we need to move the intake.
+ const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
+ (intake_goal < kIntakeMiddleAngle);
+
+ // Compute if we need to move the wrist across 0.
+ const bool wrist_needs_to_move =
+ (wrist_position < 0.0) ^ (wrist_goal < 0.0);
+
+ // If we need to move the intake, we've got to shove the elevator up. The
+ // intake is already constrained so it can't hit anything until it's clear.
+ if (intake_needs_to_move && wrist_position > 0) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+ // If we need to move the wrist, we've got to shove the elevator up too. The
+ // wrist is already constrained so it can't hit anything until it's clear.
+ // If both the intake and wrist need to move, figure out which one will
+ // require the higher motion and move that.
+ if (wrist_needs_to_move) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // TODO(austin): We won't shove the elevator up if the wrist is asked to go
+ // down below horizontal. I think that's fine.
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
new file mode 100644
index 0000000..1ad9582
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -0,0 +1,111 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+
+#include <cmath>
+#include "aos/controls/control_loops.q.h"
+#include "frc971/constants.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+// CollisionAvoidance computes the min and max allowable ranges for various
+// subsystems to avoid collisions. It also shoves the elevator up to let the
+// intake go in and out, and to let the wrist switch sides.
+class CollisionAvoidance {
+ public:
+ CollisionAvoidance();
+
+ // Reports if the superstructure is collided.
+ bool IsCollided(const SuperstructureQueue::Status *status);
+
+ // Checks and alters goals to make sure they're safe.
+ // TODO(austin): Either we will have a unit delay because this has to happen
+ // after the controls, or we need to be more clever about restructuring.
+ void UpdateGoal(const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal);
+
+ // Returns the goals to give to the respective control loops in
+ // superstructure.
+ double min_wrist_goal() const { return min_wrist_goal_; }
+ double max_wrist_goal() const { return max_wrist_goal_; }
+ double min_elevator_goal() const { return min_elevator_goal_; }
+ double min_intake_goal() const { return min_intake_goal_; }
+ double max_intake_goal() const { return max_intake_goal_; }
+
+ void update_max_wrist_goal(double max_wrist_goal) {
+ max_wrist_goal_ = ::std::min(max_wrist_goal, max_wrist_goal_);
+ }
+ void update_min_wrist_goal(double min_wrist_goal) {
+ min_wrist_goal_ = ::std::max(min_wrist_goal, min_wrist_goal_);
+ }
+ void update_max_intake_goal(double max_intake_goal) {
+ max_intake_goal_ = ::std::min(max_intake_goal, max_intake_goal_);
+ }
+ void update_min_intake_goal(double min_intake_goal) {
+ min_intake_goal_ = ::std::max(min_intake_goal, min_intake_goal_);
+ }
+ void update_min_elevator_goal(double min_elevator_goal) {
+ min_elevator_goal_ = ::std::max(min_elevator_goal, min_elevator_goal_);
+ }
+
+ // TODO(sabina): set all the numbers to correctly match the robot.
+
+ // Height above which we can move the wrist freely.
+ static constexpr double kElevatorClearHeight = 0.5;
+
+ // Height above which we can move the wrist down.
+ static constexpr double kElevatorClearWristDownHeight = 0.3;
+ // Height the carriage needs to be above to move the intake.
+ static constexpr double kElevatorClearIntakeHeight = 0.4;
+
+ // Angle constraints for the wrist when below kElevatorClearDownHeight
+ static constexpr double kWristMaxAngle = M_PI / 2.0;
+ static constexpr double kWristMinAngle = -M_PI / 2.0;
+
+ // Angles outside of which the intake is fully clear of the wrist.
+ static constexpr double kIntakeOutAngle = M_PI / 6.0;
+ static constexpr double kIntakeInAngle = -M_PI / 3.0;
+
+ // Angles within which we will crash the wrist into the elevator if the
+ // elevator is below kElevatorClearHeight.
+ static constexpr double kWristElevatorCollisionMinAngle = -M_PI / 4.0;
+ static constexpr double kWristElevatorCollisionMaxAngle = M_PI / 4.0;
+
+ // Tolerance for the elevator.
+ static constexpr double kEps = 0.02;
+ // Tolerance for the intake.
+ static constexpr double kEpsIntake = 0.05;
+ // Tolerance for the wrist.
+ static constexpr double kEpsWrist = 0.05;
+
+ private:
+ void clear_min_wrist_goal() {
+ min_wrist_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_wrist_goal() {
+ max_wrist_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_elevator_goal() {
+ min_elevator_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_intake_goal() {
+ min_intake_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_intake_goal() {
+ max_intake_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+
+ double min_wrist_goal_;
+ double max_wrist_goal_;
+ double min_elevator_goal_;
+ double min_intake_goal_;
+ double max_intake_goal_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
new file mode 100644
index 0000000..92a1a70
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -0,0 +1,348 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include "aos/commonmath.h"
+#include "gtest/gtest.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+/*
+Test List:
+ FullClockwiseRotationFromBottomBackIntakeIn
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle
+ QuarterClockwiseRotationFromMiddleFrontIntakeMoving
+
+ FullCounterClockwiseRotationFromBottomFrontIntakeIn
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeOut
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
+*/
+
+class CollisionAvoidanceTests : public ::testing::Test {
+ public:
+ void Iterate() {
+ SuperstructureQueue::Goal safe_goal;
+ bool was_collided = avoidance.IsCollided(&status);
+
+ while (true) {
+ avoidance.UpdateGoal(&status, &unsafe_goal);
+ if (!was_collided) {
+ EXPECT_FALSE(avoidance.IsCollided(&status));
+ } else {
+ was_collided = avoidance.IsCollided(&status);
+ }
+
+ safe_goal.wrist.angle =
+ ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+ avoidance.max_wrist_goal());
+
+ safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+ avoidance.min_elevator_goal());
+
+ safe_goal.intake.joint_angle =
+ ::aos::Clip(unsafe_goal.intake.joint_angle,
+ avoidance.min_intake_goal(), avoidance.max_intake_goal());
+
+ LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
+ LimitedMove(&status.elevator.position, safe_goal.elevator.height);
+ LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+ if (IsMoving()) {
+ break;
+ }
+ past_status = status;
+ }
+ }
+
+ bool IsMoving() {
+ if ((past_status.wrist.position == status.wrist.position) &&
+ (past_status.elevator.position == status.elevator.position) &&
+ (past_status.intake.position == status.intake.position)) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // provide goals and status messages
+ SuperstructureQueue::Goal unsafe_goal;
+ SuperstructureQueue::Status status;
+ SuperstructureQueue::Status past_status;
+
+ protected:
+ // setup for all tests
+ CollisionAvoidance avoidance;
+
+ void CheckGoals() {
+ // check to see if we reached the goals
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ }
+
+ private:
+ void LimitedMove(float *position, double goal) {
+ if (*position + kIterationMove < goal) {
+ *position += kIterationMove;
+ } else if (*position - kIterationMove > goal) {
+ *position -= kIterationMove;
+ } else {
+ *position = goal;
+ }
+ }
+
+ static constexpr double kIterationMove = 0.001;
+};
+// It is trying to rotate from far back to front low.
+TEST_F(CollisionAvoidanceTests, FullClockwiseRotationFromBottomBackIntakeIn) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle;
+
+ Iterate();
+
+ CheckGoals();
+}
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ status.wrist.position = avoidance.kWristMaxAngle / 2.0;
+ status.elevator.position = 0.5;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from front low to far back.
+TEST_F(CollisionAvoidanceTests,
+ FullCounterClockwiseRotationFromBottomBackIntakeIn) {
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.5;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// Unreasonable Elevator Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableElevatorGoal) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = 4.0;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.45;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+// Unreasonable Wrist Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableWristGoal) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.45;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+// Fix Collision Wrist in Elevator
+TEST_F(CollisionAvoidanceTests, FixElevatorCollision) {
+ // changes the goals
+ unsafe_goal.wrist.angle = 3.14;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes
+ status.wrist.position = 4.0;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+// Fix Collision Wrist in Intake
+TEST_F(CollisionAvoidanceTests, FixWristCollision) {
+ // changes the goals
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ // sets the status position messgaes
+ status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+// Fix Collision Wrist Above Elevator
+TEST_F(CollisionAvoidanceTests, FixWristElevatorCollision) {
+ // changes the goals
+ unsafe_goal.wrist.angle = 0.0;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes
+ status.wrist.position = 0.0;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/elevator/BUILD b/y2019/control_loops/superstructure/elevator/BUILD
new file mode 100644
index 0000000..f3d99fe
--- /dev/null
+++ b/y2019/control_loops/superstructure/elevator/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_elevator",
+ outs = [
+ "elevator_plant.h",
+ "elevator_plant.cc",
+ "integral_elevator_plant.h",
+ "integral_elevator_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:elevator) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:elevator",
+ ],
+)
+
+cc_library(
+ name = "elevator_plants",
+ srcs = [
+ "elevator_plant.cc",
+ "integral_elevator_plant.cc",
+ ],
+ hdrs = [
+ "elevator_plant.h",
+ "integral_elevator_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/intake/BUILD b/y2019/control_loops/superstructure/intake/BUILD
new file mode 100644
index 0000000..4f7f6b0
--- /dev/null
+++ b/y2019/control_loops/superstructure/intake/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "integral_intake_plant.h",
+ "integral_intake_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:intake) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:intake",
+ ],
+)
+
+cc_library(
+ name = "intake_plants",
+ srcs = [
+ "intake_plant.cc",
+ "integral_intake_plant.cc",
+ ],
+ hdrs = [
+ "intake_plant.h",
+ "integral_intake_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/stilts/BUILD b/y2019/control_loops/superstructure/stilts/BUILD
new file mode 100644
index 0000000..6beea39
--- /dev/null
+++ b/y2019/control_loops/superstructure/stilts/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_stilts",
+ outs = [
+ "stilts_plant.h",
+ "stilts_plant.cc",
+ "integral_stilts_plant.h",
+ "integral_stilts_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:stilts) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:stilts",
+ ],
+)
+
+cc_library(
+ name = "stilts_plants",
+ srcs = [
+ "stilts_plant.cc",
+ "integral_stilts_plant.cc",
+ ],
+ hdrs = [
+ "stilts_plant.h",
+ "integral_stilts_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index 2cd7238..cd004ac 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -67,10 +67,10 @@
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;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
};
message Position {
@@ -86,7 +86,7 @@
.frc971.PotAndAbsolutePosition wrist;
// Position of the intake. 0 when rollers are retracted, positive extended.
- .frc971.PotAndAbsolutePosition intake_joint;
+ .frc971.AbsolutePosition intake_joint;
// Position of the stilts, 0 when retracted (defualt), positive lifts robot.
.frc971.PotAndAbsolutePosition stilts;
@@ -124,4 +124,4 @@
queue Position position;
};
-queue_group SuperstructureQueue superstructure_queue;
\ No newline at end of file
+queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2019/control_loops/superstructure/wrist/BUILD b/y2019/control_loops/superstructure/wrist/BUILD
new file mode 100644
index 0000000..3d007be
--- /dev/null
+++ b/y2019/control_loops/superstructure/wrist/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_wrist",
+ outs = [
+ "wrist_plant.h",
+ "wrist_plant.cc",
+ "integral_wrist_plant.h",
+ "integral_wrist_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:wrist) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:wrist",
+ ],
+)
+
+cc_library(
+ name = "wrist_plants",
+ srcs = [
+ "wrist_plant.cc",
+ "integral_wrist_plant.cc",
+ ],
+ hdrs = [
+ "wrist_plant.h",
+ "integral_wrist_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index bc6c25f..428e723 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -41,12 +41,14 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
using ::frc971::control_loops::drivetrain_queue;
+using ::y2019::control_loops::superstructure::superstructure_queue;
using ::y2019::constants::Values;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -89,15 +91,30 @@
control_loops::drivetrain::kWheelRadius;
}
+double elevator_pot_translate(double voltage) {
+ return voltage * Values::kElevatorPotRatio() *
+ (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
+double wrist_pot_translate(double voltage) {
+ return voltage * Values::kWristPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+}
+
+double stilts_pot_translate(double voltage) {
+ return voltage * Values::kStiltsPotRatio() *
+ (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
constexpr double kMaxFastEncoderPulsesPerSecond =
- max(/*Values::kMaxDrivetrainEncoderPulsesPerSecond(),
- Values::kMaxIntakeMotorEncoderPulsesPerSecond()*/ 1.0, 1.0);
+ max(Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxIntakeEncoderPulsesPerSecond());
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
constexpr double kMaxMediumEncoderPulsesPerSecond =
- max(/*Values::kMaxProximalEncoderPulsesPerSecond(),
- Values::kMaxDistalEncoderPulsesPerSecond()*/ 1.0, 1.0);
+ max(Values::kMaxElevatorEncoderPulsesPerSecond(),
+ Values::kMaxWristEncoderPulsesPerSecond());
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
"medium encoders are too fast");
@@ -111,6 +128,69 @@
UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
}
+ // Elevator
+
+ void set_elevator_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ elevator_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_elevator_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ elevator_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_elevator_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ elevator_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ // Intake
+
+ void set_intake_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ intake_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_intake_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ intake_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ // Wrist
+
+ void set_wrist_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ wrist_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_wrist_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ wrist_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_wrist_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ wrist_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ // Stilts
+
+ void set_stilts_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ stilts_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_stilts_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ stilts_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_stilts_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ stilts_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -127,6 +207,113 @@
drivetrain_message.Send();
}
}
+
+ void RunDmaIteration() {
+ const auto values = constants::GetValues();
+
+ {
+ auto superstructure_message = superstructure_queue.position.MakeMessage();
+
+ // Elevator
+ CopyPosition(elevator_encoder_, &superstructure_message->elevator,
+ Values::kElevatorEncoderCountsPerRevolution(),
+ Values::kElevatorEncoderRatio(), elevator_pot_translate,
+ false, values.elevator.potentiometer_offset);
+ // Intake
+ CopyPosition(intake_encoder_, &superstructure_message->intake_joint,
+ Values::kIntakeEncoderCountsPerRevolution(),
+ Values::kIntakeEncoderRatio(), false);
+
+ // Wrist
+ CopyPosition(wrist_encoder_, &superstructure_message->wrist,
+ Values::kWristEncoderCountsPerRevolution(),
+ Values::kWristEncoderRatio(), wrist_pot_translate, false,
+ values.wrist.potentiometer_offset);
+
+ // Stilts
+ CopyPosition(stilts_encoder_, &superstructure_message->stilts,
+ Values::kStiltsEncoderCountsPerRevolution(),
+ Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
+ values.stilts.potentiometer_offset);
+
+ superstructure_message.Send();
+ }
+ }
+
+ private:
+ ::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
+ wrist_encoder_, stilts_encoder_;
+
+ ::frc971::wpilib::AbsoluteEncoder intake_encoder_;
+ // TODO(sabina): Add wrist and elevator hall effects.
+};
+
+class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_elevator_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ elevator_victor_ = ::std::move(t);
+ }
+
+ void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ intake_victor_ = ::std::move(t);
+ }
+ void set_intake_rollers_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ intake_rollers_victor_ = ::std::move(t);
+ }
+
+ void set_wrist_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ wrist_victor_ = ::std::move(t);
+ }
+
+ void set_stilts_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ stilts_victor_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::y2019::control_loops::superstructure::superstructure_queue.output
+ .FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue =
+ ::y2019::control_loops::superstructure::superstructure_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ elevator_victor_->SetSpeed(::aos::Clip(-queue->elevator_voltage,
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ intake_victor_->SetSpeed(::aos::Clip(-queue->intake_joint_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+
+ intake_rollers_victor_->SetSpeed(::aos::Clip(-queue->intake_roller_voltage,
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ wrist_victor_->SetSpeed(::aos::Clip(-queue->wrist_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+
+ stilts_victor_->SetSpeed(::aos::Clip(-queue->stilts_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Superstructure output too old.\n");
+
+ elevator_victor_->SetDisabled();
+ intake_victor_->SetDisabled();
+ intake_rollers_victor_->SetDisabled();
+ wrist_victor_->SetDisabled();
+ stilts_victor_->SetDisabled();
+ }
+
+ ::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
+ intake_rollers_victor_, wrist_victor_, stilts_victor_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -151,6 +338,21 @@
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
+ reader.set_elevator_encoder(make_encoder(2));
+ reader.set_elevator_absolute_pwm(make_unique<frc::DigitalInput>(0));
+ reader.set_elevator_potentiometer(make_unique<frc::AnalogInput>(0));
+
+ reader.set_wrist_encoder(make_encoder(3));
+ reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(1));
+ reader.set_wrist_potentiometer(make_unique<frc::AnalogInput>(2));
+
+ reader.set_intake_encoder(make_encoder(4));
+ reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(3));
+
+ reader.set_stilts_encoder(make_encoder(5));
+ reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
+
reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
::std::thread reader_thread(::std::ref(reader));
@@ -174,6 +376,21 @@
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+ SuperstructureWriter superstructure_writer;
+ superstructure_writer.set_elevator_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
+ superstructure_writer.set_intake_rollers_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ superstructure_writer.set_intake_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
+ superstructure_writer.set_wrist_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
+ superstructure_writer.set_stilts_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+
+ ::std::thread superstructure_writer_thread(
+ ::std::ref(superstructure_writer));
+
// Wait forever. Not much else to do...
while (true) {
const int r = select(0, nullptr, nullptr, nullptr, nullptr);
@@ -197,6 +414,8 @@
drivetrain_writer.Quit();
drivetrain_writer_thread.join();
+ superstructure_writer.Quit();
+ superstructure_writer_thread.join();
::aos::Cleanup();
}