Make dual shooter.
Abstracted some stuff to get two shooters running in
the same process.
Change-Id: Id7617d6cf0d90c28fe2c07aefd3896eb592575dc
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 41e6f81..9a46906 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -6,131 +6,130 @@
#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) {
+ShooterSide::ShooterSide()
+ : loop_(new StateFeedbackLoop<2, 1, 1>(
+ ::y2016::control_loops::shooter::MakeShooterLoop())) {
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 ShooterSide::SetGoal(double angular_velocity_goal_uncapped) {
+ angular_velocity_goal_ = std::min(angular_velocity_goal_uncapped,
+ kMaxSpeed);
+}
-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;
+void ShooterSide::EstimatePositionTimestep() {
+ // NULL position, so look at the loop.
+ SetPosition(loop_->X_hat(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;
+void ShooterSide::SetPosition(double current_position) {
+ current_position_ = current_position;
// Track the current position if the velocity goal is small.
- if (velocity_goal <= 1.0) {
- position_goal_ = current_position;
- }
+ if (angular_velocity_goal_ <= 1.0) position_goal_ = current_position_;
- // TODO(phil): Does this change make sense?
- // loop_->Y << current_position;
+ // Update position in the model.
Eigen::Matrix<double, 1, 1> Y;
- Y << current_position;
+ 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)) /
+ kAngularVelocityWeightScalar *
+ (angular_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)) /
+ kAngularVelocityWeightScalar *
+ (angular_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);
+ loop_->mutable_R() << position_goal_, angular_velocity_goal_;
+ position_goal_ +=
+ angular_velocity_goal_ * ::aos::controls::kLoopFrequency.ToSeconds();
- // 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);
- }
+ // Add the position to the history.
+ history_[history_position_] = current_position_;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+}
- 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.
+const ShooterStatus ShooterSide::GetStatus() {
+ // Calculate average over dt * kHistoryLength.
int old_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
- average_velocity_ =
- (history_[old_history_position] - history_[history_position_]) / dt /
- (double)(kHistoryLength - 1);
+ double avg_angular_velocity =
+ (history_[old_history_position] - history_[history_position_]) /
+ ::aos::controls::kLoopFrequency.ToSeconds() /
+ static_cast<double>(kHistoryLength - 1);
- status->average_velocity = average_velocity_;
+ // Ready if average angular velocity is close to the goal.
+ bool ready = (std::abs(angular_velocity_goal_ - avg_angular_velocity) <
+ kTolerance &&
+ angular_velocity_goal_ != 0.0);
- // 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;
+ return {avg_angular_velocity, ready};
+}
+
+double ShooterSide::GetOutput() {
+ if (angular_velocity_goal_ < 1.0) {
+ // Kill power at low angular velocities.
+ return 0.0;
}
- LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
- last_position_ = current_position;
+ return loop_->U(0, 0);
+}
+
+void ShooterSide::UpdateLoop(bool output_is_null) {
+ loop_->Update(output_is_null);
+}
+
+Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
+ : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter) {}
+
+void Shooter::RunIteration(
+ const control_loops::ShooterQueue::Goal *goal,
+ const control_loops::ShooterQueue::Position *position,
+ control_loops::ShooterQueue::Output *output,
+ control_loops::ShooterQueue::Status *status) {
+ if (goal) {
+ // Update position/goal for our two shooter sides.
+ left_.SetGoal(goal->angular_velocity_left);
+ right_.SetGoal(goal->angular_velocity_right);
+
+ if (position == nullptr) {
+ left_.EstimatePositionTimestep();
+ right_.EstimatePositionTimestep();
+ } else {
+ left_.SetPosition(position->theta_left);
+ right_.SetPosition(position->theta_right);
+ }
+ }
+
+ ShooterStatus status_left = left_.GetStatus();
+ ShooterStatus status_right = right_.GetStatus();
+ status->avg_angular_velocity_left = status_left.avg_angular_velocity;
+ status->avg_angular_velocity_right = status_right.avg_angular_velocity;
+
+ status->ready_left = status_left.ready;
+ status->ready_right = status_right.ready;
+ status->ready_both = (status_left.ready && status_right.ready);
+
+ left_.UpdateLoop(output == nullptr);
+ right_.UpdateLoop(output == nullptr);
if (output) {
- output->voltage = output_voltage;
+ output->voltage_left = left_.GetOutput();
+ output->voltage_right = right_.GetOutput();
}
}