Modify shooter control loop to take velocity.
- NOTE: Things compile and run, but the output never actually stabilizes.
Before this can be used, new motor_plant constants for the shooter MUST
be determined.
diff --git a/bot3/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
index fbfeafb..07ce397 100644
--- a/bot3/control_loops/shooter/shooter.cc
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -10,12 +10,9 @@
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),
+ loop_(new StateFeedbackLoop<1, 1, 1>(MakeShooterLoop())),
last_velocity_goal_(0) {
- memset(history_, 0, sizeof(history_));
+ loop_->Reset();
}
/*static*/ const double ShooterMotor::dt = 0.01;
@@ -26,10 +23,12 @@
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status) {
double velocity_goal = goal->velocity;
- const double current_position =
+ // Our position here is actually a velocity.
+ average_velocity_ =
(position == NULL ? loop_->X_hat(0, 0) : position->position);
double output_voltage = 0.0;
+// TODO (danielp): This must be modified for our index.
/* if (index_loop.status.FetchLatest() || index_loop.status.get()) {
if (index_loop.status->is_shooting) {
if (velocity_goal != last_velocity_goal_ &&
@@ -42,57 +41,21 @@
}*/
last_velocity_goal_ = velocity_goal;
- // 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_->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);
- loop_->R << position_goal_, velocity_goal;
- position_goal_ += velocity_goal * dt;
+ loop_->Y << average_velocity_;
+ loop_->R << velocity_goal;
loop_->Update(position, output == NULL);
// Kill power at low velocity goals.
if (velocity_goal < 1.0) {
- loop_->U[0] = 0.0;
+ loop_->U(0) = 0.0;
} else {
- output_voltage = loop_->U[0];
+ 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);
+ "PWM: %f, raw_velocity: %f, xhat[0]: %f, R[0]: %f\n",
+ output_voltage, average_velocity_, loop_->X_hat[0], loop_->R[0]);
status->average_velocity = average_velocity_;
@@ -107,8 +70,6 @@
}
LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
- last_position_ = current_position;
-
if (output) {
output->voltage = output_voltage;
}