Import shooter from 2013
Taken from commit a085044
I liberally delete some things in the Python code to get
things compiling. I also added TODOs where I am unsure whether
my changes are appropriate.
Change-Id: I4f3ae9b69ca9c2b1917e73136c766275a44d3665
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..41e6f81
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -0,0 +1,138 @@
+#include "y2016/control_loops/shooter/shooter.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2016/control_loops/shooter/shooter_plant.h"
+
+namespace y2016 {
+namespace control_loops {
+
+Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
+ : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter),
+ loop_(new StateFeedbackLoop<2, 1, 1>(
+ ::y2016::control_loops::shooter::MakeShooterLoop())),
+ history_position_(0),
+ position_goal_(0.0),
+ last_position_(0.0),
+ last_velocity_goal_(0) {
+ memset(history_, 0, sizeof(history_));
+}
+
+/*static*/ const double Shooter::dt = 0.005;
+/*static*/ const double Shooter::kMaxSpeed =
+ 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+
+void Shooter::RunIteration(
+ const control_loops::ShooterQueue::Goal *goal,
+ const control_loops::ShooterQueue::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterQueue::Status *status) {
+ double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ const double current_position =
+ (position == NULL ? loop_->X_hat(0, 0) : position->position);
+ double output_voltage = 0.0;
+
+ // TODO(phil): Set a queue to trigger a shot. For now, disable the fetch from
+ // the queue.
+ if (false) {
+ // if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ // if (index_loop.status->is_shooting) {
+ if (velocity_goal != last_velocity_goal_ && velocity_goal < 130) {
+ velocity_goal = last_velocity_goal_;
+ }
+ } else {
+ LOG(WARNING, "assuming index isn't shooting\n");
+ }
+ last_velocity_goal_ = velocity_goal;
+
+ // Track the current position if the velocity goal is small.
+ if (velocity_goal <= 1.0) {
+ position_goal_ = current_position;
+ }
+
+ // TODO(phil): Does this change make sense?
+ // loop_->Y << current_position;
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << current_position;
+ loop_->Correct(Y);
+
+ // 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_->U_max(0, 0) -
+ kVelocityWeightScalar * (velocity_goal - loop_->X_hat(1, 0)) *
+ loop_->K(0, 1)) /
+ loop_->K(0, 0) +
+ loop_->X_hat(0, 0);
+ const double min_reference =
+ (loop_->U_min(0, 0) -
+ kVelocityWeightScalar * (velocity_goal - loop_->X_hat(1, 0)) *
+ loop_->K(0, 1)) /
+ loop_->K(0, 0) +
+ loop_->X_hat(0, 0);
+
+ position_goal_ =
+ ::std::max(::std::min(position_goal_, max_reference), min_reference);
+ // TODO(phil): Does this change make sense?
+ // loop_->R << position_goal_, velocity_goal;
+ loop_->mutable_R(0, 0) = position_goal_;
+ loop_->mutable_R(1, 0) = velocity_goal;
+ position_goal_ += velocity_goal * dt;
+
+ // TODO(phil): Does this change make sense?
+ // loop_->Update(position, output == NULL);
+ loop_->Update(output == NULL);
+
+ // Kill power at low velocity goals.
+ if (velocity_goal < 1.0) {
+ loop_->mutable_U(0, 0) = 0.0;
+ } else {
+ output_voltage = loop_->U(0, 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,
+ // TODO(phil): Does this change make sense?
+ // loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+ loop_->X_hat(0, 0), loop_->X_hat(1, 0), loop_->R(0, 0), loop_->R(1, 0));
+
+ // 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_]) / dt /
+ (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 y2016