Merge branch 'angle-adjust' into merged_loops
diff --git a/aos/common/time.cc b/aos/common/time.cc
index 00c936d..995011a 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -8,21 +8,66 @@
#include "aos/common/logging/logging.h"
#include "aos/common/inttypes.h"
+#include "aos/common/mutex.h"
namespace aos {
namespace time {
-Time Time::Now(clockid_t clock) {
- timespec temp;
- if (clock_gettime(clock, &temp) != 0) {
- // TODO(aschuh): There needs to be a pluggable low level logging interface
- // so we can break this dependency loop. This also would help during
- // startup.
- LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
- static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
- }
- return Time(temp);
+// State required to enable and use mock time.
+namespace {
+// True if mock time is enabled.
+// This does not need to be checked with the mutex held because setting time to
+// be enabled or disabled is atomic, and all future operations are atomic
+// anyways. If there is a race condition setting or clearing whether time is
+// enabled or not, it will still be a race condition if current_mock_time is
+// also set atomically with enabled.
+bool mock_time_enabled = false;
+// Mutex to make time reads and writes thread safe.
+Mutex time_mutex;
+// Current time when time is mocked.
+Time current_mock_time(0, 0);
+
+// TODO(aschuh): This doesn't include SleepFor and SleepUntil.
+// TODO(aschuh): Create a clock source object and change the default?
+// That would let me create a MockTime clock source.
}
+
+void Time::EnableMockTime(const Time now) {
+ mock_time_enabled = true;
+ MutexLocker time_mutex_locker(&time_mutex);
+ current_mock_time = now;
+}
+
+void Time::DisableMockTime() {
+ MutexLocker time_mutex_locker(&time_mutex);
+ mock_time_enabled = false;
+}
+
+void Time::SetMockTime(const Time now) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ if (!mock_time_enabled) {
+ LOG(FATAL, "Tried to set mock time and mock time is not enabled\n");
+ }
+ current_mock_time = now;
+}
+
+Time Time::Now(clockid_t clock) {
+ if (mock_time_enabled) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ return current_mock_time;
+ } else {
+ timespec temp;
+ if (clock_gettime(clock, &temp) != 0) {
+ // TODO(aschuh): There needs to be a pluggable low level logging interface
+ // so we can break this dependency loop. This also would help during
+ // startup.
+ LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
+ static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
+ }
+ return Time(temp);
+ }
+}
+
void Time::Check() {
if (nsec_ >= kNSecInSec || nsec_ < 0) {
LOG(FATAL, "0 <= nsec_(%"PRId32") < %"PRId32" isn't true.\n",
diff --git a/aos/common/time.h b/aos/common/time.h
index 1d7ccae..38238f2 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -60,6 +60,7 @@
return ans;
}
#endif // SWIG
+
// CLOCK_MONOTONIC on the fitpc and CLOCK_REALTIME on the cRIO because the
// cRIO doesn't have any others.
// CLOCK_REALTIME is the default realtime clock and CLOCK_MONOTONIC doesn't
@@ -159,6 +160,15 @@
Check();
}
+ // Enables returning the mock time value for Now instead of checking the
+ // system clock. This should only be used when testing things depending on
+ // time, or many things may/will break.
+ static void EnableMockTime(const Time now);
+ // Sets now when time is being mocked.
+ static void SetMockTime(const Time now);
+ // Disables mocking time.
+ static void DisableMockTime();
+
private:
int32_t sec_, nsec_;
// LOG(FATAL)s if nsec_ is >= kNSecInSec.
diff --git a/frc971/atom_code/.gitignore b/frc971/atom_code/.gitignore
new file mode 100644
index 0000000..b1eea9c
--- /dev/null
+++ b/frc971/atom_code/.gitignore
@@ -0,0 +1,2 @@
+/shooter.csv
+/wrist.csv
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 277c378..e9c787e 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -8,9 +8,13 @@
'../control_loops/control_loops.gyp:DriveTrain',
'../control_loops/wrist/wrist.gyp:wrist',
'../control_loops/wrist/wrist.gyp:wrist_lib_test',
+ '../control_loops/index/index.gyp:index',
+ '../control_loops/index/index.gyp:index_lib_test',
'../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust',
'../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_lib_test',
'../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_csv',
+ '../control_loops/shooter/shooter.gyp:shooter_lib_test',
+ '../control_loops/shooter/shooter.gyp:shooter',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
'../input/input.gyp:GyroReader',
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
new file mode 100644
index 0000000..8ba9c33
--- /dev/null
+++ b/frc971/control_loops/index/index.cc
@@ -0,0 +1,927 @@
+#include "frc971/control_loops/index/index.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/inttypes.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+
+double IndexMotor::Frisbee::ObserveNoTopDiscSensor(
+ double index_position, double index_velocity) {
+ // The absolute disc position in meters.
+ double disc_position = absolute_position(index_position);
+ if (IndexMotor::kTopDiscDetectStart <= disc_position &&
+ disc_position <= IndexMotor::kTopDiscDetectStop) {
+ // Whoops, this shouldn't be happening.
+ // Move the disc off the way that makes most sense.
+ double distance_to_above = IndexMotor::ConvertDiscPositionToIndex(
+ ::std::abs(disc_position - IndexMotor::kTopDiscDetectStop));
+ double distance_to_below = IndexMotor::ConvertDiscPositionToIndex(
+ ::std::abs(disc_position - IndexMotor::kTopDiscDetectStart));
+ if (::std::abs(index_velocity) < 100000) {
+ if (distance_to_above < distance_to_below) {
+ LOG(INFO, "Moving disc to top slow.\n");
+ // Move it up.
+ index_start_position_ -= distance_to_above;
+ return -distance_to_above;
+ } else {
+ LOG(INFO, "Moving disc to bottom slow.\n");
+ index_start_position_ += distance_to_below;
+ return distance_to_below;
+ }
+ } else {
+ if (index_velocity > 0) {
+ // Now going up. If we didn't see it before, and we don't see it
+ // now but it should be in view, it must still be below. If it were
+ // above, it would be going further away from us.
+ LOG(INFO, "Moving fast up, shifting disc down. Disc was at %f\n",
+ absolute_position(index_position));
+ index_start_position_ += distance_to_below;
+ LOG(INFO, "Moving fast up, shifting disc down. Disc now at %f\n",
+ absolute_position(index_position));
+ return distance_to_below;
+ } else {
+ LOG(INFO, "Moving fast down, shifting disc up. Disc was at %f\n",
+ absolute_position(index_position));
+ index_start_position_ -= distance_to_above;
+ LOG(INFO, "Moving fast down, shifting disc up. Disc now at %f\n",
+ absolute_position(index_position));
+ return -distance_to_above;
+ }
+ }
+ }
+ return 0.0;
+}
+
+IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
+ : aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
+ wrist_loop_(new IndexStateFeedbackLoop(MakeIndexLoop())),
+ hopper_disc_count_(0),
+ total_disc_count_(0),
+ shot_disc_count_(0),
+ safe_goal_(Goal::HOLD),
+ loader_goal_(LoaderGoal::READY),
+ loader_state_(LoaderState::READY),
+ loader_up_(false),
+ disc_clamped_(false),
+ disc_ejected_(false),
+ last_bottom_disc_detect_(false),
+ last_top_disc_detect_(false),
+ no_prior_position_(true),
+ missing_position_count_(0) {
+}
+
+/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
+/*static*/ const double IndexMotor::kIndexStartPosition = 0.2159;
+/*static*/ const double IndexMotor::kIndexFreeLength =
+ IndexMotor::ConvertDiscAngleToDiscPosition((360 * 2 + 14) * M_PI / 180);
+/*static*/ const double IndexMotor::kLoaderFreeStopPosition =
+ kIndexStartPosition + kIndexFreeLength;
+/*static*/ const double IndexMotor::kReadyToPreload =
+ kLoaderFreeStopPosition - ConvertDiscAngleToDiscPosition(M_PI / 6.0);
+/*static*/ const double IndexMotor::kReadyToLiftPosition =
+ kLoaderFreeStopPosition + 0.2921;
+/*static*/ const double IndexMotor::kGrabberLength = 0.03175;
+/*static*/ const double IndexMotor::kGrabberStartPosition =
+ kReadyToLiftPosition - kGrabberLength;
+/*static*/ const double IndexMotor::kGrabberMovementVelocity = 0.7;
+/*static*/ const double IndexMotor::kLifterStopPosition =
+ kReadyToLiftPosition + 0.161925;
+/*static*/ const double IndexMotor::kLifterMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kEjectorStopPosition =
+ kLifterStopPosition + 0.01;
+/*static*/ const double IndexMotor::kEjectorMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kBottomDiscDetectStart = 0.00;
+/*static*/ const double IndexMotor::kBottomDiscDetectStop = 0.13;
+/*static*/ const double IndexMotor::kBottomDiscIndexDelay = 0.032;
+
+// TODO(aschuh): Verify these with the sensor actually on.
+/*static*/ const double IndexMotor::kTopDiscDetectStart =
+ (IndexMotor::kLoaderFreeStopPosition -
+ IndexMotor::ConvertDiscAngleToDiscPosition(49 * M_PI / 180));
+/*static*/ const double IndexMotor::kTopDiscDetectStop =
+ (IndexMotor::kLoaderFreeStopPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(19 * M_PI / 180));
+
+// I measured the angle between 2 discs. That then gives me the distance
+// between 2 posedges (or negedges). Then subtract off the width of the
+// positive pulse, and that gives the width of the negative pulse.
+/*static*/ const double IndexMotor::kTopDiscDetectMinSeperation =
+ (IndexMotor::ConvertDiscAngleToDiscPosition(120 * M_PI / 180) -
+ (IndexMotor::kTopDiscDetectStop - IndexMotor::kTopDiscDetectStart));
+
+const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kTransferRollerRadius = 1.25 * 0.0254 / 2;
+
+/*static*/ const int IndexMotor::kGrabbingDelay = 5;
+/*static*/ const int IndexMotor::kLiftingDelay = 30;
+/*static*/ const int IndexMotor::kShootingDelay = 5;
+/*static*/ const int IndexMotor::kLoweringDelay = 20;
+
+// TODO(aschuh): Tune these.
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kMinMotionVoltage = 6.0;
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kNoMotionCuttoffCount = 20;
+
+// Distance to move the indexer when grabbing a disc.
+const double kNextPosition = 10.0;
+
+/*static*/ double IndexMotor::ConvertDiscAngleToIndex(const double angle) {
+ return (angle * (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(
+ const double angle) {
+ return angle * (kDiscRadius + kRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertDiscPositionToDiscAngle(
+ const double position) {
+ return position / (kDiscRadius + kRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscAngle(const double angle) {
+ return (angle / (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscPosition(const double angle) {
+ return IndexMotor::ConvertDiscAngleToDiscPosition(
+ ConvertIndexToDiscAngle(angle));
+}
+
+/*static*/ double IndexMotor::ConvertTransferToDiscPosition(
+ const double angle) {
+ const double gear_ratio = (1 + (kDiscRadius * 2 + kTransferRollerRadius) /
+ kTransferRollerRadius);
+ return angle / gear_ratio * (kDiscRadius + kTransferRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertDiscPositionToIndex(
+ const double position) {
+ return IndexMotor::ConvertDiscAngleToIndex(
+ ConvertDiscPositionToDiscAngle(position));
+}
+
+bool IndexMotor::MinDiscPosition(double *disc_position, Frisbee **found_disc) {
+ bool found_start = false;
+ for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+ Frisbee &frisbee = frisbees_[i];
+ if (!found_start) {
+ if (frisbee.has_position()) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ found_start = true;
+ }
+ } else {
+ if (frisbee.position() <= *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
+ }
+ }
+ return found_start;
+}
+
+bool IndexMotor::MaxDiscPosition(double *disc_position, Frisbee **found_disc) {
+ bool found_start = false;
+ for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+ Frisbee &frisbee = frisbees_[i];
+ if (!found_start) {
+ if (frisbee.has_position()) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ found_start = true;
+ }
+ } else {
+ if (frisbee.position() > *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
+ }
+ }
+ return found_start;
+}
+
+void IndexMotor::IndexStateFeedbackLoop::CapU() {
+ // If the voltage has been low for a large number of cycles, cut the motor
+ // power. This is generally very bad controls practice since this isn't LTI,
+ // but we don't really care about tracking anything other than large step
+ // inputs, and the loader doesn't need to be that accurate.
+ if (::std::abs(U(0, 0)) < kMinMotionVoltage) {
+ ++low_voltage_count_;
+ if (low_voltage_count_ > kNoMotionCuttoffCount) {
+ printf("Limiting power from %f to 0\n", U(0, 0));
+ U(0, 0) = 0.0;
+ }
+ } else {
+ low_voltage_count_ = 0;
+ }
+
+ for (int i = 0; i < kNumOutputs; ++i) {
+ if (U[i] > plant.U_max[i]) {
+ U[i] = plant.U_max[i];
+ } else if (U[i] < plant.U_min[i]) {
+ U[i] = plant.U_min[i];
+ }
+ }
+}
+
+
+// Positive angle is towards the shooter, and positive power is towards the
+// shooter.
+void IndexMotor::RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status) {
+ // Make goal easy to work with and sanity check it.
+ Goal goal_enum = static_cast<Goal>(goal->goal_state);
+ if (goal->goal_state < 0 || goal->goal_state > 4) {
+ LOG(ERROR, "Goal state is %"PRId32" which is out of range. Going to HOLD.\n",
+ goal->goal_state);
+ goal_enum = Goal::HOLD;
+ }
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ double intake_voltage = 0.0;
+ double transfer_voltage = 0.0;
+ if (output) {
+ output->intake_voltage = 0.0;
+ output->transfer_voltage = 0.0;
+ output->index_voltage = 0.0;
+ }
+
+ status->ready_to_intake = false;
+
+ // Compute a safe index position that we can use.
+ if (position) {
+ wrist_loop_->Y << position->index_position;
+ // Set the goal to be the current position if this is the first time through
+ // so we don't always spin the indexer to the 0 position before starting.
+ if (no_prior_position_) {
+ wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
+ wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
+ no_prior_position_ = false;
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // The open positions for the upper is right here and isn't a hard edge.
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
+ }
+
+ // If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
+ if (missing_position_count_ > 50) {
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // We can't really trust the open range any more if the crio rebooted.
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
+ // Adjust the disc positions so that they don't have to move.
+ const double disc_offset =
+ position->index_position - wrist_loop_->X_hat(0, 0);
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ frisbee->OffsetDisc(disc_offset);
+ }
+ wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
+ }
+ missing_position_count_ = 0;
+ } else {
+ ++missing_position_count_;
+ }
+ const double index_position = wrist_loop_->X_hat(0, 0);
+
+ if (position) {
+ // Reset the open region if we saw a negedge.
+ if (position->bottom_disc_negedge_wait_count !=
+ last_bottom_disc_negedge_wait_count_) {
+ // Saw a negedge, must be a new region.
+ lower_open_region_.Restart(position->bottom_disc_negedge_wait_position);
+ }
+ // Reset the open region if we saw a negedge.
+ if (position->top_disc_negedge_count != last_top_disc_negedge_count_) {
+ // Saw a negedge, must be a new region.
+ upper_open_region_.Restart(position->top_disc_negedge_position);
+ }
+
+ // No disc. Expand the open region.
+ if (!position->bottom_disc_detect) {
+ lower_open_region_.Expand(index_position);
+ }
+
+ // No disc. Expand the open region.
+ if (!position->top_disc_detect) {
+ upper_open_region_.Expand(index_position);
+ }
+
+ if (!position->top_disc_detect) {
+ // We don't see a disc. Verify that there are no discs that we should be
+ // seeing.
+ // Assume that discs will move slow enough that we won't miss one as it
+ // goes by. They will either pile up above or below the sensor.
+
+ double cumulative_offset = 0.0;
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ frisbee->OffsetDisc(cumulative_offset);
+ double amount_moved = frisbee->ObserveNoTopDiscSensor(
+ wrist_loop_->X_hat(0, 0), wrist_loop_->X_hat(1, 0));
+ cumulative_offset += amount_moved;
+ }
+ }
+
+ if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
+ const double index_position = wrist_loop_->X_hat(0, 0) -
+ position->index_position + position->top_disc_posedge_position;
+ // TODO(aschuh): Sanity check this number...
+ // Requires storing when the disc was last seen with the sensor off, and
+ // figuring out what to do if things go south.
+
+ // 1 if discs are going up, 0 if we have no clue, and -1 if they are going
+ // down.
+ int disc_direction = 0;
+ if (wrist_loop_->X_hat(1, 0) > 50.0) {
+ disc_direction = 1;
+ } else if (wrist_loop_->X_hat(1, 0) < -50.0) {
+ disc_direction = -1;
+ } else {
+ // Save the upper and lower positions that we last saw a disc at.
+ // If there is a big buffer above, must be a disc from below.
+ // If there is a big buffer below, must be a disc from above.
+ // This should work to replace the velocity threshold above.
+
+ const double open_width = upper_open_region_.width();
+ const double relative_upper_open_precentage =
+ (upper_open_region_.upper_bound() - index_position) / open_width;
+ const double relative_lower_open_precentage =
+ (index_position - upper_open_region_.lower_bound()) / open_width;
+ printf("Width %f upper %f lower %f\n",
+ open_width, relative_upper_open_precentage,
+ relative_lower_open_precentage);
+
+ if (ConvertIndexToDiscPosition(open_width) <
+ kTopDiscDetectMinSeperation * 0.9) {
+ LOG(ERROR, "Discs are way too close to each other. Doing nothing\n");
+ } else if (relative_upper_open_precentage > 0.75) {
+ // Looks like it is a disc going down from above since we are near
+ // the upper edge.
+ disc_direction = -1;
+ LOG(INFO, "Disc edge going down\n");
+ } else if (relative_lower_open_precentage > 0.75) {
+ // Looks like it is a disc going up from below since we are near
+ // the lower edge.
+ disc_direction = 1;
+ LOG(INFO, "Disc edge going up\n");
+ } else {
+ LOG(ERROR,
+ "Got an edge in the middle of what should be an open region.\n");
+ LOG(ERROR, "Open width: %f upper precentage %f %%\n",
+ open_width, relative_upper_open_precentage);
+ }
+ }
+
+ if (disc_direction > 0) {
+ // Moving up at a reasonable clip.
+ // Find the highest disc that is below the top disc sensor.
+ // While we are at it, count the number above and log an error if there
+ // are too many.
+ if (frisbees_.size() == 0) {
+ Frisbee new_frisbee;
+ new_frisbee.has_been_indexed_ = true;
+ new_frisbee.index_start_position_ = index_position -
+ ConvertDiscPositionToIndex(kTopDiscDetectStart -
+ kIndexStartPosition);
+ ++hopper_disc_count_;
+ ++total_disc_count_;
+ frisbees_.push_front(new_frisbee);
+ LOG(WARNING, "Added a disc to the hopper at the top sensor\n");
+ }
+
+ int above_disc_count = 0;
+ double highest_position = 0;
+ Frisbee *highest_frisbee_below_sensor = NULL;
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ // It is save to use the top position for the cuttoff, since the
+ // sensor being low will result in discs being pushed off of it.
+ if (disc_position >= kTopDiscDetectStop) {
+ ++above_disc_count;
+ } else if (!highest_frisbee_below_sensor ||
+ disc_position > highest_position) {
+ highest_frisbee_below_sensor = &*frisbee;
+ highest_position = disc_position;
+ }
+ }
+ if (above_disc_count > 1) {
+ LOG(ERROR, "We have 2 discs above the top sensor.\n");
+ }
+
+ // We now have the disc. Shift all the ones below the sensor up by the
+ // computed delta.
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ highest_position - kTopDiscDetectStart);
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ if (disc_position < kTopDiscDetectStop) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
+ LOG(INFO, "Currently have %d discs, saw posedge moving up. "
+ "Moving down by %f to %f\n", frisbees_.size(),
+ ConvertIndexToDiscPosition(disc_delta),
+ highest_frisbee_below_sensor->absolute_position(
+ wrist_loop_->X_hat(0, 0)));
+ } else if (disc_direction < 0) {
+ // Moving down at a reasonable clip.
+ // There can only be 1 disc up top that would give us a posedge.
+ // Find it and place it at the one spot that it can be.
+ double min_disc_position = 0;
+ Frisbee *min_frisbee = NULL;
+ MinDiscPosition(&min_disc_position, &min_frisbee);
+ if (!min_frisbee) {
+ // Uh, oh, we see a disc but there isn't one...
+ LOG(ERROR, "Saw a disc up top but there isn't one in the hopper\n");
+ } else {
+ const double disc_position = min_frisbee->absolute_position(
+ index_position);
+
+ const double disc_delta_meters = disc_position - kTopDiscDetectStop;
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ disc_delta_meters);
+ LOG(INFO, "Posedge going down. Moving top disc down by %f\n",
+ disc_delta_meters);
+ for (auto frisbee = frisbees_.begin(), end = frisbees_.end();
+ frisbee != end; ++frisbee) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
+ } else {
+ LOG(ERROR, "Not sure how to handle the upper posedge, doing nothing\n");
+ }
+ }
+ }
+
+ // Bool to track if it is safe for the goal to change yet.
+ bool safe_to_change_state_ = true;
+ switch (safe_goal_) {
+ case Goal::HOLD:
+ // The goal should already be good, so sit tight with everything the same
+ // as it was.
+ break;
+ case Goal::READY_LOWER:
+ case Goal::INTAKE:
+ {
+ Time now = Time::Now();
+ if (position) {
+ // Posedge of the disc entering the beam break.
+ if (position->bottom_disc_posedge_count !=
+ last_bottom_disc_posedge_count_) {
+ transfer_frisbee_.Reset();
+ transfer_frisbee_.bottom_posedge_time_ = now;
+ LOG(INFO, "Posedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_posedge_time_.ToSeconds());
+ ++hopper_disc_count_;
+ ++total_disc_count_;
+ }
+
+ // Disc exited the beam break now.
+ if (position->bottom_disc_negedge_count !=
+ last_bottom_disc_negedge_count_) {
+ transfer_frisbee_.bottom_negedge_time_ = now;
+ LOG(INFO, "Negedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_negedge_time_.ToSeconds());
+ frisbees_.push_front(transfer_frisbee_);
+ }
+
+ if (position->bottom_disc_detect) {
+ intake_voltage = transfer_voltage = 12.0;
+ // Must wait until the disc gets out before we can change state.
+ safe_to_change_state_ = false;
+
+ // TODO(aschuh): A disc on the way through needs to start moving
+ // the indexer if it isn't already moving. Maybe?
+
+ Time elapsed_posedge_time = now -
+ transfer_frisbee_.bottom_posedge_time_;
+ if (elapsed_posedge_time >= Time::InSeconds(0.3)) {
+ // It has been too long. The disc must be jammed.
+ LOG(ERROR, "Been way too long. Jammed disc?\n");
+ printf("Been way too long. Jammed disc?\n");
+ }
+ }
+
+ // Check all non-indexed discs and see if they should be indexed.
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ if (!frisbee->has_been_indexed_) {
+ intake_voltage = transfer_voltage = 12.0;
+
+ if (last_bottom_disc_negedge_wait_count_ !=
+ position->bottom_disc_negedge_wait_count) {
+ // We have an index difference.
+ // Save the indexer position, and the time.
+ if (last_bottom_disc_negedge_wait_count_ + 1 !=
+ position->bottom_disc_negedge_wait_count) {
+ LOG(ERROR, "Funny, we got 2 edges since we last checked.\n");
+ }
+
+ // Save the captured position as the position at which the disc
+ // touched the indexer.
+ LOG(INFO, "Grabbed on the index now at %f\n", index_position);
+ printf("Grabbed on the index now at %f\n", index_position);
+ frisbee->has_been_indexed_ = true;
+ frisbee->index_start_position_ =
+ position->bottom_disc_negedge_wait_position;
+ }
+ }
+ if (!frisbee->has_been_indexed_) {
+ // All discs must be indexed before it is safe to stop indexing.
+ safe_to_change_state_ = false;
+ }
+ }
+
+ // Figure out where the indexer should be to move the discs down to
+ // the right position.
+ double max_disc_position = 0;
+ if (MaxDiscPosition(&max_disc_position, NULL)) {
+ LOG(DEBUG, "There is a disc down here!\n");
+ // TODO(aschuh): Figure out what to do if grabbing the next one
+ // would cause things to jam into the loader.
+ // Say we aren't ready any more. Undefined behavior will result if
+ // that isn't observed.
+ double bottom_disc_position =
+ max_disc_position + ConvertDiscAngleToIndex(M_PI);
+ wrist_loop_->R << bottom_disc_position, 0.0;
+
+ // Verify that we are close enough to the goal so that we should be
+ // fine accepting the next disc.
+ double disc_error_meters = ConvertIndexToDiscPosition(
+ wrist_loop_->X_hat(0, 0) - bottom_disc_position);
+ // We are ready for the next disc if the first one is in the first
+ // half circle of the indexer. It will take time for the disc to
+ // come into the indexer, so we will be able to move it out of the
+ // way in time.
+ // This choice also makes sure that we don't claim that we aren't
+ // ready between full speed intaking.
+ if (-ConvertDiscAngleToIndex(M_PI) < disc_error_meters &&
+ disc_error_meters < 0.04) {
+ // We are only ready if we aren't being asked to change state or
+ // are full.
+ status->ready_to_intake =
+ (safe_goal_ == goal_enum) && hopper_disc_count_ < 4;
+ } else {
+ status->ready_to_intake = false;
+ }
+ } else {
+ // No discs! We are always ready for more if we aren't being
+ // asked to change state.
+ status->ready_to_intake = (safe_goal_ == goal_enum);
+ printf("Ready to intake, zero discs. %d %d %d\n",
+ status->ready_to_intake, hopper_disc_count_, safe_goal_);
+ }
+
+ // Turn on the transfer roller if we are ready.
+ if (status->ready_to_intake && hopper_disc_count_ < 4 &&
+ safe_goal_ == Goal::INTAKE) {
+ intake_voltage = transfer_voltage = 12.0;
+ }
+ }
+ LOG(DEBUG, "INTAKE\n");
+ }
+ break;
+ case Goal::READY_SHOOTER:
+ case Goal::SHOOT:
+ // Check if we have any discs to shoot or load and handle them.
+ double min_disc_position = 0;
+ if (MinDiscPosition(&min_disc_position, NULL)) {
+ const double ready_disc_position = min_disc_position +
+ ConvertDiscPositionToIndex(kReadyToPreload - kIndexStartPosition);
+
+ const double grabbed_disc_position =
+ min_disc_position +
+ ConvertDiscPositionToIndex(kReadyToLiftPosition -
+ kIndexStartPosition + 0.03);
+
+ // Check the state of the loader FSM.
+ // If it is ready to load discs, position the disc so that it is ready
+ // to be grabbed.
+ // If it isn't ready, there is a disc in there. It needs to finish it's
+ // cycle first.
+ if (loader_state_ != LoaderState::READY) {
+ // We already have a disc in the loader.
+ // Stage the discs back a bit.
+ wrist_loop_->R << ready_disc_position, 0.0;
+ printf("Loader not ready but asked to shoot\n");
+
+ // Shoot if we are grabbed and being asked to shoot.
+ if (loader_state_ == LoaderState::GRABBED &&
+ safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ }
+
+ // Must wait until it has been grabbed to continue.
+ if (loader_state_ == LoaderState::GRABBING) {
+ safe_to_change_state_ = false;
+ }
+ } else {
+ // No disc up top right now.
+ wrist_loop_->R << grabbed_disc_position, 0.0;
+
+ // See if the disc has gotten pretty far up yet.
+ if (wrist_loop_->X_hat(0, 0) > ready_disc_position) {
+ // Point of no return. We are committing to grabbing it now.
+ safe_to_change_state_ = false;
+ const double robust_grabbed_disc_position =
+ (grabbed_disc_position -
+ ConvertDiscPositionToIndex(kGrabberLength));
+
+ // If close, start grabbing and/or shooting.
+ if (wrist_loop_->X_hat(0, 0) > robust_grabbed_disc_position) {
+ // Start the state machine.
+ if (safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ } else {
+ loader_goal_ = LoaderGoal::GRAB;
+ }
+ // This frisbee is now gone. Take it out of the queue.
+ frisbees_.pop_back();
+ }
+ }
+ }
+ } else {
+ if (loader_state_ != LoaderState::READY) {
+ // Shoot if we are grabbed and being asked to shoot.
+ if (loader_state_ == LoaderState::GRABBED &&
+ safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ }
+ } else {
+ // Ok, no discs in sight. Spin the hopper up by 150% of it's full
+ // range and verify that we don't see anything.
+ printf("Moving the indexer to verify that it is clear\n");
+ const double hopper_clear_verification_position =
+ ::std::max(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 2.5;
+
+ wrist_loop_->R << hopper_clear_verification_position, 0.0;
+ if (::std::abs(wrist_loop_->X_hat(0, 0) -
+ hopper_clear_verification_position) <
+ ConvertDiscPositionToIndex(0.05)) {
+ printf("Should be empty\n");
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ != 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still discs there\n");
+ }
+ }
+ }
+ }
+
+ {
+ const double hopper_clear_verification_position =
+ ::std::max(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 2.5;
+
+ if (wrist_loop_->X_hat(0, 0) >
+ hopper_clear_verification_position +
+ ConvertDiscPositionToIndex(0.05)) {
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ LOG(ERROR, "Upper is [%f %f]\n",
+ upper_open_region_.upper_bound(),
+ upper_open_region_.lower_bound());
+ LOG(ERROR, "Lower is [%f %f]\n",
+ lower_open_region_.upper_bound(),
+ lower_open_region_.lower_bound());
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ != 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still discs there\n");
+ }
+ }
+ }
+
+ LOG(DEBUG, "READY_SHOOTER or SHOOT\n");
+ break;
+ }
+
+ // The only way out of the loader is to shoot the disc. The FSM can only go
+ // forwards.
+ switch (loader_state_) {
+ case LoaderState::READY:
+ LOG(DEBUG, "Loader READY\n");
+ // Open and down, ready to accept a disc.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = false;
+ if (loader_goal_ == LoaderGoal::GRAB ||
+ loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+ if (loader_goal_ == LoaderGoal::GRAB) {
+ LOG(INFO, "Told to GRAB, moving on\n");
+ } else {
+ LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
+ }
+ loader_state_ = LoaderState::GRABBING;
+ loader_countdown_ = kGrabbingDelay;
+ } else {
+ break;
+ }
+ case LoaderState::GRABBING:
+ LOG(DEBUG, "Loader GRABBING %d\n", loader_countdown_);
+ // Closing the grabber.
+ loader_up_ = false;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::GRABBED;
+ }
+ case LoaderState::GRABBED:
+ LOG(DEBUG, "Loader GRABBED\n");
+ // Grabber closed.
+ loader_up_ = false;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+ shooter.status.FetchLatest();
+ if (shooter.status.get()) {
+ // TODO(aschuh): If we aren't shooting nicely, wait until the shooter
+ // is up to speed rather than just spinning.
+ if (shooter.status->average_velocity > 130) {
+ loader_state_ = LoaderState::LIFTING;
+ loader_countdown_ = kLiftingDelay;
+ LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
+ } else {
+ LOG(WARNING, "Told to SHOOT_AND_RESET, shooter too slow at %f\n",
+ shooter.status->average_velocity);
+ break;
+ }
+ } else {
+ LOG(ERROR, "Told to SHOOT_AND_RESET, no shooter data, moving on.\n");
+ loader_state_ = LoaderState::LIFTING;
+ loader_countdown_ = kLiftingDelay;
+ }
+ } else if (loader_goal_ == LoaderGoal::READY) {
+ LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
+ printf("Can't go to ready when we have something grabbed.\n");
+ break;
+ } else {
+ break;
+ }
+ case LoaderState::LIFTING:
+ LOG(DEBUG, "Loader LIFTING %d\n", loader_countdown_);
+ // Lifting the disc.
+ loader_up_ = true;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::LIFTED;
+ }
+ case LoaderState::LIFTED:
+ LOG(DEBUG, "Loader LIFTED\n");
+ // Disc lifted. Time to eject it out.
+ loader_up_ = true;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ loader_state_ = LoaderState::SHOOTING;
+ loader_countdown_ = kShootingDelay;
+ case LoaderState::SHOOTING:
+ LOG(DEBUG, "Loader SHOOTING %d\n", loader_countdown_);
+ // Ejecting the disc into the shooter.
+ loader_up_ = true;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::SHOOT;
+ }
+ case LoaderState::SHOOT:
+ LOG(DEBUG, "Loader SHOOT\n");
+ // The disc has been shot.
+ loader_up_ = true;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ loader_state_ = LoaderState::LOWERING;
+ loader_countdown_ = kLoweringDelay;
+ --hopper_disc_count_;
+ ++shot_disc_count_;
+ case LoaderState::LOWERING:
+ LOG(DEBUG, "Loader LOWERING %d\n", loader_countdown_);
+ // Lowering the loader back down.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::LOWERED;
+ }
+ case LoaderState::LOWERED:
+ LOG(DEBUG, "Loader LOWERED\n");
+ // The indexer is lowered.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = false;
+ loader_state_ = LoaderState::READY;
+ // Once we have shot, we need to hang out in READY until otherwise
+ // notified.
+ loader_goal_ = LoaderGoal::READY;
+ break;
+ }
+
+ // Update the observer.
+ wrist_loop_->Update(position != NULL, output == NULL);
+
+ if (position) {
+ LOG(DEBUG, "pos=%f\n", position->index_position);
+ last_bottom_disc_detect_ = position->bottom_disc_detect;
+ last_top_disc_detect_ = position->top_disc_detect;
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ }
+
+ status->hopper_disc_count = hopper_disc_count_;
+ status->total_disc_count = total_disc_count_;
+ status->shot_disc_count = shot_disc_count_;
+ status->preloaded = (loader_state_ != LoaderState::READY);
+
+ if (output) {
+ output->intake_voltage = intake_voltage;
+ output->transfer_voltage = transfer_voltage;
+ output->index_voltage = wrist_loop_->U(0, 0);
+ output->loader_up = loader_up_;
+ output->disc_clamped = disc_clamped_;
+ output->disc_ejected = disc_ejected_;
+ }
+
+ if (safe_to_change_state_) {
+ safe_goal_ = goal_enum;
+ }
+ if (hopper_disc_count_ < 0) {
+ LOG(ERROR, "NEGATIVE DISCS. VERY VERY BAD\n");
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index.gyp b/frc971/control_loops/index/index.gyp
new file mode 100644
index 0000000..f61da8c
--- /dev/null
+++ b/frc971/control_loops/index/index.gyp
@@ -0,0 +1,73 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'index_loop',
+ 'type': 'static_library',
+ 'sources': ['index_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/index',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'index_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'index.cc',
+ 'index_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'index_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'index_loop',
+ ],
+ },
+ {
+ 'target_name': 'index_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'index_lib_test.cc',
+ 'transfer_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'index_loop',
+ 'index_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'index',
+ 'type': 'executable',
+ 'sources': [
+ 'index_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'index_lib',
+ 'index_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
new file mode 100644
index 0000000..c9dc86f
--- /dev/null
+++ b/frc971/control_loops/index/index.h
@@ -0,0 +1,342 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_H_
+#define FRC971_CONTROL_LOOPS_WRIST_H_
+
+#include <deque>
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class IndexTest_InvalidStateTest_Test;
+class IndexTest_LostDisc_Test;
+}
+
+// This class represents a region of space.
+class Region {
+ public:
+ Region () : upper_bound_(0.0), lower_bound_(0.0) {}
+
+ // Restarts the region tracking by starting over with a 0 width region with
+ // the bounds at [edge, edge].
+ void Restart(double edge) {
+ upper_bound_ = edge;
+ lower_bound_ = edge;
+ }
+
+ // Expands the region to include the new point.
+ void Expand(double new_point) {
+ if (new_point > upper_bound_) {
+ upper_bound_ = new_point;
+ } else if (new_point < lower_bound_) {
+ lower_bound_ = new_point;
+ }
+ }
+
+ // Returns the width of the region.
+ double width() const { return upper_bound_ - lower_bound_; }
+ // Returns the upper and lower bounds.
+ double upper_bound() const { return upper_bound_; }
+ double lower_bound() const { return lower_bound_; }
+
+ private:
+ // Upper bound of the region.
+ double upper_bound_;
+ // Lower bound of the region.
+ double lower_bound_;
+};
+
+class IndexMotor
+ : public aos::control_loops::ControlLoop<control_loops::IndexLoop> {
+ public:
+ explicit IndexMotor(
+ control_loops::IndexLoop *my_index = &control_loops::index_loop);
+
+ static const double kTransferStartPosition;
+ static const double kIndexStartPosition;
+ // The distance from where the disc first grabs on the indexer to where it
+ // just bairly clears the loader.
+ static const double kIndexFreeLength;
+ // The distance to where the disc just starts to enter the loader.
+ static const double kLoaderFreeStopPosition;
+ // The distance to where the next disc gets positioned while the current disc
+ // is shooting.
+ static const double kReadyToPreload;
+
+ // Distance that the grabber pulls the disc in by.
+ static const double kGrabberLength;
+ // Distance to where the grabber takes over.
+ static const double kGrabberStartPosition;
+
+ // The distance to where the disc hits the back of the loader and is ready to
+ // lift.
+ static const double kReadyToLiftPosition;
+
+ static const double kGrabberMovementVelocity;
+ // TODO(aschuh): This depends on the shooter angle...
+ // Distance to where the shooter is up and ready to shoot.
+ static const double kLifterStopPosition;
+ static const double kLifterMovementVelocity;
+
+ // Distance to where the disc has been launched.
+ // TODO(aschuh): This depends on the shooter angle...
+ static const double kEjectorStopPosition;
+ static const double kEjectorMovementVelocity;
+
+ // Start and stop position of the bottom disc detect sensor in meters.
+ static const double kBottomDiscDetectStart;
+ static const double kBottomDiscDetectStop;
+ // Delay between the negedge of the disc detect and when it engages on the
+ // indexer.
+ static const double kBottomDiscIndexDelay;
+
+ static const double kTopDiscDetectStart;
+ static const double kTopDiscDetectStop;
+
+ // Minimum distance between 2 frisbees as seen by the top disc detect sensor.
+ static const double kTopDiscDetectMinSeperation;
+
+ // Converts the angle of the indexer to the angle of the disc.
+ static double ConvertIndexToDiscAngle(const double angle);
+ // Converts the angle of the indexer to the position that the center of the
+ // disc has traveled.
+ static double ConvertIndexToDiscPosition(const double angle);
+
+ // Converts the angle of the transfer roller to the position that the center
+ // of the disc has traveled.
+ static double ConvertTransferToDiscPosition(const double angle);
+
+ // Converts the distance around the indexer to the position of
+ // the index roller.
+ static double ConvertDiscPositionToIndex(const double position);
+ // Converts the angle around the indexer to the position of the index roller.
+ static double ConvertDiscAngleToIndex(const double angle);
+ // Converts the angle around the indexer to the position of the disc in the
+ // indexer.
+ static double ConvertDiscAngleToDiscPosition(const double angle);
+ // Converts the distance around the indexer to the angle of the disc around
+ // the indexer.
+ static double ConvertDiscPositionToDiscAngle(const double position);
+
+ // Disc radius in meters.
+ static const double kDiscRadius;
+ // Roller radius in meters.
+ static const double kRollerRadius;
+ // Transfer roller radius in meters.
+ static const double kTransferRollerRadius;
+
+ // Time that it takes to grab the disc in cycles.
+ static const int kGrabbingDelay;
+ // Time that it takes to lift the loader in cycles.
+ static const int kLiftingDelay;
+ // Time that it takes to shoot the disc in cycles.
+ static const int kShootingDelay;
+ // Time that it takes to lower the loader in cycles.
+ static const int kLoweringDelay;
+
+ // Object representing a Frisbee tracked by the indexer.
+ class Frisbee {
+ public:
+ Frisbee()
+ : bottom_posedge_time_(0, 0),
+ bottom_negedge_time_(0, 0) {
+ Reset();
+ }
+
+ // Resets a Frisbee so it can be reused.
+ void Reset() {
+ bottom_posedge_time_ = ::aos::time::Time(0, 0);
+ bottom_negedge_time_ = ::aos::time::Time(0, 0);
+ has_been_indexed_ = false;
+ index_start_position_ = 0.0;
+ }
+
+ // Returns true if the position is valid.
+ bool has_position() const {
+ return has_been_indexed_;
+ }
+
+ // Returns the most up to date and accurate position that we have for the
+ // disc. This is the indexer position that the disc grabbed at.
+ double position() const {
+ return index_start_position_;
+ }
+
+ // Returns the absolute position of the disc in meters in the hopper given
+ // that the indexer is at the provided position.
+ double absolute_position(const double index_position) const {
+ return IndexMotor::ConvertIndexToDiscPosition(
+ index_position - index_start_position_) +
+ IndexMotor::kIndexStartPosition;
+ }
+
+ // Shifts the disc down the indexer by the provided offset. This is to
+ // handle when the cRIO reboots.
+ void OffsetDisc(double offset) {
+ index_start_position_ += offset;
+ }
+
+ // Potentially offsets the position with the knowledge that no discs are
+ // currently blocking the top sensor. This knowledge can be used to move
+ // this disc if it is believed to be blocking the top sensor.
+ // Returns the amount that the disc moved due to this observation.
+ double ObserveNoTopDiscSensor(double index_position, double index_velocity);
+
+ // Posedge and negedge disc times.
+ ::aos::time::Time bottom_posedge_time_;
+ ::aos::time::Time bottom_negedge_time_;
+
+ // True if the disc has a valid index position.
+ bool has_been_indexed_;
+ // Location of the index when the disc first contacted it.
+ double index_start_position_;
+ };
+
+ // Returns where the indexer thinks the frisbees are.
+ const ::std::deque<Frisbee> &frisbees() const { return frisbees_; }
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status);
+
+ private:
+ friend class testing::IndexTest_InvalidStateTest_Test;
+ friend class testing::IndexTest_LostDisc_Test;
+
+ // This class implements the CapU function correctly given all the extra
+ // information that we know about from the wrist motor.
+ class IndexStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ IndexStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ low_voltage_count_(0) {
+ }
+
+ // Voltage below which the indexer won't move with a disc in it.
+ static const double kMinMotionVoltage;
+ // Maximum number of cycles to apply a low voltage to the motor.
+ static const double kNoMotionCuttoffCount;
+
+ // Caps U, but disables the motor after a number of cycles of inactivity.
+ virtual void CapU();
+ private:
+ // Number of cycles that we have seen a small voltage being applied.
+ uint32_t low_voltage_count_;
+ };
+
+ // Sets disc_position to the minimum or maximum disc position.
+ // Sets found_disc to point to the frisbee that was found, and ignores it if
+ // found_disc is NULL.
+ // Returns true if there were discs, and false if there weren't.
+ // On false, disc_position is left unmodified.
+ bool MinDiscPosition(double *disc_position, Frisbee **found_disc);
+ bool MaxDiscPosition(double *disc_position, Frisbee **found_disc);
+
+ // The state feedback control loop to talk to for the index.
+ ::std::unique_ptr<IndexStateFeedbackLoop> wrist_loop_;
+
+ // Count of the number of discs that we have collected.
+ int32_t hopper_disc_count_;
+ int32_t total_disc_count_;
+ int32_t shot_disc_count_;
+
+ enum class Goal {
+ // Hold position, in a low power state.
+ HOLD = 0,
+ // Get ready to load discs by shifting the discs down.
+ READY_LOWER = 1,
+ // Ready the discs, spin up the transfer roller, and accept discs.
+ INTAKE = 2,
+ // Get ready to shoot, and place a disc in the loader.
+ READY_SHOOTER = 3,
+ // Shoot at will.
+ SHOOT = 4
+ };
+
+ // These two enums command and track the loader loading discs into the
+ // shooter.
+ enum class LoaderState {
+ // Open and down, ready to accept a disc.
+ READY,
+ // Closing the grabber.
+ GRABBING,
+ // Grabber closed.
+ GRABBED,
+ // Lifting the disc.
+ LIFTING,
+ // Disc lifted.
+ LIFTED,
+ // Ejecting the disc into the shooter.
+ SHOOTING,
+ // The disc has been shot.
+ SHOOT,
+ // Lowering the loader back down.
+ LOWERING,
+ // The indexer is lowered.
+ LOWERED
+ };
+
+ // TODO(aschuh): If we are grabbed and asked to be ready, now what?
+ // LOG ?
+ enum class LoaderGoal {
+ // Get the loader ready to accept another disc.
+ READY,
+ // Grab a disc now.
+ GRAB,
+ // Lift it up, shoot, and reset.
+ // Waits to shoot until the shooter is stable.
+ // Resets the goal to READY once one disc has been shot.
+ SHOOT_AND_RESET
+ };
+
+ // The current goal
+ Goal safe_goal_;
+
+ // Loader goal, state, and counter.
+ LoaderGoal loader_goal_;
+ LoaderState loader_state_;
+ int loader_countdown_;
+
+ // Current state of the pistons.
+ bool loader_up_;
+ bool disc_clamped_;
+ bool disc_ejected_;
+
+ // The frisbee that is flying through the transfer rollers.
+ Frisbee transfer_frisbee_;
+
+ // Bottom disc detect from the last valid packet for detecting edges.
+ bool last_bottom_disc_detect_;
+ bool last_top_disc_detect_;
+ int32_t last_bottom_disc_posedge_count_;
+ int32_t last_bottom_disc_negedge_count_;
+ int32_t last_bottom_disc_negedge_wait_count_;
+ int32_t last_top_disc_posedge_count_;
+ int32_t last_top_disc_negedge_count_;
+
+ // Frisbees are in order such that the newest frisbee is on the front.
+ ::std::deque<Frisbee> frisbees_;
+
+ // True if we haven't seen a position before.
+ bool no_prior_position_;
+ // Number of position messages that we have missed in a row.
+ uint32_t missing_position_count_;
+
+ // The no-disc regions for both the bottom and top beam break sensors.
+ Region upper_open_region_;
+ Region lower_open_region_;
+
+ DISALLOW_COPY_AND_ASSIGN(IndexMotor);
+};
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_H_
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
new file mode 100644
index 0000000..43c596c
--- /dev/null
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -0,0 +1,1308 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/index/index.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+#include "frc971/control_loops/index/transfer_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+class Frisbee {
+ public:
+ // Creates a frisbee starting at the specified position in the frisbee path,
+ // and with the transfer and index rollers at the specified positions.
+ Frisbee(double transfer_roller_position,
+ double index_roller_position,
+ double position = IndexMotor::kBottomDiscDetectStart - 0.001)
+ : transfer_roller_position_(transfer_roller_position),
+ index_roller_position_(index_roller_position),
+ position_(position),
+ has_been_shot_(false),
+ has_bottom_disc_negedge_wait_position_(false),
+ bottom_disc_negedge_wait_position_(0.0),
+ after_negedge_time_left_(IndexMotor::kBottomDiscIndexDelay),
+ counted_negedge_wait_(false),
+ has_top_disc_posedge_position_(false),
+ top_disc_posedge_position_(0.0),
+ has_top_disc_negedge_position_(false),
+ top_disc_negedge_position_(0.0) {
+ }
+
+ // Returns true if the frisbee is controlled by the transfer roller.
+ bool IsTouchingTransfer(double position) const {
+ return (position >= IndexMotor::kBottomDiscDetectStart &&
+ position <= IndexMotor::kIndexStartPosition);
+ }
+ bool IsTouchingTransfer() const { return IsTouchingTransfer(position_); }
+
+ // Returns true if the frisbee is in a place where it is unsafe to grab.
+ bool IsUnsafeToGrab() const {
+ return (position_ > (IndexMotor::kLoaderFreeStopPosition) &&
+ position_ < IndexMotor::kGrabberStartPosition);
+ }
+
+ // Returns true if the frisbee is controlled by the indexing roller.
+ bool IsTouchingIndex(double position) const {
+ return (position >= IndexMotor::kIndexStartPosition &&
+ position < IndexMotor::kGrabberStartPosition);
+ }
+ bool IsTouchingIndex() const { return IsTouchingIndex(position_); }
+
+ // Returns true if the frisbee is in a position such that the disc can be
+ // lifted.
+ bool IsUnsafeToLift() const {
+ return (position_ >= IndexMotor::kLoaderFreeStopPosition &&
+ position_ <= IndexMotor::kReadyToLiftPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the grabber will
+ // pull it into the loader.
+ bool IsTouchingGrabber() const {
+ return (position_ >= IndexMotor::kGrabberStartPosition &&
+ position_ < IndexMotor::kReadyToLiftPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the disc can be
+ // lifted.
+ bool IsTouchingLoader() const {
+ return (position_ >= IndexMotor::kReadyToLiftPosition &&
+ position_ < IndexMotor::kLifterStopPosition);
+ }
+
+ // Returns true if the frisbee is touching the ejector.
+ bool IsTouchingEjector() const {
+ return (position_ >= IndexMotor::kLifterStopPosition &&
+ position_ < IndexMotor::kEjectorStopPosition);
+ }
+
+ // Returns true if the disc is triggering the bottom disc detect sensor.
+ bool bottom_disc_detect(double position) const {
+ return (position >= IndexMotor::kBottomDiscDetectStart &&
+ position <= IndexMotor::kBottomDiscDetectStop);
+ }
+ bool bottom_disc_detect() const { return bottom_disc_detect(position_); }
+
+ // Returns true if the disc is triggering the top disc detect sensor.
+ bool top_disc_detect(double position) const {
+ return (position >= IndexMotor::kTopDiscDetectStart &&
+ position <= IndexMotor::kTopDiscDetectStop);
+ }
+ bool top_disc_detect() const { return top_disc_detect(position_); }
+
+ // Returns true if the bottom disc sensor will negedge after the disc moves
+ // by dx.
+ bool will_negedge_bottom_disc_detect(double disc_dx) {
+ if (bottom_disc_detect()) {
+ return !bottom_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Returns true if the bottom disc sensor will posedge after the disc moves
+ // by dx.
+ bool will_posedge_top_disc_detect(double disc_dx) {
+ if (!top_disc_detect()) {
+ return top_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Returns true if the bottom disc sensor will negedge after the disc moves
+ // by dx.
+ bool will_negedge_top_disc_detect(double disc_dx) {
+ if (top_disc_detect()) {
+ return !top_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Handles potentially dealing with the delayed negedge.
+ // Computes the index position when time expires using the cached old indexer
+ // position, the elapsed time, and the average velocity.
+ void HandleAfterNegedge(
+ double index_velocity, double elapsed_time, double time_left) {
+ if (!has_bottom_disc_negedge_wait_position_) {
+ if (after_negedge_time_left_ < time_left) {
+ // Assume constant velocity and compute the position.
+ bottom_disc_negedge_wait_position_ =
+ index_roller_position_ +
+ index_velocity * (elapsed_time + after_negedge_time_left_);
+ after_negedge_time_left_ = 0.0;
+ has_bottom_disc_negedge_wait_position_ = true;
+ } else {
+ after_negedge_time_left_ -= time_left;
+ }
+ }
+ }
+
+ // Updates the position of the disc assuming that it has started on the
+ // transfer. The elapsed time is the simulated amount of time that has
+ // elapsed since the simulation timestep started and this method was called.
+ // time_left is the amount of time left to spend during this timestep.
+ double UpdateTransferPositionForTime(double transfer_roller_velocity,
+ double index_roller_velocity,
+ double elapsed_time,
+ double time_left) {
+ double disc_dx = IndexMotor::ConvertTransferToDiscPosition(
+ transfer_roller_velocity * time_left);
+ bool shrunk_time = false;
+ if (!IsTouchingTransfer(position_ + disc_dx)) {
+ shrunk_time = true;
+ time_left = (IndexMotor::kIndexStartPosition - position_) /
+ transfer_roller_velocity;
+ disc_dx = IndexMotor::ConvertTransferToDiscPosition(
+ transfer_roller_velocity * time_left);
+ }
+
+ if (will_negedge_bottom_disc_detect(disc_dx)) {
+ // Compute the time from the negedge to the end of the cycle assuming
+ // constant velocity.
+ const double elapsed_time =
+ (position_ + disc_dx - IndexMotor::kBottomDiscDetectStop) /
+ disc_dx * time_left;
+
+ // I am not implementing very short delays until this fails.
+ assert(elapsed_time <= after_negedge_time_left_);
+ after_negedge_time_left_ -= elapsed_time;
+ } else if (position_ >= IndexMotor::kBottomDiscDetectStop) {
+ HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
+ }
+
+ if (shrunk_time) {
+ EXPECT_LT(0, transfer_roller_velocity);
+ position_ = IndexMotor::kIndexStartPosition;
+ } else {
+ position_ += disc_dx;
+ }
+ printf("Transfer Roller: Disc is at %f\n", position_);
+ return time_left;
+ }
+
+ // Updates the position of the disc assuming that it has started on the
+ // indexer. The elapsed time is the simulated amount of time that has
+ // elapsed since the simulation timestep started and this method was called.
+ // time_left is the amount of time left to spend during this timestep.
+ double UpdateIndexPositionForTime(double index_roller_velocity,
+ double elapsed_time,
+ double time_left) {
+ double index_dx = IndexMotor::ConvertIndexToDiscPosition(
+ index_roller_velocity * time_left);
+ bool shrunk_time = false;
+ if (!IsTouchingIndex(position_ + index_dx)) {
+ shrunk_time = true;
+ time_left = (IndexMotor::kGrabberStartPosition - position_) /
+ index_roller_velocity;
+ index_dx = IndexMotor::ConvertTransferToDiscPosition(
+ index_roller_velocity * time_left);
+ }
+
+ if (position_ >= IndexMotor::kBottomDiscDetectStop) {
+ HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
+ }
+
+ if (will_posedge_top_disc_detect(index_dx)) {
+ // Wohoo! Find the edge.
+ // Assume constant velocity and compute the position.
+ double edge_position = index_roller_velocity > 0 ?
+ IndexMotor::kTopDiscDetectStart : IndexMotor::kTopDiscDetectStop;
+ const double disc_time =
+ (edge_position - position_) / index_roller_velocity;
+ top_disc_posedge_position_ = index_roller_position_ +
+ IndexMotor::ConvertDiscPositionToIndex(
+ index_roller_velocity * (elapsed_time + disc_time));
+ has_top_disc_posedge_position_ = true;
+ printf("Posedge on top sensor at %f\n", top_disc_posedge_position_);
+ }
+
+ if (will_negedge_top_disc_detect(index_dx)) {
+ // Wohoo! Find the edge.
+ // Assume constant velocity and compute the position.
+ double edge_position = index_roller_velocity > 0 ?
+ IndexMotor::kTopDiscDetectStop : IndexMotor::kTopDiscDetectStart;
+ const double disc_time =
+ (edge_position - position_) / index_roller_velocity;
+ top_disc_negedge_position_ = index_roller_position_ +
+ IndexMotor::ConvertDiscPositionToIndex(
+ index_roller_velocity * (elapsed_time + disc_time));
+ has_top_disc_negedge_position_ = true;
+ printf("Negedge on top sensor at %f\n", top_disc_negedge_position_);
+ }
+
+ if (shrunk_time) {
+ if (index_roller_velocity > 0) {
+ position_ = IndexMotor::kGrabberStartPosition;
+ } else {
+ position_ = IndexMotor::kIndexStartPosition;
+ }
+ } else {
+ position_ += index_dx;
+ }
+ printf("Index: Disc is at %f\n", position_);
+ return time_left;
+ }
+
+ // Updates the position given velocities, piston comands, and the time left in
+ // the simulation cycle.
+ void UpdatePositionForTime(double transfer_roller_velocity,
+ double index_roller_velocity,
+ bool clamped,
+ bool lifted,
+ bool ejected,
+ double time_left) {
+ double elapsed_time = 0.0;
+ // We are making this assumption below
+ ASSERT_LE(IndexMotor::kBottomDiscDetectStop,
+ IndexMotor::kIndexStartPosition);
+ if (IsTouchingTransfer() || position() < 0.0) {
+ double deltat = UpdateTransferPositionForTime(
+ transfer_roller_velocity, index_roller_velocity,
+ elapsed_time, time_left);
+ time_left -= deltat;
+ elapsed_time += deltat;
+ }
+
+ if (IsTouchingIndex() && time_left >= 0) {
+ // Verify that we aren't trying to grab or lift when it isn't safe.
+ EXPECT_FALSE(clamped && IsUnsafeToGrab());
+ EXPECT_FALSE(lifted && IsUnsafeToLift());
+
+ double deltat = UpdateIndexPositionForTime(
+ index_roller_velocity, elapsed_time, time_left);
+ time_left -= deltat;
+ elapsed_time += deltat;
+ }
+ if (IsTouchingGrabber()) {
+ if (clamped) {
+ const double grabber_dx =
+ IndexMotor::kGrabberMovementVelocity * time_left;
+ position_ = ::std::min(position_ + grabber_dx,
+ IndexMotor::kReadyToLiftPosition);
+ }
+ EXPECT_FALSE(lifted) << "Can't lift while in grabber";
+ EXPECT_FALSE(ejected) << "Can't eject while in grabber";
+ printf("Grabber: Disc is at %f\n", position_);
+ } else if (IsTouchingLoader()) {
+ if (lifted) {
+ const double lifter_dx =
+ IndexMotor::kLifterMovementVelocity * time_left;
+ position_ = ::std::min(position_ + lifter_dx,
+ IndexMotor::kLifterStopPosition);
+ }
+ EXPECT_TRUE(clamped);
+ EXPECT_FALSE(ejected);
+ printf("Loader: Disc is at %f\n", position_);
+ } else if (IsTouchingEjector()) {
+ EXPECT_TRUE(lifted);
+ if (ejected) {
+ const double ejector_dx =
+ IndexMotor::kEjectorMovementVelocity * time_left;
+ position_ = ::std::min(position_ + ejector_dx,
+ IndexMotor::kEjectorStopPosition);
+ EXPECT_FALSE(clamped);
+ }
+ printf("Ejector: Disc is at %f\n", position_);
+ } else if (position_ == IndexMotor::kEjectorStopPosition) {
+ printf("Shot: Disc is at %f\n", position_);
+ has_been_shot_ = true;
+ }
+ }
+
+ // Updates the position of the frisbee in the frisbee path.
+ void UpdatePosition(double transfer_roller_position,
+ double index_roller_position,
+ bool clamped,
+ bool lifted,
+ bool ejected) {
+ const double transfer_roller_velocity =
+ (transfer_roller_position - transfer_roller_position_) / 0.01;
+ const double index_roller_velocity =
+ (index_roller_position - index_roller_position_) / 0.01;
+ UpdatePositionForTime(transfer_roller_velocity,
+ index_roller_velocity,
+ clamped,
+ lifted,
+ ejected,
+ 0.01);
+ transfer_roller_position_ = transfer_roller_position;
+ index_roller_position_ = index_roller_position;
+ }
+
+ // Returns if the disc has been shot and can be removed from the robot.
+ bool has_been_shot() const {
+ return has_been_shot_;
+ }
+
+ // Returns the position of the disc in the system.
+ double position() const {
+ return position_;
+ }
+
+ // Sets whether or not we have counted the delayed negedge.
+ void set_counted_negedge_wait(bool counted_negedge_wait) {
+ counted_negedge_wait_ = counted_negedge_wait;
+ }
+
+ // Returns if we have counted the delayed negedge.
+ bool counted_negedge_wait() { return counted_negedge_wait_; }
+
+ // Returns true if the negedge wait position is valid.
+ bool has_bottom_disc_negedge_wait_position() {
+ return has_bottom_disc_negedge_wait_position_;
+ }
+
+ // Returns the negedge wait position.
+ double bottom_disc_negedge_wait_position() {
+ return bottom_disc_negedge_wait_position_;
+ }
+
+ // Returns the last position where a posedge was seen.
+ double top_disc_posedge_position() { return top_disc_posedge_position_; }
+
+ // Returns the last position where a negedge was seen.
+ double top_disc_negedge_position() { return top_disc_negedge_position_; }
+
+ // True if the top disc has seen a posedge.
+ // Reading this flag clears it.
+ bool has_top_disc_posedge_position() {
+ bool prev = has_top_disc_posedge_position_;
+ has_top_disc_posedge_position_ = false;
+ return prev;
+ }
+
+ // True if the top disc has seen a negedge.
+ // Reading this flag clears it.
+ bool has_top_disc_negedge_position() {
+ bool prev = has_top_disc_negedge_position_;
+ has_top_disc_negedge_position_ = false;
+ return prev;
+ }
+
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndex(double offset) {
+ index_roller_position_ += offset;
+ }
+
+ private:
+ // Previous transfer roller position for computing deltas.
+ double transfer_roller_position_;
+ // Previous index roller position for computing deltas.
+ double index_roller_position_;
+ // Position in the robot.
+ double position_;
+ // True if the disc has been shot.
+ bool has_been_shot_;
+ // True if the delay after the negedge of the beam break has occured.
+ bool has_bottom_disc_negedge_wait_position_;
+ // Posiiton of the indexer when the delayed negedge occures.
+ double bottom_disc_negedge_wait_position_;
+ // Time left after the negedge before we need to sample the indexer position.
+ double after_negedge_time_left_;
+ // Bool for the user to record if they have counted the negedge from this
+ // disc.
+ bool counted_negedge_wait_;
+ // True if the top disc sensor posedge has occured and
+ // hasn't been counted yet.
+ bool has_top_disc_posedge_position_;
+ // The position at which the posedge occured.
+ double top_disc_posedge_position_;
+ // True if the top disc sensor negedge has occured and
+ // hasn't been counted yet.
+ bool has_top_disc_negedge_position_;
+ // The position at which the negedge occured.
+ double top_disc_negedge_position_;
+};
+
+
+// Class which simulates the index and sends out queue messages containing the
+// position.
+class IndexMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // index, which will be treated as 0 by the encoder.
+ IndexMotorSimulation()
+ : index_plant_(new StateFeedbackPlant<2, 1, 1>(MakeIndexPlant())),
+ transfer_plant_(new StateFeedbackPlant<2, 1, 1>(MakeTransferPlant())),
+ bottom_disc_posedge_count_(0),
+ bottom_disc_negedge_count_(0),
+ bottom_disc_negedge_wait_count_(0),
+ bottom_disc_negedge_wait_position_(0),
+ top_disc_posedge_count_(0),
+ top_disc_posedge_position_(0.0),
+ top_disc_negedge_count_(0),
+ top_disc_negedge_position_(0.0),
+ my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status") {
+ }
+
+ // Starts a disc offset from the start of the index.
+ void InsertDisc(double offset = IndexMotor::kBottomDiscDetectStart - 0.001) {
+ Frisbee new_frisbee(transfer_roller_position(),
+ index_roller_position(),
+ offset);
+ ASSERT_FALSE(new_frisbee.bottom_disc_detect());
+ ASSERT_FALSE(new_frisbee.top_disc_detect());
+ frisbees.push_back(new_frisbee);
+ }
+
+ // Returns true if the bottom disc sensor is triggered.
+ bool BottomDiscDetect() const {
+ bool bottom_disc_detect = false;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ bottom_disc_detect |= frisbee->bottom_disc_detect();
+ }
+ return bottom_disc_detect;
+ }
+
+ // Returns true if the top disc sensor is triggered.
+ bool TopDiscDetect() const {
+ bool top_disc_detect = false;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ top_disc_detect |= frisbee->top_disc_detect();
+ }
+ return top_disc_detect;
+ }
+
+ // Updates all discs, and verifies that the state of the system is sane.
+ void UpdateDiscs(bool clamped, bool lifted, bool ejected) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ const bool old_bottom_disc_detect = frisbee->bottom_disc_detect();
+ frisbee->UpdatePosition(transfer_roller_position(),
+ index_roller_position(),
+ clamped,
+ lifted,
+ ejected);
+
+ // Look for disc detect edges and report them.
+ const bool bottom_disc_detect = frisbee->bottom_disc_detect();
+ if (old_bottom_disc_detect && !bottom_disc_detect) {
+ printf("Negedge of disc\n");
+ ++bottom_disc_negedge_count_;
+ }
+
+ if (!old_bottom_disc_detect && frisbee->bottom_disc_detect()) {
+ printf("Posedge of disc\n");
+ ++bottom_disc_posedge_count_;
+ }
+
+ // See if the frisbee has a delayed negedge and encoder value to report
+ // back.
+ if (frisbee->has_bottom_disc_negedge_wait_position()) {
+ if (!frisbee->counted_negedge_wait()) {
+ bottom_disc_negedge_wait_position_ =
+ frisbee->bottom_disc_negedge_wait_position();
+ ++bottom_disc_negedge_wait_count_;
+ frisbee->set_counted_negedge_wait(true);
+ }
+ }
+ if (frisbee->has_top_disc_posedge_position()) {
+ ++top_disc_posedge_count_;
+ top_disc_posedge_position_ = frisbee->top_disc_posedge_position();
+ }
+ if (frisbee->has_top_disc_negedge_position()) {
+ ++top_disc_negedge_count_;
+ top_disc_negedge_position_ = frisbee->top_disc_negedge_position();
+ }
+ }
+
+ // Make sure nobody is too close to anybody else.
+ Frisbee *last_frisbee = NULL;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ if (last_frisbee) {
+ const double distance = frisbee->position() - last_frisbee->position();
+ double min_distance;
+ if (frisbee->IsTouchingTransfer() ||
+ last_frisbee->IsTouchingTransfer()) {
+ min_distance = 0.3;
+ } else {
+ min_distance =
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI * 2.0 / 3.0);
+ }
+
+ EXPECT_LT(min_distance, ::std::abs(distance)) << "Discs too close";
+ }
+ last_frisbee = &*frisbee;
+ }
+
+ // Remove any shot frisbees.
+ for (int i = 0; i < static_cast<int>(frisbees.size()); ++i) {
+ if (frisbees[i].has_been_shot()) {
+ shot_frisbees.push_back(frisbees[i]);
+ frisbees.erase(frisbees.begin() + i);
+ --i;
+ }
+ }
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::IndexLoop::Position> position =
+ my_index_loop_.position.MakeMessage();
+ position->index_position = index_roller_position();
+ position->bottom_disc_detect = BottomDiscDetect();
+ position->top_disc_detect = TopDiscDetect();
+ position->bottom_disc_posedge_count = bottom_disc_posedge_count_;
+ position->bottom_disc_negedge_count = bottom_disc_negedge_count_;
+ position->bottom_disc_negedge_wait_count = bottom_disc_negedge_wait_count_;
+ position->bottom_disc_negedge_wait_position =
+ bottom_disc_negedge_wait_position_;
+ position->top_disc_posedge_count = top_disc_posedge_count_;
+ position->top_disc_posedge_position = top_disc_posedge_position_;
+ position->top_disc_negedge_count = top_disc_negedge_count_;
+ position->top_disc_negedge_position = top_disc_negedge_position_;
+ printf("bdd: %x tdd: %x posedge %d negedge %d "
+ "delaycount %d delaypos %f topcount %d toppos %f "
+ "topcount %d toppos %f\n",
+ position->bottom_disc_detect,
+ position->top_disc_detect,
+ position->bottom_disc_posedge_count,
+ position->bottom_disc_negedge_count,
+ position->bottom_disc_negedge_wait_count,
+ position->bottom_disc_negedge_wait_position,
+ position->top_disc_posedge_count,
+ position->top_disc_posedge_position,
+ position->top_disc_negedge_count,
+ position->top_disc_negedge_position);
+ position.Send();
+ }
+
+ // Simulates the index moving for one timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+
+ index_plant_->U << my_index_loop_.output->index_voltage;
+ index_plant_->Update();
+
+ transfer_plant_->U << my_index_loop_.output->transfer_voltage;
+ transfer_plant_->Update();
+ printf("tv: %f iv: %f tp : %f ip: %f\n",
+ my_index_loop_.output->transfer_voltage,
+ my_index_loop_.output->index_voltage,
+ transfer_roller_position(), index_roller_position());
+
+ UpdateDiscs(my_index_loop_.output->disc_clamped,
+ my_index_loop_.output->loader_up,
+ my_index_loop_.output->disc_ejected);
+ }
+
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndices(double offset) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ frisbee->OffsetIndex(offset);
+ }
+ }
+
+ // Plants for the index and transfer rollers.
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> index_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> transfer_plant_;
+
+ // Posedge and negedge counts for the beam break.
+ int32_t bottom_disc_posedge_count_;
+ int32_t bottom_disc_negedge_count_;
+
+ // Delayed negedge count and corrisponding position.
+ int32_t bottom_disc_negedge_wait_count_;
+ int32_t bottom_disc_negedge_wait_position_;
+
+ // Posedge count and position for the upper disc sensor.
+ int32_t top_disc_posedge_count_;
+ double top_disc_posedge_position_;
+
+ // Negedge count and position for the upper disc sensor.
+ int32_t top_disc_negedge_count_;
+ double top_disc_negedge_position_;
+
+ // Returns the absolute angle of the index.
+ double index_roller_position() const {
+ return index_plant_->Y(0, 0);
+ }
+
+ // Returns the absolute angle of the index.
+ double transfer_roller_position() const {
+ return transfer_plant_->Y(0, 0);
+ }
+
+ // Frisbees being tracked in the robot.
+ ::std::vector<Frisbee> frisbees;
+ // Frisbees that have been shot.
+ ::std::vector<Frisbee> shot_frisbees;
+
+ private:
+ // Control loop for the indexer.
+ IndexLoop my_index_loop_;
+};
+
+
+class IndexTest : public ::testing::Test {
+ protected:
+ IndexTest() : my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status"),
+ index_motor_(&my_index_loop_),
+ index_motor_plant_(),
+ loop_count_(0) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ ::frc971::control_loops::shooter.goal.Clear();
+ ::frc971::control_loops::shooter.status.Clear();
+ ::frc971::control_loops::shooter.output.Clear();
+ ::frc971::control_loops::shooter.position.Clear();
+ SendDSPacket(true);
+ Time::EnableMockTime(Time(0, 0));
+ }
+
+ virtual ~IndexTest() {
+ ::aos::robot_state.Clear();
+ ::frc971::control_loops::shooter.goal.Clear();
+ ::frc971::control_loops::shooter.status.Clear();
+ ::frc971::control_loops::shooter.output.Clear();
+ ::frc971::control_loops::shooter.position.Clear();
+ Time::DisableMockTime();
+ }
+
+ // Sends a DS packet with the enable bit set to enabled.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ // Updates the current mock time.
+ void UpdateTime() {
+ loop_count_ += 1;
+ Time::SetMockTime(Time::InMS(10 * loop_count_));
+ }
+
+ // Lets N cycles of time pass.
+ void SimulateNCycles(int cycles) {
+ for (int i = 0; i < cycles; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+ }
+
+ // Loads n discs into the indexer at the bottom.
+ void LoadNDiscs(int n) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ // Spin it up.
+ SimulateNCycles(100);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+
+ // Stuff N discs in, waiting between each one for a tiny bit of time so they
+ // don't get too close.
+ int num_grabbed = 0;
+ int wait_counter = 0;
+ while (num_grabbed < n) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (!index_motor_plant_.BottomDiscDetect()) {
+ if (wait_counter > 0) {
+ --wait_counter;
+ } else {
+ index_motor_plant_.InsertDisc();
+ ++num_grabbed;
+ wait_counter = 9;
+ }
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ // Settle.
+ for (int i = 0; i < 100; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+ }
+
+ // Loads 2 discs, and then offsets them. We then send the first disc to the
+ // grabber, and the second disc back down to the bottom. Verify that both
+ // discs get found correctly. Positive numbers shift the discs up.
+ void TestDualLostDiscs(double top_disc_offset, double bottom_disc_offset) {
+ LoadNDiscs(2);
+
+ // Move them in the indexer so they need to be re-found.
+ // The top one is moved further than the bottom one so that both edges need to
+ // be inspected.
+ index_motor_plant_.frisbees[0].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(top_disc_offset));
+ index_motor_plant_.frisbees[1].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(bottom_disc_offset));
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that the disc has been grabbed.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+ // And that we are preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Pull the disc back down.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(300);
+
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.02);
+
+ // Verify that we found the disc as accurately as the FPGA allows.
+ my_index_loop_.position.FetchLatest();
+ EXPECT_NEAR(
+ index_motor_.frisbees()[0].absolute_position(
+ my_index_loop_.position->index_position),
+ index_motor_plant_.frisbees[1].position(), 0.0001);
+ }
+
+ // Copy of core that works in this process only.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ IndexLoop my_index_loop_;
+
+ // Create a loop and simulation plant.
+ IndexMotor index_motor_;
+ IndexMotorSimulation index_motor_plant_;
+
+ // Number of loop cycles that have been executed for tracking the current
+ // time.
+ int loop_count_;
+};
+
+// Tests that the index grabs 1 disc and places it at the correct position.
+TEST_F(IndexTest, GrabSingleDisc) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 250; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ }
+ if (i > 0) {
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ index_motor_plant_.frisbees[0].position(), 0.05);
+}
+
+// Tests that the index grabs 1 disc and places it at the correct position when
+// told to hold immediately after the disc starts into the bot.
+TEST_F(IndexTest, GrabAndHold) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ // The disc has been seen. Tell the indexer to now hold.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(0).Send();
+ } else if (i > 102) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(0, my_index_loop_.status->shot_disc_count);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that the index grabs two discs and places them at the correct
+// positions.
+TEST_F(IndexTest, GrabTwoDiscs) {
+ LoadNDiscs(2);
+
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+}
+
+// Tests that the index grabs 2 discs, and loads one up into the loader to get
+// ready to shoot. It then pulls the second disc back down to be ready to
+// intake more.
+TEST_F(IndexTest, ReadyGrabsOneDisc) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that the disc has been grabbed.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+ // And that we are preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Pull the disc back down and verify that the transfer roller doesn't turn on
+ // until we are ready.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 100; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+ if (!my_index_loop_.status->ready_to_intake) {
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0)
+ << "Transfer should be off until indexer is ready";
+ }
+
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ printf("Top disc error is %f\n",
+ IndexMotor::kReadyToLiftPosition -
+ index_motor_plant_.frisbees[0].position());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.02);
+ printf("Bottom disc error is %f\n",
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI))-
+ index_motor_plant_.frisbees[1].position());
+}
+
+// Tests that the index grabs 1 disc and continues to pull it in correctly when
+// in the READY_LOWER state. The transfer roller should be disabled then.
+TEST_F(IndexTest, GrabAndReady) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ } else if (i > 150) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that grabbing 4 discs ends up with 4 discs in the bot and us no longer
+// ready.
+TEST_F(IndexTest, GrabFourDiscs) {
+ LoadNDiscs(4);
+
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 4);
+ EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ index_motor_plant_.frisbees[3].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[1].position() -
+ index_motor_plant_.frisbees[2].position()), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[2].position() -
+ index_motor_plant_.frisbees[3].position()), 0.10);
+}
+
+// Tests that shooting 4 discs works.
+TEST_F(IndexTest, ShootFourDiscs) {
+ LoadNDiscs(4);
+
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
+
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+
+ // Lifting and shooting takes a while...
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 4);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_FALSE(my_index_loop_.output->disc_clamped);
+ EXPECT_FALSE(my_index_loop_.output->loader_up);
+ EXPECT_FALSE(my_index_loop_.output->disc_ejected);
+
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.shot_frisbees.size());
+}
+
+// Tests that discs aren't pulled out of the loader half way through being
+// grabbed when being asked to index.
+TEST_F(IndexTest, PreloadToIndexEarlyTransition) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ for (int i = 0; i < 300; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ // Drop out of the loop as soon as it enters the loader.
+ // This will require it to finish the job before intaking more.
+ my_index_loop_.status.FetchLatest();
+ if (index_motor_plant_.frisbees[0].position() >
+ IndexMotor::kLoaderFreeStopPosition) {
+ break;
+ }
+ }
+
+ // Pull the disc back down and verify that the transfer roller doesn't turn on
+ // until we are ready.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ SimulateNCycles(100);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+}
+
+// Tests that disabling while grabbing a disc doesn't cause problems.
+TEST_F(IndexTest, HandleDisable) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ } else if (i > 150) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(i < 102 || i > 110);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that we can shoot after grabbing in the loader.
+TEST_F(IndexTest, GrabbedToShoot) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that it is preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Shoot them all.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(200);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ EXPECT_FALSE(my_index_loop_.status->preloaded);
+}
+
+// Tests that the cRIO can reboot and we don't loose discs.
+TEST_F(IndexTest, cRIOReboot) {
+ LoadNDiscs(2);
+
+ SimulateNCycles(100);
+ for (int i = 0; i < 100; ++i) {
+ // No position for a while is a cRIO reboot.
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(false);
+ UpdateTime();
+ }
+
+ // Shift the plant.
+ const double kPlantOffset = 5000.0;
+ index_motor_plant_.index_plant_->Y(0, 0) += kPlantOffset;
+ index_motor_plant_.index_plant_->X(0, 0) += kPlantOffset;
+ index_motor_plant_.bottom_disc_posedge_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_wait_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_wait_position_ = -1502;
+
+ // Shift the discs
+ index_motor_plant_.OffsetIndices(kPlantOffset);
+ // Let time elapse to see if the loop wants to move the discs or not.
+ SimulateNCycles(1000);
+
+ // Verify that 2 discs are at the bottom of the hopper.
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+}
+
+// Tests that invalid states are rejected.
+TEST_F(IndexTest, InvalidStateTest) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(10).Send();
+ SimulateNCycles(2);
+ // Verify that the goal is correct.
+ EXPECT_GE(4, static_cast<int>(index_motor_.safe_goal_));
+ EXPECT_LE(0, static_cast<int>(index_motor_.safe_goal_));
+}
+
+// Tests that the motor is turned off after a number of cycles of low power.
+TEST_F(IndexTest, ZeroPowerAfterTimeout) {
+ LoadNDiscs(4);
+ SimulateNCycles(100);
+
+ // Verify that the motor is hard off. This relies on floating point math
+ // never really getting to 0 unless you set it explicitly.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->index_voltage, 0.0);
+}
+
+// Tests that preloading 2 discs relocates the discs if they shift on the
+// indexer. Test shifting all 4 ways.
+TEST_F(IndexTest, ShiftedDiscsAreRefound) {
+ TestDualLostDiscs(0.10, 0.15);
+}
+
+TEST_F(IndexTest, ShiftedDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(0.15, 0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefound) {
+ TestDualLostDiscs(-0.15, -0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(-0.10, -0.15);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, IntakingAfterLoading) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(10);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CanShootOneDiscAfterReady) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(100);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(1, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, GotExtraDisc) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.1);
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.6);
+ SimulateNCycles(100);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(3, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(4 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, LostDisc) {
+ LoadNDiscs(3);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ index_motor_plant_.frisbees.erase(
+ index_motor_plant_.frisbees.begin() + 1);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(2, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(3 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+ EXPECT_EQ(0u, index_motor_.frisbees_.size());
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(0.0, my_index_loop_.output->index_voltage);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CRIOReboot) {
+ index_motor_plant_.index_plant_->Y(0, 0) = 5000.0;
+ index_motor_plant_.index_plant_->X(0, 0) = 5000.0;
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(10);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ EXPECT_EQ(1, my_index_loop_.status->hopper_disc_count);
+}
+
+// Verifies that the indexer can shoot a disc and then intake and shoot another
+// one. This verifies that the code that forgets discs works correctly.
+TEST_F(IndexTest, CanShootIntakeAndShoot) {
+ for (int i = 1; i < 4; ++i) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(500);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(i, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_EQ(i, my_index_loop_.status->shot_disc_count);
+ }
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index_main.cc b/frc971/control_loops/index/index_main.cc
new file mode 100644
index 0000000..9ca3290
--- /dev/null
+++ b/frc971/control_loops/index/index_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/index/index.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::IndexMotor indexer;
+ indexer.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
new file mode 100644
index 0000000..61e7a61
--- /dev/null
+++ b/frc971/control_loops/index/index_motor.q
@@ -0,0 +1,76 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group IndexLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The state for the indexer to be in.
+ // 0 means hold position, in a low power state.
+ // 1 means get ready to load discs by shifting the discs down.
+ // 2 means ready the discs, spin up the transfer roller, and accept discs.
+ // 3 means get ready to shoot, and place a disc grabbed in the loader.
+ // 4 means shoot at will.
+ int32_t goal_state;
+ };
+
+ message Position {
+ // Index position
+ double index_position;
+
+ // Current values of both sensors.
+ bool top_disc_detect;
+ bool bottom_disc_detect;
+ // Counts for positive and negative edges on the bottom sensor.
+ int32_t bottom_disc_posedge_count;
+ int32_t bottom_disc_negedge_count;
+ // The most recent encoder position read after the most recent
+ // negedge and a count of how many times it's been updated.
+ double bottom_disc_negedge_wait_position;
+ int32_t bottom_disc_negedge_wait_count;
+ // The most recent index position at the posedge of the top disc detect
+ // and a count of how many edges have been seen.
+ int32_t top_disc_posedge_count;
+ double top_disc_posedge_position;
+ // The most recent index position at the negedge of the top disc detect
+ // and a count of how many edges have been seen.
+ int32_t top_disc_negedge_count;
+ double top_disc_negedge_position;
+ };
+
+ message Output {
+ // Intake roller(s) output voltage.
+ // Positive means into the robot.
+ double intake_voltage;
+ // Transfer roller output voltage.
+ // Positive means into the robot.
+ double transfer_voltage;
+ // Index roller. Positive means up towards the shooter.
+ double index_voltage;
+ // Loader pistons.
+ bool disc_clamped;
+ bool loader_up;
+ bool disc_ejected;
+ };
+
+ message Status {
+ // Number of discs in the hopper
+ int32_t hopper_disc_count;
+ // Number of discs seen by the hopper.
+ int32_t total_disc_count;
+ // Number of discs shot by the shooter.
+ int32_t shot_disc_count;
+ // Disc ready in the loader.
+ bool preloaded;
+ // Indexer ready to accept more discs.
+ bool ready_to_intake;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group IndexLoop index_loop;
diff --git a/frc971/control_loops/index/index_motor_plant.cc b/frc971/control_loops/index/index_motor_plant.cc
new file mode 100644
index 0000000..51661fb
--- /dev/null
+++ b/frc971/control_loops/index/index_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/index/index_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2.40538224198, 0.0619371641882;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeIndexPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/index/index_motor_plant.h b/frc971/control_loops/index/index_motor_plant.h
new file mode 100644
index 0000000..4600ccf
--- /dev/null
+++ b/frc971/control_loops/index/index_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/transfer_motor_plant.cc b/frc971/control_loops/index/transfer_motor_plant.cc
new file mode 100644
index 0000000..9333f6a
--- /dev/null
+++ b/frc971/control_loops/index/transfer_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/index/transfer_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.06905877421, 0.0368709177253;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeTransferPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/index/transfer_motor_plant.h b/frc971/control_loops/index/transfer_motor_plant.h
new file mode 100644
index 0000000..565e8c7
--- /dev/null
+++ b/frc971/control_loops/index/transfer_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/index.py b/frc971/control_loops/python/index.py
new file mode 100755
index 0000000..c65397b
--- /dev/null
+++ b/frc971/control_loops/python/index.py
@@ -0,0 +1,94 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Index(control_loop.ControlLoop):
+ def __init__(self):
+ super(Index, self).__init__("Index")
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the index in kg m^2
+ self.J = 0.00013
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.5, .55])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ index = Index()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ index.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(index.X[0, 0])
+ simulated_v.append(index.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ index = Index()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(index.K * (R - index.X_hat), index.U_min, index.U_max)
+ index.UpdateObserver(U)
+ index.Update(U)
+ close_loop_x.append(index.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .c file name"
+ else:
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ else:
+ index.DumpHeaderFile(argv[1])
+ index.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
new file mode 100755
index 0000000..9cda41c
--- /dev/null
+++ b/frc971/control_loops/python/shooter.py
@@ -0,0 +1,140 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from matplotlib import pylab
+import control_loop
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self):
+ super(Shooter, self).__init__("Shooter")
+ # Stall Torque in N m
+ self.stall_torque = 0.49819248
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0 - 2700.0
+ # Free Current in Amps
+ self.free_current = 1.4
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.0012
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current / 2
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 11.0 / 34.0
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.6, .981])
+
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ shooter_data = numpy.genfromtxt('shooter/shooter_data.csv', delimiter=',')
+ shooter = Shooter()
+ simulated_x = []
+ real_x = []
+ x_vel = []
+ initial_x = shooter_data[0, 2]
+ last_x = initial_x
+ for i in xrange(shooter_data.shape[0]):
+ shooter.Update(numpy.matrix([[shooter_data[i, 1]]]))
+ simulated_x.append(shooter.X[0, 0])
+ x_offset = shooter_data[i, 2] - initial_x
+ real_x.append(x_offset / 2)
+ x_vel.append(shooter_data[i, 2] - last_x)
+ last_x = shooter_data[i, 2]
+
+ sim_delay = 1
+ pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+ simulated_x, label='Simulation')
+ pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+ pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+ pylab.legend()
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ shooter = Shooter()
+ close_loop_x = []
+ close_loop_U = []
+ velocity_goal = 300
+ R = numpy.matrix([[0.0], [velocity_goal]])
+ for _ in pylab.linspace(0,1.99,200):
+ # Iterate the position up.
+ R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
+ # Prevents the position goal from going beyond what is necessary.
+ velocity_weight_scalar = 0.35
+ max_reference = (
+ (shooter.U_max[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ min_reference = (
+ (shooter.U_min[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ R[0, 0] = numpy.clip(R[0, 0], min_reference, max_reference)
+ U = numpy.clip(shooter.K * (R - shooter.X_hat),
+ shooter.U_min, shooter.U_max)
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ close_loop_x.append(shooter.X[1, 0])
+ close_loop_U.append(U[0, 0])
+
+ #pylab.plotfile("shooter.csv", (0,1))
+ #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+ #pylab.plotfile("shooter.csv", (0,2))
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
+ pylab.show()
+
+ # Simulate spin down.
+ spin_down_x = [];
+ R = numpy.matrix([[50.0], [0.0]])
+ for _ in xrange(150):
+ U = 0
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ spin_down_x.append(shooter.X[1, 0])
+
+ #pylab.plot(range(150), spin_down_x)
+ #pylab.show()
+
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ else:
+ shooter.DumpHeaderFile(argv[1])
+ shooter.DumpCppFile(argv[2], argv[1])
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/transfer.py b/frc971/control_loops/python/transfer.py
new file mode 100755
index 0000000..2ec7926
--- /dev/null
+++ b/frc971/control_loops/python/transfer.py
@@ -0,0 +1,91 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Transfer(control_loop.ControlLoop):
+ def __init__(self):
+ super(Transfer, self).__init__("Transfer")
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the transfer in kg m^2
+ self.J = 0.00013
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.75, .6])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ transfer = Transfer()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ transfer.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(transfer.X[0, 0])
+ simulated_v.append(transfer.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ transfer = Transfer()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(transfer.K * (R - transfer.X_hat), transfer.U_min, transfer.U_max)
+ transfer.UpdateObserver(U)
+ transfer.Update(U)
+ close_loop_x.append(transfer.X[0, 0])
+
+ #pylab.plot(range(100), close_loop_x)
+ #pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ transfer.DumpHeaderFile(argv[1])
+ transfer.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..cecf10f
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -0,0 +1,109 @@
+#include "frc971/control_loops/shooter/shooter.h"
+
+#include "aos/aos_core.h"
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
+ : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
+ loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
+ history_position_(0),
+ position_goal_(0.0),
+ last_position_(0.0) {
+ memset(history_, 0, sizeof(history_));
+}
+
+/*static*/ const double ShooterMotor::dt = 0.01;
+/*static*/ const double ShooterMotor::kMaxSpeed =
+ 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+
+void ShooterMotor::RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status) {
+ const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ const double current_position =
+ (position == NULL ? loop_->X_hat[0] : position->position);
+ double output_voltage = 0.0;
+
+ // Track the current position if the velocity goal is small.
+ if (velocity_goal <= 1.0) {
+ position_goal_ = current_position;
+ }
+
+ loop_->Y << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ // Prevents integral windup by limiting the position error such that the
+ // error can't produce much more than full power.
+ const double kVelocityWeightScalar = 0.35;
+ const double max_reference =
+ (loop_->plant.U_max[0] - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[0];
+ const double min_reference =
+ (loop_->plant.U_min[0] - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[0];
+
+ position_goal_ = ::std::max(::std::min(position_goal_, max_reference),
+ min_reference);
+ loop_->R << position_goal_, velocity_goal;
+ position_goal_ += velocity_goal * dt;
+
+ loop_->Update(position, output == NULL);
+
+ // Kill power at low velocity goals.
+ if (velocity_goal < 1.0) {
+ loop_->U[0] = 0.0;
+ } else {
+ output_voltage = loop_->U[0];
+ }
+
+ LOG(DEBUG,
+ "PWM: %f, raw_pos: %f rotations: %f "
+ "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
+ output_voltage, current_position,
+ current_position / (2 * M_PI),
+ (current_position - last_position_) / dt,
+ loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+
+ // Calculates the velocity over the last kHistoryLength * .01 seconds
+ // by taking the difference between the current and next history positions.
+ int old_history_position = ((history_position_ == 0) ?
+ kHistoryLength : history_position_) - 1;
+ average_velocity_ = (history_[old_history_position] -
+ history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+
+ status->average_velocity = average_velocity_;
+
+ // Determine if the velocity is close enough to the goal to be ready.
+ if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
+ velocity_goal != 0.0) {
+ LOG(DEBUG, "Steady: ");
+ status->ready = true;
+ } else {
+ LOG(DEBUG, "Not ready: ");
+ status->ready = false;
+ }
+ LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+
+ last_position_ = current_position;
+
+ if (output) {
+ output->voltage = output_voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..bf1cd67
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -0,0 +1,83 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'shooter_loop',
+ 'type': 'static_library',
+ 'sources': ['shooter_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/shooter',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'shooter_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shooter.cc',
+ 'shooter_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'shooter_loop',
+ 'shooter_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_csv',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_csv.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'shooter_lib',
+ 'shooter_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..77c605b
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.h
@@ -0,0 +1,54 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class ShooterMotor
+ : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ public:
+ explicit ShooterMotor(
+ control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+
+ // Control loop time step.
+ static const double dt;
+
+ // Maximum speed of the shooter wheel which the encoder is rated for in
+ // rad/sec.
+ static const double kMaxSpeed;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status);
+
+ private:
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+
+ // History array and stuff for determining average velocity and whether
+ // we are ready to shoot.
+ static const int kHistoryLength = 5;
+ double history_[kHistoryLength];
+ ptrdiff_t history_position_;
+ double average_velocity_;
+
+ double position_goal_;
+ double last_position_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
diff --git a/frc971/control_loops/shooter/shooter_csv.cc b/frc971/control_loops/shooter/shooter_csv.cc
new file mode 100644
index 0000000..de2b4ee
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_csv.cc
@@ -0,0 +1,51 @@
+#include "stdio.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+
+using ::frc971::control_loops::shooter;
+using ::aos::time::Time;
+
+int main(int argc, char * argv[]) {
+ FILE *data_file = NULL;
+ FILE *output_file = NULL;
+
+ if (argc == 2) {
+ data_file = fopen(argv[1], "w");
+ output_file = data_file;
+ } else {
+ printf("Logging to stdout instead\n");
+ output_file = stdout;
+ }
+
+ fprintf(data_file, "time, power, position");
+
+ ::aos::Init();
+
+ Time start_time = Time::Now();
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(2000);
+ shooter.goal.FetchLatest();
+ shooter.status.FetchLatest();
+ shooter.position.FetchLatest();
+ shooter.output.FetchLatest();
+ if (shooter.output.get() &&
+ shooter.position.get()) {
+ fprintf(output_file, "\n%f, %f, %f",
+ (shooter.position->sent_time - start_time).ToSeconds(),
+ shooter.output->voltage,
+ shooter.position->position);
+ }
+ }
+
+ if (data_file) {
+ fclose(data_file);
+ }
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/control_loops/shooter/shooter_data.csv b/frc971/control_loops/shooter/shooter_data.csv
new file mode 100644
index 0000000..1c110f8
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_data.csv
@@ -0,0 +1,117 @@
+738.970696729,0.000000,2609.485398
+738.990725254,0.000000,2609.485398
+739.10668572,0.000000,2609.485398
+739.30868416,0.000000,2609.485398
+739.50644324,0.000000,2609.485398
+739.70993003,0.000000,2609.485398
+739.90893299,0.000000,2609.485398
+739.110790660,0.000000,2609.485398
+739.130856480,0.000000,2609.485398
+739.150843939,0.000000,2609.485398
+739.170774964,0.000000,2609.485398
+739.190835406,0.000000,2609.485398
+739.210871962,0.000000,2609.485398
+739.230838889,0.000000,2609.485398
+739.250849394,0.000000,2609.485398
+739.270710020,0.000000,2609.485398
+739.290835485,0.000000,2609.485398
+739.310975196,0.000000,2609.485398
+739.330836940,0.000000,2609.485398
+739.350841719,0.000000,2609.485398
+739.370882116,0.000000,2609.485398
+739.390924188,0.000000,2609.485398
+739.410957743,0.000000,2609.485398
+739.430902249,0.000000,2609.485398
+739.450846964,0.000000,2609.485398
+739.470722325,0.000000,2609.485398
+739.490794153,0.000000,2609.485398
+739.510974652,0.000000,2609.485398
+739.530838142,0.000000,2609.485398
+739.550941466,0.000000,2609.485398
+739.571202981,0.000000,2609.485398
+739.590857785,0.000000,2609.485398
+739.610997565,0.000000,2609.485398
+739.630899678,0.000000,2609.485398
+739.650656101,0.000000,2609.485398
+739.671141736,0.000000,2609.485398
+739.690876859,0.000000,2609.485398
+739.711039759,0.000000,2609.485398
+739.730890047,0.000000,2609.485398
+739.750892591,0.000000,2609.485398
+739.770626663,0.000000,2609.485398
+739.790778110,6.000000,2609.485398
+739.810913701,6.000000,2609.528710
+739.830692404,6.000000,2609.788585
+739.850725399,6.000000,2610.329990
+739.870711739,6.000000,2611.087957
+739.890776372,6.000000,2612.019173
+739.910893176,6.000000,2613.231920
+739.930686684,6.000000,2614.639573
+739.950843716,6.000000,2616.198820
+739.970639251,6.000000,2617.996284
+739.990663653,6.000000,2619.966998
+740.10690431,6.000000,2622.067650
+740.30636963,6.000000,2624.341551
+740.50667023,6.000000,2626.788701
+740.70669008,6.000000,2629.387445
+740.90640333,6.000000,2632.116126
+740.110875518,6.000000,2634.974745
+740.130666584,6.000000,2637.963300
+740.150771653,6.000000,2641.060137
+740.170676698,6.000000,2644.265254
+740.190627279,6.000000,2647.600309
+740.210613899,6.000000,2651.021988
+740.230642424,6.000000,2654.551949
+740.251043972,6.000000,2658.168534
+740.270911233,6.000000,2661.871744
+740.290883116,6.000000,2665.661579
+740.310902352,6.000000,2669.538039
+740.330648299,6.000000,2673.479467
+740.350654054,6.000000,2677.485864
+740.370725670,6.000000,2681.578886
+740.390879841,6.000000,2685.715220
+740.410951525,6.000000,2689.916522
+740.430850006,6.000000,2694.182794
+740.450856740,6.000000,2698.514033
+740.470878143,6.000000,2702.888586
+740.490685270,6.000000,2707.306450
+740.510980587,6.000000,2711.810940
+740.530967487,6.000000,2716.315429
+740.550840964,6.000000,2720.884887
+740.570872352,6.000000,2725.497658
+740.590882927,6.000000,2730.153741
+740.611002875,6.000000,2734.831480
+740.630612282,6.000000,2739.574187
+740.650721402,6.000000,2744.338551
+740.670723318,6.000000,2749.146228
+740.690835093,6.000000,2753.975560
+740.710883940,6.000000,2758.826549
+740.730851843,6.000000,2763.720850
+740.750864165,6.000000,2768.658463
+740.770859165,6.000000,2773.617733
+740.790917442,6.000000,2778.577002
+740.810989130,6.000000,2783.579585
+740.830854016,6.000000,2788.625479
+740.850606738,6.000000,2793.649717
+740.870962746,6.000000,2798.738924
+740.890848306,6.000000,2803.828131
+740.910991649,6.000000,2808.938994
+740.930855839,6.000000,2814.071513
+740.950973968,6.000000,2819.247345
+740.970789060,6.000000,2824.401520
+740.990920668,6.000000,2829.599008
+741.10980832,6.000000,2834.796496
+741.30662594,6.000000,2840.015640
+741.50930045,6.000000,2845.256440
+741.70977009,6.000000,2850.453928
+741.90974176,6.000000,2855.759697
+741.110959398,6.000000,2861.022153
+741.130866259,6.000000,2866.306266
+741.150815025,6.000000,2871.612035
+741.170950337,6.000000,2876.917804
+741.190887999,6.000000,2882.245229
+741.210981335,6.000000,2887.572654
+741.230875206,6.000000,2892.900079
+741.250802599,6.000000,2898.270816
+741.270604281,6.000000,2903.619898
+741.290897085,6.000000,2908.990635
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..beb62f1
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,193 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/control_loops/shooter/shooter.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterMotorSimulation {
+ public:
+ // Constructs a shooter simulation. I'm not sure how the construction of
+ // the queue (my_shooter_loop_) actually works (same format as wrist).
+ ShooterMotorSimulation()
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+ my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372, ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status") {
+ }
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ my_shooter_loop_.position.MakeMessage();
+ position->position = shooter_plant_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ shooter_plant_->U << my_shooter_loop_.output->voltage;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+ private:
+ ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372,
+ ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status"),
+ shooter_motor_(&my_shooter_loop_),
+ shooter_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and the shooter won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_NEAR(my_shooter_loop_.goal->velocity,
+ my_shooter_loop_.status->average_velocity,
+ 10.0);
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ ShooterLoop my_shooter_loop_;
+
+ // Create a control loop and simulation.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ SendDSPacket(true);
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ VerifyNearGoal();
+ my_shooter_loop_.output.FetchLatest();
+ EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ VerifyNearGoal();
+
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+ }
+}
+
+// Test to make sure that it does not exceed the encoder's rated speed.
+TEST_F(ShooterTest, RateLimitTest) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(1000.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_GT(shooter_motor_.kMaxSpeed,
+ shooter_motor_plant_.shooter_plant_->X(1, 0));
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(i > 50);
+ }
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_main.cc b/frc971/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..72b820e
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/shooter/shooter.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ShooterMotor shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/shooter/shooter_motor.q b/frc971/control_loops/shooter/shooter_motor.q
new file mode 100644
index 0000000..f2abf25
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor.q
@@ -0,0 +1,31 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group ShooterLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Goal velocity in rad/sec
+ double velocity;
+ };
+
+ message Status {
+ // True if the shooter is up to speed.
+ bool ready;
+ // The average velocity over the last 0.1 seconds.
+ double average_velocity;
+ };
+
+ message Position {
+ // The angle of the shooter wheel measured in rad/sec.
+ double position;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue Status status;
+};
+
+queue_group ShooterLoop shooter;
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..c13ce2d
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.0098571228289, 0.0, 0.971561310859;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00785005397639, 1.56249765488;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.07156131086, 28.0940195016;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 0.486400730028, 0.247515916371;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeShooterPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
new file mode 100644
index 0000000..ac42b0b
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 8fe18f8..894487b 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -1,7 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
-// wikipedia article is <http://en.wikipedia.org/wiki/State_observer>
+#include <assert.h>
// Stupid vxworks system headers define it which blows up Eigen...
#undef m_data
@@ -56,20 +56,19 @@
virtual ~StateFeedbackPlant() {}
- // If U is outside the hardware range, limit it before the plant tries to use
- // it.
- virtual void CapU() {
+ // Assert that U is within the hardware range.
+ virtual void CheckU() {
for (int i = 0; i < kNumOutputs; ++i) {
- if (U[i] > U_max[i]) {
- U[i] = U_max[i];
- } else if (U[i] < U_min[i]) {
- U[i] = U_min[i];
- }
+ assert(U[i] <= U_max[i]);
+ assert(U[i] >= U_min[i]);
}
}
+
// Computes the new X and Y given the control input.
void Update() {
- CapU();
+ // Powers outside of the range are more likely controller bugs than things
+ // that the plant should deal with.
+ CheckU();
X = A * X + B * U;
Y = C * X + D * U;
}