Copy back a lot of the 2014 code.
Change-Id: I552292d8bd7bce4409e02d254bef06a9cc009568
diff --git a/y2014/control_loops/shooter/replay_shooter.cc b/y2014/control_loops/shooter/replay_shooter.cc
new file mode 100644
index 0000000..20c953f
--- /dev/null
+++ b/y2014/control_loops/shooter/replay_shooter.cc
@@ -0,0 +1,24 @@
+#include "aos/common/controls/replay_control_loop.h"
+#include "aos/linux_code/init.h"
+
+#include "y2014/control_loops/shooter/shooter.q.h"
+
+// Reads one or more log files and sends out all the queue messages (in the
+// correct order and at the correct time) to feed a "live" shooter process.
+
+int main(int argc, char **argv) {
+ if (argc <= 1) {
+ fprintf(stderr, "Need at least one file to replay!\n");
+ return EXIT_FAILURE;
+ }
+
+ ::aos::InitNRT();
+
+ ::aos::controls::ControlLoopReplayer<::frc971::control_loops::ShooterQueue>
+ replayer(&::frc971::control_loops::shooter_queue, "shooter");
+ for (int i = 1; i < argc; ++i) {
+ replayer.ProcessFile(argv[i]);
+ }
+
+ ::aos::Cleanup();
+}
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..f137235
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -0,0 +1,695 @@
+#include "y2014/control_loops/shooter/shooter.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+#include <limits>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2014/constants.h"
+#include "y2014/control_loops/shooter/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+using ::aos::time::Time;
+
+void ZeroedStateFeedbackLoop::CapU() {
+ const double old_voltage = voltage_;
+ voltage_ += U(0, 0);
+
+ uncapped_voltage_ = voltage_;
+
+ // Make sure that reality and the observer can't get too far off. There is a
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // against last cycle's voltage.
+ if (X_hat(2, 0) > last_voltage_ + 4.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
+ LOG(DEBUG, "Capping due to runaway\n");
+ } else if (X_hat(2, 0) < last_voltage_ - 4.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
+ LOG(DEBUG, "Capping due to runaway\n");
+ }
+
+ voltage_ = std::min(max_voltage_, voltage_);
+ voltage_ = std::max(-max_voltage_, voltage_);
+ mutable_U(0, 0) = voltage_ - old_voltage;
+
+ LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
+
+ last_voltage_ = voltage_;
+ capped_goal_ = false;
+}
+
+void ZeroedStateFeedbackLoop::CapGoal() {
+ if (uncapped_voltage() > max_voltage_) {
+ double dx;
+ if (controller_index() == 0) {
+ dx = (uncapped_voltage() - max_voltage_) /
+ (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ mutable_R(0, 0) -= dx;
+ mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ } else {
+ dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
+ mutable_R(0, 0) -= dx;
+ }
+ capped_goal_ = true;
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
+ } else if (uncapped_voltage() < -max_voltage_) {
+ double dx;
+ if (controller_index() == 0) {
+ dx = (uncapped_voltage() + max_voltage_) /
+ (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ mutable_R(0, 0) -= dx;
+ mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ } else {
+ dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
+ mutable_R(0, 0) -= dx;
+ }
+ capped_goal_ = true;
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
+ } else {
+ capped_goal_ = false;
+ }
+}
+
+void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
+ if (controller_index() == 0) {
+ mutable_R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
+ } else {
+ mutable_R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
+ }
+}
+
+void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
+ double known_position) {
+ double old_position = absolute_position();
+ double previous_offset = offset_;
+ offset_ = known_position - encoder_val;
+ double doffset = offset_ - previous_offset;
+ mutable_X_hat(0, 0) += doffset;
+ // Offset our measurements because the offset is baked into them.
+ // This is safe because if we got here, it means position != nullptr, which
+ // means we already set Y to something and it won't just get overwritten.
+ mutable_Y(0, 0) += doffset;
+ // Offset the goal so we don't move.
+ mutable_R(0, 0) += doffset;
+ if (controller_index() == 0) {
+ mutable_R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
+ }
+ LOG_STRUCT(
+ DEBUG, "sensor edge (fake?)",
+ ShooterChangeCalibration(encoder_val, known_position, old_position,
+ absolute_position(), previous_offset, offset_));
+}
+
+ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
+ : aos::controls::ControlLoop<control_loops::ShooterGroup>(my_shooter),
+ shooter_(MakeShooterLoop()),
+ state_(STATE_INITIALIZE),
+ loading_problem_end_time_(0, 0),
+ load_timeout_(0, 0),
+ shooter_brake_set_time_(0, 0),
+ unload_timeout_(0, 0),
+ shot_end_time_(0, 0),
+ cycles_not_moved_(0),
+ shot_count_(0),
+ zeroed_(false),
+ distal_posedge_validation_cycles_left_(0),
+ proximal_posedge_validation_cycles_left_(0),
+ last_distal_current_(true),
+ last_proximal_current_(true) {}
+
+double ShooterMotor::PowerToPosition(double power) {
+ const frc971::constants::Values &values = constants::GetValues();
+ double maxpower = 0.5 * kSpringConstant *
+ (kMaxExtension * kMaxExtension -
+ (kMaxExtension - values.shooter.upper_limit) *
+ (kMaxExtension - values.shooter.upper_limit));
+ if (power < 0) {
+ LOG_STRUCT(WARNING, "negative power", PowerAdjustment(power, 0));
+ power = 0;
+ } else if (power > maxpower) {
+ LOG_STRUCT(WARNING, "power too high", PowerAdjustment(power, maxpower));
+ power = maxpower;
+ }
+
+ double mp = kMaxExtension * kMaxExtension - (power + power) / kSpringConstant;
+ double new_pos = 0.10;
+ if (mp < 0) {
+ LOG(ERROR,
+ "Power calculation has negative number before square root (%f).\n", mp);
+ } else {
+ new_pos = kMaxExtension - ::std::sqrt(mp);
+ }
+
+ new_pos = ::std::min(::std::max(new_pos, values.shooter.lower_limit),
+ values.shooter.upper_limit);
+ return new_pos;
+}
+
+double ShooterMotor::PositionToPower(double position) {
+ double power = kSpringConstant * position * (kMaxExtension - position / 2.0);
+ return power;
+}
+
+void ShooterMotor::CheckCalibrations(
+ const control_loops::ShooterGroup::Position *position) {
+ CHECK_NOTNULL(position);
+ const frc971::constants::Values &values = constants::GetValues();
+
+ // TODO(austin): Validate that this is the right edge.
+ // If we see a posedge on any of the hall effects,
+ if (position->pusher_proximal.posedge_count != last_proximal_posedge_count_ &&
+ !last_proximal_current_) {
+ proximal_posedge_validation_cycles_left_ = 2;
+ }
+ if (proximal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_proximal.current) {
+ --proximal_posedge_validation_cycles_left_;
+ if (proximal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_proximal.posedge_value,
+ values.shooter.pusher_proximal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using proximal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ proximal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+
+ if (position->pusher_distal.posedge_count != last_distal_posedge_count_ &&
+ !last_distal_current_) {
+ distal_posedge_validation_cycles_left_ = 2;
+ }
+ if (distal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_distal.current) {
+ --distal_posedge_validation_cycles_left_;
+ if (distal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_distal.posedge_value,
+ values.shooter.pusher_distal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using distal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ distal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+}
+
+// Positive is out, and positive power is out.
+void ShooterMotor::RunIteration(
+ const control_loops::ShooterGroup::Goal *goal,
+ const control_loops::ShooterGroup::Position *position,
+ control_loops::ShooterGroup::Output *output,
+ control_loops::ShooterGroup::Status *status) {
+ constexpr double dt = 0.01;
+
+ if (goal && ::std::isnan(goal->shot_power)) {
+ state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because got a shot power of NAN.\n");
+ }
+
+ // we must always have these or we have issues.
+ if (status == NULL) {
+ if (output) output->voltage = 0;
+ LOG(ERROR, "Thought I would just check for null and die.\n");
+ return;
+ }
+ status->ready = false;
+
+ if (WasReset()) {
+ state_ = STATE_INITIALIZE;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
+ }
+ if (position) {
+ shooter_.CorrectPosition(position->position);
+ }
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) output->voltage = 0;
+
+ const frc971::constants::Values &values = constants::GetValues();
+
+ // Don't even let the control loops run.
+ bool shooter_loop_disable = false;
+
+ const bool disabled =
+ !::aos::joystick_state.get() || !::aos::joystick_state->enabled;
+
+ // If true, move the goal if we saturate.
+ bool cap_goal = false;
+
+ // TODO(austin): Move the offset if we see or don't see a hall effect when we
+ // expect to see one.
+ // Probably not needed yet.
+
+ if (position) {
+ int last_controller_index = shooter_.controller_index();
+ if (position->plunger && position->latch) {
+ // Use the controller without the spring if the latch is set and the
+ // plunger is back
+ shooter_.set_controller_index(1);
+ } else {
+ // Otherwise use the controller with the spring.
+ shooter_.set_controller_index(0);
+ }
+ if (shooter_.controller_index() != last_controller_index) {
+ shooter_.RecalculatePowerGoal();
+ }
+ }
+
+ switch (state_) {
+ case STATE_INITIALIZE:
+ if (position) {
+ // Reinitialize the internal filter state.
+ shooter_.InitializeState(position->position);
+
+ // Start off with the assumption that we are at the value
+ // futhest back given our sensors.
+ if (position->pusher_distal.current) {
+ shooter_.SetCalibration(position->position,
+ values.shooter.pusher_distal.lower_angle);
+ } else if (position->pusher_proximal.current) {
+ shooter_.SetCalibration(position->position,
+ values.shooter.pusher_proximal.upper_angle);
+ } else {
+ shooter_.SetCalibration(position->position,
+ values.shooter.upper_limit);
+ }
+
+ // Go to the current position.
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ // If the plunger is all the way back, we want to be latched.
+ latch_piston_ = position->plunger;
+ brake_piston_ = false;
+ if (position->latch == latch_piston_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ shooter_loop_disable = true;
+ LOG(DEBUG,
+ "Not moving on until the latch has moved to avoid a crash\n");
+ }
+ } else {
+ // If we can't start yet because we don't know where we are, set the
+ // latch and brake to their defaults.
+ latch_piston_ = true;
+ brake_piston_ = true;
+ }
+ break;
+ case STATE_REQUEST_LOAD:
+ if (position) {
+ zeroed_ = false;
+ if (position->pusher_distal.current ||
+ position->pusher_proximal.current) {
+ // We started on the sensor, back up until we are found.
+ // If the plunger is all the way back and not latched, it won't be
+ // there for long.
+ state_ = STATE_LOAD_BACKTRACK;
+
+ // The plunger is already back and latched. Don't release it.
+ if (position->plunger && position->latch) {
+ latch_piston_ = true;
+ } else {
+ latch_piston_ = false;
+ }
+ } else if (position->plunger && position->latch) {
+ // The plunger is back and we are latched. We most likely got here
+ // from Initialize, in which case we want to 'load' again anyways to
+ // zero.
+ Load();
+ latch_piston_ = true;
+ } else {
+ // Off the sensor, start loading.
+ Load();
+ latch_piston_ = false;
+ }
+ }
+
+ // Hold our current position.
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ brake_piston_ = false;
+ break;
+ case STATE_LOAD_BACKTRACK:
+ // If we are here, then that means we started past the edge where we want
+ // to zero. Move backwards until we don't see the sensor anymore.
+ // The plunger is contacting the pusher (or will be shortly).
+
+ if (!disabled) {
+ shooter_.SetGoalPosition(
+ shooter_.goal_position() + values.shooter.zeroing_speed * dt,
+ values.shooter.zeroing_speed);
+ }
+ cap_goal = true;
+ shooter_.set_max_voltage(4.0);
+
+ if (position) {
+ if (!position->pusher_distal.current &&
+ !position->pusher_proximal.current) {
+ Load();
+ }
+ latch_piston_ = position->plunger;
+ }
+
+ brake_piston_ = false;
+ break;
+ case STATE_LOAD:
+ // If we are disabled right now, reset the timer.
+ if (disabled) {
+ Load();
+ // Latch defaults to true when disabled. Leave it latched until we have
+ // useful sensor data.
+ latch_piston_ = true;
+ }
+ if (output == nullptr) {
+ load_timeout_ += ::aos::controls::kLoopFrequency;
+ }
+ // Go to 0, which should be the latch position, or trigger a hall effect
+ // on the way. If we don't see edges where we are supposed to, the
+ // offset will be updated by code above.
+ shooter_.SetGoalPosition(0.0, 0.0);
+
+ if (position) {
+ CheckCalibrations(position);
+
+ // Latch if the plunger is far enough back to trigger the hall effect.
+ // This happens when the distal sensor is triggered.
+ latch_piston_ = position->pusher_distal.current || position->plunger;
+
+ // Check if we are latched and back. Make sure the plunger is all the
+ // way back as well.
+ if (position->plunger && position->latch &&
+ position->pusher_distal.current) {
+ if (!zeroed_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ state_ = STATE_PREPARE_SHOT;
+ }
+ } else if (position->plunger &&
+ ::std::abs(shooter_.absolute_position() -
+ shooter_.goal_position()) < 0.001) {
+ // We are at the goal, but not latched.
+ state_ = STATE_LOADING_PROBLEM;
+ loading_problem_end_time_ = Time::Now() + kLoadProblemEndTimeout;
+ }
+ }
+ if (load_timeout_ < Time::Now()) {
+ if (position) {
+ if (!position->pusher_distal.current ||
+ !position->pusher_proximal.current) {
+ state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to load.\n");
+ }
+ }
+ }
+ brake_piston_ = false;
+ break;
+ case STATE_LOADING_PROBLEM:
+ if (disabled) {
+ state_ = STATE_REQUEST_LOAD;
+ break;
+ }
+ // We got to the goal, but the latch hasn't registered as down. It might
+ // be stuck, or on it's way but not there yet.
+ if (Time::Now() > loading_problem_end_time_) {
+ // Timeout by unloading.
+ Unload();
+ } else if (position && position->plunger && position->latch) {
+ // If both trigger, we are latched.
+ state_ = STATE_PREPARE_SHOT;
+ }
+ // Move a bit further back to help it trigger.
+ // If the latch is slow due to the air flowing through the tubes or
+ // inertia, but is otherwise free, this won't have much time to do
+ // anything and is safe. Otherwise this gives us a bit more room to free
+ // up the latch.
+ shooter_.SetGoalPosition(values.shooter.lower_limit, 0.0);
+ if (position) {
+ LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
+ position->plunger, position->latch);
+ }
+
+ latch_piston_ = true;
+ brake_piston_ = false;
+ break;
+ case STATE_PREPARE_SHOT:
+ // Move the shooter to the shot power set point and then lock the brake.
+ // TODO(austin): Timeout. Low priority.
+
+ if (goal) {
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ }
+
+ LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
+ shooter_.absolute_position(),
+ goal ? PowerToPosition(goal->shot_power)
+ : ::std::numeric_limits<double>::quiet_NaN());
+ if (goal &&
+ ::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) < 0.001 &&
+ ::std::abs(shooter_.absolute_velocity()) < 0.005) {
+ // We are there, set the brake and move on.
+ latch_piston_ = true;
+ brake_piston_ = true;
+ shooter_brake_set_time_ = Time::Now() + kShooterBrakeSetTime;
+ state_ = STATE_READY;
+ } else {
+ latch_piston_ = true;
+ brake_piston_ = false;
+ }
+ if (goal && goal->unload_requested) {
+ Unload();
+ }
+ break;
+ case STATE_READY:
+ LOG(DEBUG, "In ready\n");
+ // Wait until the brake is set, and a shot is requested or the shot power
+ // is changed.
+ if (Time::Now() > shooter_brake_set_time_) {
+ status->ready = true;
+ // We have waited long enough for the brake to set, turn the shooter
+ // control loop off.
+ shooter_loop_disable = true;
+ LOG(DEBUG, "Brake is now set\n");
+ if (goal && goal->shot_requested && !disabled) {
+ LOG(DEBUG, "Shooting now\n");
+ shooter_loop_disable = true;
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
+ firing_starting_position_ = shooter_.absolute_position();
+ state_ = STATE_FIRE;
+ }
+ }
+ if (state_ == STATE_READY && goal &&
+ ::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) > 0.002) {
+ // TODO(austin): Add a state to release the brake.
+
+ // TODO(austin): Do we want to set the brake here or after shooting?
+ // Depends on air usage.
+ status->ready = false;
+ LOG(DEBUG, "Preparing shot again.\n");
+ state_ = STATE_PREPARE_SHOT;
+ }
+
+ if (goal) {
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ }
+
+ latch_piston_ = true;
+ brake_piston_ = true;
+
+ if (goal && goal->unload_requested) {
+ Unload();
+ }
+ break;
+
+ case STATE_FIRE:
+ if (disabled) {
+ if (position) {
+ if (position->plunger) {
+ // If disabled and the plunger is still back there, reset the
+ // timeout.
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
+ }
+ }
+ }
+ shooter_loop_disable = true;
+ // Count the number of contiguous cycles during which we haven't moved.
+ if (::std::abs(last_position_.position - shooter_.absolute_position()) <
+ 0.0005) {
+ ++cycles_not_moved_;
+ } else {
+ cycles_not_moved_ = 0;
+ }
+
+ // If we have moved any amount since the start and the shooter has now
+ // been still for a couple cycles, the shot finished.
+ // Also move on if it times out.
+ if ((::std::abs(firing_starting_position_ -
+ shooter_.absolute_position()) > 0.0005 &&
+ cycles_not_moved_ > 3) ||
+ Time::Now() > shot_end_time_) {
+ state_ = STATE_REQUEST_LOAD;
+ ++shot_count_;
+ }
+ latch_piston_ = false;
+ brake_piston_ = true;
+ break;
+ case STATE_UNLOAD:
+ // Reset the timeouts.
+ if (disabled) Unload();
+
+ // If it is latched and the plunger is back, move the pusher back to catch
+ // the plunger.
+ bool all_back;
+ if (position) {
+ all_back = position->plunger && position->latch;
+ } else {
+ all_back = last_position_.plunger && last_position_.latch;
+ }
+
+ if (all_back) {
+ // Pull back to 0, 0.
+ shooter_.SetGoalPosition(0.0, 0.0);
+ if (shooter_.absolute_position() < 0.005) {
+ // When we are close enough, 'fire'.
+ latch_piston_ = false;
+ } else {
+ latch_piston_ = true;
+
+ if (position) {
+ CheckCalibrations(position);
+ }
+ }
+ } else {
+ // The plunger isn't all the way back, or it is and it is unlatched, so
+ // we can now unload.
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ latch_piston_ = false;
+ state_ = STATE_UNLOAD_MOVE;
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
+ }
+
+ if (Time::Now() > unload_timeout_) {
+ // We have been stuck trying to unload for way too long, give up and
+ // turn everything off.
+ state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to unload.\n");
+ }
+
+ brake_piston_ = false;
+ break;
+ case STATE_UNLOAD_MOVE: {
+ if (disabled) {
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
+ shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+ }
+ cap_goal = true;
+ shooter_.set_max_voltage(6.0);
+
+ // Slowly move back until we hit the upper limit.
+ // If we were at the limit last cycle, we are done unloading.
+ // This is because if we saturate, we might hit the limit before we are
+ // actually there.
+ if (shooter_.goal_position() >= values.shooter.upper_limit) {
+ shooter_.SetGoalPosition(values.shooter.upper_limit, 0.0);
+ // We don't want the loop fighting the spring when we are unloaded.
+ // Turn it off.
+ shooter_loop_disable = true;
+ state_ = STATE_READY_UNLOAD;
+ } else {
+ shooter_.SetGoalPosition(
+ ::std::min(
+ values.shooter.upper_limit,
+ shooter_.goal_position() + values.shooter.unload_speed * dt),
+ values.shooter.unload_speed);
+ }
+
+ latch_piston_ = false;
+ brake_piston_ = false;
+ } break;
+ case STATE_READY_UNLOAD:
+ if (goal && goal->load_requested) {
+ state_ = STATE_REQUEST_LOAD;
+ }
+ // If we are ready to load again,
+ shooter_loop_disable = true;
+
+ latch_piston_ = false;
+ brake_piston_ = false;
+ break;
+
+ case STATE_ESTOP:
+ LOG(WARNING, "estopped\n");
+ // Totally lost, go to a safe state.
+ shooter_loop_disable = true;
+ latch_piston_ = true;
+ brake_piston_ = true;
+ break;
+ }
+
+ if (!shooter_loop_disable) {
+ LOG_STRUCT(DEBUG, "running the loop",
+ ShooterStatusToLog(shooter_.goal_position(),
+ shooter_.absolute_position()));
+ if (!cap_goal) {
+ shooter_.set_max_voltage(12.0);
+ }
+ shooter_.Update(output == NULL);
+ if (cap_goal) {
+ shooter_.CapGoal();
+ }
+ // We don't really want to output anything if we went through everything
+ // assuming the motors weren't working.
+ if (output) output->voltage = shooter_.voltage();
+ } else {
+ shooter_.Update(true);
+ shooter_.ZeroPower();
+ if (output) output->voltage = 0.0;
+ }
+
+ status->hard_stop_power = PositionToPower(shooter_.absolute_position());
+
+ if (output) {
+ output->latch_piston = latch_piston_;
+ output->brake_piston = brake_piston_;
+ }
+
+ if (position) {
+ LOG_STRUCT(DEBUG, "internal state",
+ ShooterStateToLog(
+ shooter_.absolute_position(), shooter_.absolute_velocity(),
+ state_, position->latch, position->pusher_proximal.current,
+ position->pusher_distal.current, position->plunger,
+ brake_piston_, latch_piston_));
+
+ last_position_ = *position;
+
+ last_distal_posedge_count_ = position->pusher_distal.posedge_count;
+ last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
+ }
+
+ status->shots = shot_count_;
+}
+
+void ShooterMotor::ZeroOutputs() {
+ queue_group()->output.MakeWithBuilder()
+ .voltage(0)
+ .latch_piston(latch_piston_)
+ .brake_piston(brake_piston_)
+ .Send();
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/shooter/shooter.gyp b/y2014/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..260fb11
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter.gyp
@@ -0,0 +1,82 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'replay_shooter',
+ 'type': 'executable',
+ 'variables': {
+ 'no_rsync': 1,
+ },
+ 'sources': [
+ 'replay_shooter.cc',
+ ],
+ 'dependencies': [
+ 'shooter_queue',
+ '<(AOS)/common/controls/controls.gyp:replay_control_loop',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'shooter_loop',
+ 'type': 'static_library',
+ 'sources': ['shooter.q'],
+ 'variables': {
+ 'header_path': 'y2014/control_loops/shooter',
+ },
+ 'dependencies': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'shooter_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shooter.cc',
+ 'shooter_motor_plant.cc',
+ 'unaugmented_shooter_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'shooter_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/y2014/y2014.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ ],
+ 'export_dependent_settings': [
+ 'shooter_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'shooter_loop',
+ 'shooter_lib',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'shooter_lib',
+ ],
+ },
+ ],
+}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..1339f6b
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter.h
@@ -0,0 +1,224 @@
+#ifndef Y2014_CONTROL_LOOPS_shooter_shooter_H_
+#define Y2014_CONTROL_LOOPS_shooter_shooter_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "aos/common/time.h"
+
+#include "y2014/constants.h"
+#include "y2014/control_loops/shooter/shooter_motor_plant.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class ShooterTest_UnloadWindupPositive_Test;
+class ShooterTest_UnloadWindupNegative_Test;
+class ShooterTest_RezeroWhileUnloading_Test;
+};
+
+using ::aos::time::Time;
+
+// Note: Everything in this file assumes that there is a 1 cycle delay between
+// power being requested and it showing up at the motor. It assumes that
+// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
+// that isn't true.
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about.
+// It does not have any zeroing logic in it, only logic to deal with a delta U
+// controller.
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> &&loop)
+ : StateFeedbackLoop<3, 1, 1>(::std::move(loop)),
+ voltage_(0.0),
+ last_voltage_(0.0),
+ uncapped_voltage_(0.0),
+ offset_(0.0),
+ max_voltage_(12.0),
+ capped_goal_(false) {}
+
+ const static int kZeroingMaxVoltage = 5;
+
+ virtual void CapU();
+
+ // Returns the accumulated voltage.
+ double voltage() const { return voltage_; }
+
+ // Returns the uncapped voltage.
+ double uncapped_voltage() const { return uncapped_voltage_; }
+
+ // Zeros the accumulator.
+ void ZeroPower() { voltage_ = 0.0; }
+
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double encoder_val, double known_position);
+
+ double offset() const { return offset_; }
+
+ double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
+ double absolute_velocity() const { return X_hat(1, 0); }
+
+ void CorrectPosition(double position) {
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << position + offset_ - kPositionOffset;
+ Correct(Y);
+ }
+
+ // Recomputes the power goal for the current controller and position/velocity.
+ void RecalculatePowerGoal();
+
+ double goal_position() const { return R(0, 0) + kPositionOffset; }
+ double goal_velocity() const { return R(1, 0); }
+ void InitializeState(double position) {
+ mutable_X_hat(0, 0) = position - kPositionOffset;
+ mutable_X_hat(1, 0) = 0.0;
+ mutable_X_hat(2, 0) = 0.0;
+ }
+
+ void SetGoalPosition(double desired_position, double desired_velocity) {
+ LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position,
+ desired_velocity);
+
+ mutable_R() << desired_position - kPositionOffset, desired_velocity,
+ (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
+ A(1, 1) / A(1, 2) * desired_velocity);
+ }
+
+ double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
+
+ void set_max_voltage(double max_voltage) { max_voltage_ = max_voltage; }
+ bool capped_goal() const { return capped_goal_; }
+
+ void CapGoal();
+
+ // Friend the test classes for acces to the internal state.
+ friend class testing::ShooterTest_RezeroWhileUnloading_Test;
+
+ private:
+ // The offset between what is '0' (0 rate on the spring) and the 0 (all the
+ // way cocked).
+ constexpr static double kPositionOffset = kMaxExtension;
+ // The accumulated voltage to apply to the motor.
+ double voltage_;
+ double last_voltage_;
+ double uncapped_voltage_;
+ double offset_;
+ double max_voltage_;
+ bool capped_goal_;
+};
+
+const Time kUnloadTimeout = Time::InSeconds(10);
+const Time kLoadTimeout = Time::InSeconds(2);
+const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
+const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
+// Time to wait after releasing the latch piston before winching back again.
+const Time kShotEndTimeout = Time::InSeconds(0.2);
+const Time kPrepareFireEndTime = Time::InMS(40);
+
+class ShooterMotor
+ : public aos::controls::ControlLoop<control_loops::ShooterGroup> {
+ public:
+ explicit ShooterMotor(control_loops::ShooterGroup *my_shooter =
+ &control_loops::shooter_queue_group);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return shooter_.capped_goal(); }
+
+ double PowerToPosition(double power);
+ double PositionToPower(double position);
+ void CheckCalibrations(const control_loops::ShooterGroup::Position *position);
+
+ typedef enum {
+ STATE_INITIALIZE = 0,
+ STATE_REQUEST_LOAD = 1,
+ STATE_LOAD_BACKTRACK = 2,
+ STATE_LOAD = 3,
+ STATE_LOADING_PROBLEM = 4,
+ STATE_PREPARE_SHOT = 5,
+ STATE_READY = 6,
+ STATE_FIRE = 8,
+ STATE_UNLOAD = 9,
+ STATE_UNLOAD_MOVE = 10,
+ STATE_READY_UNLOAD = 11,
+ STATE_ESTOP = 12
+ } State;
+
+ State state() { return state_; }
+
+ protected:
+ virtual void RunIteration(
+ const ShooterGroup::Goal *goal,
+ const control_loops::ShooterGroup::Position *position,
+ ShooterGroup::Output *output, ShooterGroup::Status *status);
+
+ private:
+ // We have to override this to keep the pistons in the correct positions.
+ virtual void ZeroOutputs();
+
+ // Friend the test classes for acces to the internal state.
+ friend class testing::ShooterTest_UnloadWindupPositive_Test;
+ friend class testing::ShooterTest_UnloadWindupNegative_Test;
+ friend class testing::ShooterTest_RezeroWhileUnloading_Test;
+
+ // Enter state STATE_UNLOAD
+ void Unload() {
+ state_ = STATE_UNLOAD;
+ unload_timeout_ = Time::Now() + kUnloadTimeout;
+ }
+ // Enter state STATE_LOAD
+ void Load() {
+ state_ = STATE_LOAD;
+ load_timeout_ = Time::Now() + kLoadTimeout;
+ }
+
+ control_loops::ShooterGroup::Position last_position_;
+
+ ZeroedStateFeedbackLoop shooter_;
+
+ // state machine state
+ State state_;
+
+ // time to giving up on loading problem
+ Time loading_problem_end_time_;
+
+ // The end time when loading for it to timeout.
+ Time load_timeout_;
+
+ // wait for brake to set
+ Time shooter_brake_set_time_;
+
+ // The timeout for unloading.
+ Time unload_timeout_;
+
+ // time that shot must have completed
+ Time shot_end_time_;
+
+ // track cycles that we are stuck to detect errors
+ int cycles_not_moved_;
+
+ double firing_starting_position_;
+
+ // True if the latch should be engaged and the brake should be engaged.
+ bool latch_piston_;
+ bool brake_piston_;
+ int32_t last_distal_posedge_count_;
+ int32_t last_proximal_posedge_count_;
+ uint32_t shot_count_;
+ bool zeroed_;
+ int distal_posedge_validation_cycles_left_;
+ int proximal_posedge_validation_cycles_left_;
+ bool last_distal_current_;
+ bool last_proximal_current_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2014_CONTROL_LOOPS_shooter_shooter_H_
diff --git a/y2014/control_loops/shooter/shooter.q b/y2014/control_loops/shooter/shooter.q
new file mode 100644
index 0000000..bc83ff7
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter.q
@@ -0,0 +1,100 @@
+package frc971.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group ShooterGroup {
+ implements aos.control_loops.ControlLoop;
+
+ message Output {
+ double voltage;
+ // true: latch engaged, false: latch open
+ bool latch_piston;
+ // true: brake engaged false: brake released
+ bool brake_piston;
+ };
+ message Goal {
+ // Shot power in joules.
+ double shot_power;
+ // Shoots as soon as this is true.
+ bool shot_requested;
+ bool unload_requested;
+ bool load_requested;
+ };
+
+ // Back is when the springs are all the way stretched.
+ message Position {
+ // In meters, out is positive.
+ double position;
+
+ // If the latch piston is fired and this hall effect has been triggered, the
+ // plunger is all the way back and latched.
+ bool plunger;
+ // Gets triggered when the pusher is all the way back.
+ PosedgeOnlyCountedHallEffectStruct pusher_distal;
+ // Triggers just before pusher_distal.
+ PosedgeOnlyCountedHallEffectStruct pusher_proximal;
+ // Triggers when the latch engages.
+ bool latch;
+ };
+ message Status {
+ // Whether it's ready to shoot right now.
+ bool ready;
+ // Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
+ bool cocked;
+ // How many times we've shot.
+ int32_t shots;
+ bool done;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ double hard_stop_power;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group ShooterGroup shooter_queue_group;
+
+struct ShooterStateToLog {
+ double absolute_position;
+ double absolute_velocity;
+ uint32_t state;
+ bool latch_sensor;
+ bool proximal;
+ bool distal;
+ bool plunger;
+ bool brake;
+ bool latch_piston;
+};
+
+struct ShooterVoltageToLog {
+ double X_hat;
+ double applied;
+};
+
+struct ShooterMovingGoal {
+ double dx;
+};
+
+struct ShooterChangeCalibration {
+ double encoder;
+ double real_position;
+ double old_position;
+ double new_position;
+ double old_offset;
+ double new_offset;
+};
+
+struct ShooterStatusToLog {
+ double goal;
+ double position;
+};
+
+struct PowerAdjustment {
+ double requested_power;
+ double actual_power;
+};
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..a65e56f
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,723 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/network/team_number.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter.h"
+#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
+#include "y2014/constants.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+static const int kTestTeam = 1;
+
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ // Override this to define how to set up the environment.
+ virtual void SetUp() { aos::network::OverrideTeamNumber(kTestTeam); }
+};
+
+::testing::Environment *const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
+
+
+// Class which simulates the shooter and sends out queue messages containing the
+// position.
+class ShooterSimulation {
+ public:
+ // Constructs a motor simulation.
+ ShooterSimulation(double initial_position)
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
+ latch_piston_state_(false),
+ latch_delay_count_(0),
+ plunger_latched_(false),
+ brake_piston_state_(true),
+ brake_delay_count_(0),
+ shooter_queue_group_(
+ ".frc971.control_loops.shooter_queue_group", 0xcbf22ba9,
+ ".frc971.control_loops.shooter_queue_group.goal",
+ ".frc971.control_loops.shooter_queue_group.position",
+ ".frc971.control_loops.shooter_queue_group.output",
+ ".frc971.control_loops.shooter_queue_group.status") {
+ Reinitialize(initial_position);
+ }
+
+ // The difference between the position with 0 at the back, and the position
+ // with 0 measured where the spring has 0 force.
+ constexpr static double kPositionOffset = kMaxExtension;
+
+ void Reinitialize(double initial_position) {
+ LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
+ StateFeedbackPlant<2, 1, 1> *plant = shooter_plant_.get();
+ initial_position_ = initial_position;
+ plant->mutable_X(0, 0) = initial_position_ - kPositionOffset;
+ plant->mutable_X(1, 0) = 0.0;
+ plant->mutable_Y() = plant->C() * plant->X();
+ last_voltage_ = 0.0;
+ last_plant_position_ = 0.0;
+ SetPhysicalSensors(&last_position_message_);
+ }
+
+ // Returns the absolute angle of the shooter.
+ double GetAbsolutePosition() const {
+ return shooter_plant_->Y(0, 0) + kPositionOffset;
+ }
+
+ // Returns the adjusted angle of the shooter.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Makes sure pos is inside range (inclusive)
+ bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
+ return (pos >= pair.lower_angle && pos <= pair.upper_angle);
+ }
+
+ // Sets the values of the physical sensors that can be directly observed
+ // (encoder, hall effect).
+ void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
+ const frc971::constants::Values &values = constants::GetValues();
+
+ position->position = GetPosition();
+
+ LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ if (plunger_latched_) {
+ position->plunger = true;
+ // Only disengage the spring if we are greater than 0, which is where the
+ // latch will take the load off the pusher.
+ if (GetAbsolutePosition() > 0.0) {
+ shooter_plant_->set_plant_index(1);
+ } else {
+ shooter_plant_->set_plant_index(0);
+ }
+ } else {
+ shooter_plant_->set_plant_index(0);
+ position->plunger =
+ CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
+ }
+ position->pusher_distal.current =
+ CheckRange(GetAbsolutePosition(), values.shooter.pusher_distal);
+ position->pusher_proximal.current =
+ CheckRange(GetAbsolutePosition(), values.shooter.pusher_proximal);
+ }
+
+ void UpdateEffectEdge(
+ PosedgeOnlyCountedHallEffectStruct *sensor,
+ const PosedgeOnlyCountedHallEffectStruct &last_sensor,
+ const constants::Values::AnglePair &limits,
+ const control_loops::ShooterGroup::Position &last_position) {
+ sensor->posedge_count = last_sensor.posedge_count;
+ sensor->negedge_count = last_sensor.negedge_count;
+
+ sensor->posedge_value = last_sensor.posedge_value;
+
+ if (sensor->current && !last_sensor.current) {
+ ++sensor->posedge_count;
+ if (last_position.position + initial_position_ < limits.lower_angle) {
+ LOG(DEBUG, "Posedge value on lower edge of sensor, count is now %d\n",
+ sensor->posedge_count);
+ sensor->posedge_value = limits.lower_angle - initial_position_;
+ } else {
+ LOG(DEBUG, "Posedge value on upper edge of sensor, count is now %d\n",
+ sensor->posedge_count);
+ sensor->posedge_value = limits.upper_angle - initial_position_;
+ }
+ }
+ if (!sensor->current && last_sensor.current) {
+ ++sensor->negedge_count;
+ }
+ }
+
+ void SendPositionMessage() {
+ // the first bool is false
+ SendPositionMessage(false, false, false, false);
+ }
+
+ // Sends out the position queue messages.
+ // if the first bool is false then this is
+ // just the default state, otherwise will force
+ // it into a state using the passed values
+ void SendPositionMessage(bool use_passed, bool plunger_in,
+ bool latch_in, bool brake_in) {
+ const frc971::constants::Values &values = constants::GetValues();
+ ::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
+ shooter_queue_group_.position.MakeMessage();
+
+ if (use_passed) {
+ plunger_latched_ = latch_in && plunger_in;
+ latch_piston_state_ = plunger_latched_;
+ brake_piston_state_ = brake_in;
+ }
+
+ SetPhysicalSensors(position.get());
+
+ position->latch = latch_piston_state_;
+
+ // Handle pusher distal hall effect
+ UpdateEffectEdge(&position->pusher_distal,
+ last_position_message_.pusher_distal,
+ values.shooter.pusher_distal, last_position_message_);
+
+ // Handle pusher proximal hall effect
+ UpdateEffectEdge(&position->pusher_proximal,
+ last_position_message_.pusher_proximal,
+ values.shooter.pusher_proximal, last_position_message_);
+
+ last_position_message_ = *position;
+ position.Send();
+ }
+
+ // Simulates the claw moving for one timestep.
+ void Simulate() {
+ last_plant_position_ = GetAbsolutePosition();
+ EXPECT_TRUE(shooter_queue_group_.output.FetchLatest());
+ if (shooter_queue_group_.output->latch_piston && !latch_piston_state_ &&
+ latch_delay_count_ <= 0) {
+ ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
+ latch_delay_count_ = 6;
+ } else if (!shooter_queue_group_.output->latch_piston &&
+ latch_piston_state_ && latch_delay_count_ >= 0) {
+ ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
+ latch_delay_count_ = -6;
+ }
+
+ if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
+ brake_delay_count_ <= 0) {
+ ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
+ brake_delay_count_ = 5;
+ } else if (!shooter_queue_group_.output->brake_piston &&
+ brake_piston_state_ && brake_delay_count_ >= 0) {
+ ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
+ brake_delay_count_ = -5;
+ }
+
+ // Handle brake internal state
+ if (!brake_piston_state_ && brake_delay_count_ > 0) {
+ if (brake_delay_count_ == 1) {
+ brake_piston_state_ = true;
+ }
+ brake_delay_count_--;
+ } else if (brake_piston_state_ && brake_delay_count_ < 0) {
+ if (brake_delay_count_ == -1) {
+ brake_piston_state_ = false;
+ }
+ brake_delay_count_++;
+ }
+
+ if (brake_piston_state_) {
+ shooter_plant_->mutable_U() << 0.0;
+ shooter_plant_->mutable_X(1, 0) = 0.0;
+ shooter_plant_->mutable_Y() = shooter_plant_->C() * shooter_plant_->X() +
+ shooter_plant_->D() * shooter_plant_->U();
+ } else {
+ shooter_plant_->mutable_U() << last_voltage_;
+ //shooter_plant_->U << shooter_queue_group_.output->voltage;
+ shooter_plant_->Update();
+ }
+ LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
+
+ // Handle latch hall effect
+ if (!latch_piston_state_ && latch_delay_count_ > 0) {
+ LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
+ if (latch_delay_count_ == 1) {
+ latch_piston_state_ = true;
+ EXPECT_GE(constants::GetValues().shooter.latch_max_safe_position,
+ GetAbsolutePosition());
+ plunger_latched_ = true;
+ }
+ latch_delay_count_--;
+ } else if (latch_piston_state_ && latch_delay_count_ < 0) {
+ LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
+ if (latch_delay_count_ == -1) {
+ latch_piston_state_ = false;
+ if (GetAbsolutePosition() > 0.002) {
+ EXPECT_TRUE(brake_piston_state_) << "Must have the brake set when "
+ "releasing the latch for "
+ "powerful shots.";
+ }
+ plunger_latched_ = false;
+ // TODO(austin): The brake should be set for a number of cycles after
+ // this as well.
+ shooter_plant_->mutable_X(0, 0) += 0.005;
+ }
+ latch_delay_count_++;
+ }
+
+ EXPECT_GE(constants::GetValues().shooter.upper_hard_limit,
+ GetAbsolutePosition());
+ EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
+ GetAbsolutePosition());
+
+ last_voltage_ = shooter_queue_group_.output->voltage;
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
+ }
+
+ // pointer to plant
+ const ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+ // true latch closed
+ bool latch_piston_state_;
+ // greater than zero, delaying close. less than zero delaying open
+ int latch_delay_count_;
+
+ // Goes to true after latch_delay_count_ hits 0 while the plunger is back.
+ bool plunger_latched_;
+
+ // true brake locked
+ bool brake_piston_state_;
+ // greater than zero, delaying close. less than zero delaying open
+ int brake_delay_count_;
+
+ private:
+ ShooterGroup shooter_queue_group_;
+ double initial_position_;
+ double last_voltage_;
+
+ control_loops::ShooterGroup::Position last_position_message_;
+ double last_plant_position_;
+};
+
+class ShooterTest : public ::aos::testing::ControlLoopTest {
+
+ protected:
+ // 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.
+ ShooterGroup shooter_queue_group_;
+
+ // Create a loop and simulation plant.
+ ShooterMotor shooter_motor_;
+ ShooterSimulation shooter_motor_plant_;
+
+ void Reinitialize(double position) {
+ shooter_motor_plant_.Reinitialize(position);
+ }
+
+ ShooterTest()
+ : shooter_queue_group_(
+ ".frc971.control_loops.shooter_queue_group", 0xcbf22ba9,
+ ".frc971.control_loops.shooter_queue_group.goal",
+ ".frc971.control_loops.shooter_queue_group.position",
+ ".frc971.control_loops.shooter_queue_group.output",
+ ".frc971.control_loops.shooter_queue_group.status"),
+ shooter_motor_(&shooter_queue_group_),
+ shooter_motor_plant_(0.2) {
+ }
+
+ void VerifyNearGoal() {
+ shooter_queue_group_.goal.FetchLatest();
+ shooter_queue_group_.position.FetchLatest();
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 1e-4);
+ }
+};
+
+TEST_F(ShooterTest, PowerConversion) {
+ const frc971::constants::Values &values = constants::GetValues();
+ // test a couple of values return the right thing
+ EXPECT_NEAR(0.254001, shooter_motor_.PowerToPosition(140.0), 0.00001);
+ EXPECT_NEAR(0.00058, shooter_motor_.PowerToPosition(0.53), 0.00001);
+ EXPECT_NEAR(0.095251238129837101, shooter_motor_.PowerToPosition(73.67),
+ 0.00001);
+
+ // value too large should get max
+ EXPECT_NEAR(values.shooter.upper_limit,
+ shooter_motor_.PowerToPosition(505050.99), 0.00001);
+ // negative values should zero
+ EXPECT_NEAR(0, shooter_motor_.PowerToPosition(-123.4), 0.00001);
+}
+
+// Test that PowerToPosition and PositionToPower are inverses of each other.
+// Note that PowerToPosition will cap position whereas PositionToPower will not
+// cap power.
+TEST_F(ShooterTest, InversePowerConversion) {
+ // Test a few values.
+ double power = 140.0;
+ double position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = .53;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = 71.971;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, GoesToValue) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, Fire) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 120; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(35.0)
+ .shot_requested(true)
+ .Send();
+
+ bool hit_fire = false;
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
+ if (!hit_fire) {
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(17.0)
+ .shot_requested(false)
+ .Send();
+ }
+ hit_fire = true;
+ }
+ }
+
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ EXPECT_TRUE(hit_fire);
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, FireLong) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
+
+ bool hit_fire = false;
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
+ if (!hit_fire) {
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_requested(false)
+ .Send();
+ }
+ hit_fire = true;
+ }
+ }
+
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ EXPECT_TRUE(hit_fire);
+}
+
+// Verifies that it doesn't try to go out too far if you give it a ridicilous
+// power.
+TEST_F(ShooterTest, LoadTooFar) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(500.0).Send();
+ for (int i = 0; i < 160; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ EXPECT_LT(
+ shooter_motor_plant_.GetAbsolutePosition(),
+ constants::GetValuesForTeam(kTestTeam).shooter.upper_limit);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, MoveGoal) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(14.0).Send();
+
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+
+TEST_F(ShooterTest, Unload) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.015);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+}
+
+// Tests that it rezeros while unloading.
+TEST_F(ShooterTest, RezeroWhileUnloading) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+
+ shooter_motor_.shooter_.offset_ += 0.01;
+ for (int i = 0; i < 50; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.015);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, UnloadWindupNegative) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ int kicked_delay = 20;
+ int capped_goal_count = 0;
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
+ LOG(DEBUG, "State is UnloadMove\n");
+ --kicked_delay;
+ if (kicked_delay == 0) {
+ shooter_motor_.shooter_.mutable_R(0, 0) -= 100;
+ }
+ }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+ EXPECT_LE(1, capped_goal_count);
+ EXPECT_GE(3, capped_goal_count);
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, UnloadWindupPositive) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ int kicked_delay = 20;
+ int capped_goal_count = 0;
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
+ LOG(DEBUG, "State is UnloadMove\n");
+ --kicked_delay;
+ if (kicked_delay == 0) {
+ shooter_motor_.shooter_.mutable_R(0, 0) += 0.1;
+ }
+ }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+ EXPECT_LE(1, capped_goal_count);
+ EXPECT_GE(3, capped_goal_count);
+}
+
+double HallEffectMiddle(constants::Values::AnglePair pair) {
+ return (pair.lower_angle + pair.upper_angle) / 2.0;
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, StartsOnDistal) {
+ Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+
+// Tests that the shooter zeros correctly and goes to a position.
+TEST_F(ShooterTest, StartsOnProximal) {
+ Reinitialize(
+ HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 300; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+class ShooterZeroingTest : public ShooterTest,
+ public ::testing::WithParamInterface<
+ ::std::tr1::tuple<bool, bool, bool, double>> {};
+
+TEST_P(ShooterZeroingTest, AllDisparateStartingZero) {
+ bool latch = ::std::tr1::get<0>(GetParam());
+ bool brake = ::std::tr1::get<1>(GetParam());
+ bool plunger_back = ::std::tr1::get<2>(GetParam());
+ double start_pos = ::std::tr1::get<3>(GetParam());
+ // flag to initialize test
+ //printf("@@@@ l= %d b= %d p= %d s= %.3f\n",
+ // latch, brake, plunger_back, start_pos);
+ bool initialized = false;
+ Reinitialize(start_pos);
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(120.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
+ initialized = true;
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+INSTANTIATE_TEST_CASE_P(
+ ShooterZeroingTest, ShooterZeroingTest,
+ ::testing::Combine(
+ ::testing::Bool(), ::testing::Bool(), ::testing::Bool(),
+ ::testing::Values(
+ 0.05,
+ constants::GetValuesForTeam(kTestTeam).shooter.upper_limit - 0.05,
+ HallEffectMiddle(constants::GetValuesForTeam(kTestTeam)
+ .shooter.pusher_proximal),
+ HallEffectMiddle(constants::GetValuesForTeam(kTestTeam)
+ .shooter.pusher_distal),
+ constants::GetValuesForTeam(kTestTeam)
+ .shooter.latch_max_safe_position -
+ 0.001)));
+
+// TODO(austin): Slip the encoder somewhere.
+
+// TODO(austin): Test all the timeouts...
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/shooter/shooter_main.cc b/y2014/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..31b24bf
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "y2014/control_loops/shooter/shooter.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ShooterMotor shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2014/control_loops/shooter/shooter_motor_plant.cc b/y2014/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..7da6407
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,77 @@
+#include "y2014/control_loops/shooter/shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 0.999391114909, 0.00811316740387, 7.59766686183e-05, -0.113584343654, 0.64780421498, 0.0141730519709, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00811505488455, 7.59852687598e-05, 0.0, 0.648331305446, 0.0141763492481, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeSprungShooterController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.64719532989, 57.0572680832, 636.74290365;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 450.571849185, 11.8404918938, 0.997195329889;
+ Eigen::Matrix<double, 3, 3> A_inv;
+ A_inv << 0.99918700445, -0.0125139220268, 0.00010144556732, 0.175194908375, 1.54148211958, -0.0218608169185, 0.0, 0.0, 1.0;
+ return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeSprungShooterPlantCoefficients());
+}
+
+StateFeedbackController<3, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.64833130545, 57.2417604572, 636.668851906;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 349.173113146, 8.65077618169, 0.848331305446;
+ Eigen::Matrix<double, 3, 3> A_inv;
+ A_inv << 1.0, -0.0125168333171, 0.000101457731824, 0.0, 1.5424212769, -0.021865902709, 0.0, 0.0, 1.0;
+ return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeShooterPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>> plants(2);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeSprungShooterPlantCoefficients()));
+ plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeShooterPlantCoefficients()));
+ return StateFeedbackPlant<3, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeShooterLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<3, 1, 1>>> controllers(2);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeSprungShooterController()));
+ controllers[1] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeShooterController()));
+ return StateFeedbackLoop<3, 1, 1>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/shooter/shooter_motor_plant.h b/y2014/control_loops/shooter/shooter_motor_plant.h
new file mode 100644
index 0000000..45d6ed1
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,28 @@
+#ifndef Y2014_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define Y2014_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+static constexpr double kMaxExtension = 0.323850;
+
+static constexpr double kSpringConstant = 2800.000000;
+
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeSprungShooterController();
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeShooterController();
+
+StateFeedbackPlant<3, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeShooterLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2014_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.cc
new file mode 100644
index 0000000..8311948
--- /dev/null
+++ b/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -0,0 +1,77 @@
+#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 0.999391114909, 0.00811316740387, -0.113584343654, 0.64780421498;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 7.59766686183e-05, 0.0141730519709;
+ 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 StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00811505488455, 0.0, 0.648331305446;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 7.59852687598e-05, 0.0141763492481;
+ 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 StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawSprungShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.54719532989, 43.9345489758;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2126.06977433, 41.3223370936;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 0.99918700445, -0.0125139220268, 0.175194908375, 1.54148211958;
+ return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeRawSprungShooterPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeRawShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.54833130545, 44.1155797675;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2133.83569145, 41.3499425476;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.0, -0.0125168333171, 0.0, 1.5424212769;
+ return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeRawShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawShooterPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(2);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawSprungShooterPlantCoefficients()));
+ plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawShooterPlantCoefficients()));
+ return StateFeedbackPlant<2, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawShooterLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(2);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeRawSprungShooterController()));
+ controllers[1] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeRawShooterController()));
+ return StateFeedbackLoop<2, 1, 1>(&controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h b/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h
new file mode 100644
index 0000000..5b7de85
--- /dev/null
+++ b/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h
@@ -0,0 +1,24 @@
+#ifndef Y2014_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_
+#define Y2014_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawSprungShooterController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawShooterController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawShooterPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawShooterLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2014_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_