Designed a shooter loop using voltage error estimation.
This gets rid of all the corner cases with the moving position style
loop from before, and doesn't have overshoot issues like standard
integral loops have.
Change-Id: I4e4eb1767038563cf851040ce8218e73ca60904a
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 9a46906..6550fc2 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -9,129 +9,92 @@
namespace y2016 {
namespace control_loops {
+namespace shooter {
+
+// TODO(austin): Pseudo current limit?
ShooterSide::ShooterSide()
- : loop_(new StateFeedbackLoop<2, 1, 1>(
- ::y2016::control_loops::shooter::MakeShooterLoop())) {
- memset(history_, 0, sizeof(history_));
+ : loop_(new StateFeedbackLoop<3, 1, 1>(MakeIntegralShooterLoop())) {
+ history_.fill(0);
+ Y_.setZero();
}
-void ShooterSide::SetGoal(double angular_velocity_goal_uncapped) {
- angular_velocity_goal_ = std::min(angular_velocity_goal_uncapped,
- kMaxSpeed);
+void ShooterSide::set_goal(double angular_velocity_goal) {
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
}
-void ShooterSide::EstimatePositionTimestep() {
- // NULL position, so look at the loop.
- SetPosition(loop_->X_hat(0, 0));
-}
-
-void ShooterSide::SetPosition(double current_position) {
- current_position_ = current_position;
-
- // Track the current position if the velocity goal is small.
- if (angular_velocity_goal_ <= 1.0) position_goal_ = current_position_;
-
+void ShooterSide::set_position(double current_position) {
// Update position in the model.
- Eigen::Matrix<double, 1, 1> Y;
- Y << current_position_;
- loop_->Correct(Y);
-
- // Prevents integral windup by limiting the position error such that the
- // error can't produce much more than full power.
- const double max_reference =
- (loop_->U_max(0, 0) -
- 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) -
- 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);
-
- loop_->mutable_R() << position_goal_, angular_velocity_goal_;
- position_goal_ +=
- angular_velocity_goal_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ Y_ << current_position;
// Add the position to the history.
- history_[history_position_] = current_position_;
+ history_[history_position_] = current_position;
history_position_ = (history_position_ + 1) % kHistoryLength;
}
-const ShooterStatus ShooterSide::GetStatus() {
- // Calculate average over dt * kHistoryLength.
- int old_history_position =
- ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
- double avg_angular_velocity =
- (history_[old_history_position] - history_[history_position_]) /
- ::aos::controls::kLoopFrequency.ToSeconds() /
- static_cast<double>(kHistoryLength - 1);
-
- // 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);
-
- return {avg_angular_velocity, ready};
-}
-
-double ShooterSide::GetOutput() {
- if (angular_velocity_goal_ < 1.0) {
- // Kill power at low angular velocities.
- return 0.0;
- }
-
+double ShooterSide::voltage() const {
return loop_->U(0, 0);
}
-void ShooterSide::UpdateLoop(bool output_is_null) {
- loop_->Update(output_is_null);
+void ShooterSide::Update(bool disabled) {
+ loop_->mutable_R() = loop_->next_R();
+ if (loop_->R(1, 0) < 1.0) {
+ // Kill power at low angular velocities.
+ disabled = true;
+ }
+
+ loop_->Correct(Y_);
+ loop_->Update(disabled);
}
-Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
- : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter) {}
+void ShooterSide::SetStatus(ShooterSideStatus *status) {
+ // Compute the oldest point in the history.
+ const int oldest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
-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) {
+ // Compute the distance moved over that time period.
+ status->avg_angular_velocity =
+ (history_[oldest_history_position] - history_[history_position_]) /
+ (::aos::controls::kLoopFrequency.ToSeconds() *
+ static_cast<double>(kHistoryLength - 1));
+
+ status->angular_velocity = loop_->X_hat(1, 0);
+
+ // Ready if average angular velocity is close to the goal.
+ status->ready = (std::abs(loop_->next_R(1, 0) -
+ status->avg_angular_velocity) < kTolerance &&
+ loop_->next_R(1, 0) > 1.0);
+}
+
+Shooter::Shooter(ShooterQueue *my_shooter)
+ : aos::controls::ControlLoop<ShooterQueue>(my_shooter) {}
+
+void Shooter::RunIteration(const ShooterQueue::Goal *goal,
+ const ShooterQueue::Position *position,
+ ShooterQueue::Output *output,
+ 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);
- }
+ left_.set_goal(goal->angular_velocity);
+ right_.set_goal(goal->angular_velocity);
}
- 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;
+ left_.set_position(position->theta_left);
+ right_.set_position(position->theta_right);
- status->ready_left = status_left.ready;
- status->ready_right = status_right.ready;
- status->ready_both = (status_left.ready && status_right.ready);
+ left_.Update(output == nullptr);
+ right_.Update(output == nullptr);
- left_.UpdateLoop(output == nullptr);
- right_.UpdateLoop(output == nullptr);
+ left_.SetStatus(&status->left);
+ right_.SetStatus(&status->right);
+ status->ready = (status->left.ready && status->right.ready);
if (output) {
- output->voltage_left = left_.GetOutput();
- output->voltage_right = right_.GetOutput();
+ output->voltage_left = left_.voltage();
+ output->voltage_right = right_.voltage();
}
}
+} // namespace shooter
} // namespace control_loops
} // namespace y2016