Updated the shooter loop to simplify it and add more tests. Fixed the poles to be close to last year.
diff --git a/frc971/control_loops/shooter.cc b/frc971/control_loops/shooter.cc
index 102032e..3cb2415 100644
--- a/frc971/control_loops/shooter.cc
+++ b/frc971/control_loops/shooter.cc
@@ -10,96 +10,84 @@
namespace frc971 {
namespace control_loops {
-namespace {
-// Motor controllers have a range of PWM values where they don't trigger,
-// and we use these values to avoid that.
-// TODO: find these values for the Talons.
-static const double positive_deadband_power = 0.0;
-static const double negative_deadband_power = 0.0;
-}
-
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),
- time_(0.0) {
- memset(history, 0, sizeof(history));
- // Creates the header for the data file. Overwrites anything already there.
- loop_->StartDataFile("shooter.csv");
+ last_position_(0.0) {
+ memset(history_, 0, sizeof(history_));
}
+/*static*/ const double ShooterMotor::dt = 0.01;
+/*static*/ const double ShooterMotor::kMaxSpeed =
+ 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+
void ShooterMotor::RunIteration(
const control_loops::ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status) {
-
- bool bad_pos = false;
- if (position == NULL) {
- LOG(WARNING, "no position given\n");
- bad_pos = true;
- }
-
- const double velocity_goal = std::min(goal->velocity, max_speed);
- static double last_position = 0.0;
+ const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ const double current_position =
+ (position == NULL ? loop_->X_hat[0] : position->position);
double output_voltage = 0.0;
- if (!bad_pos) {
- static bool first_time = true;
- if (first_time) {
- position_goal_ = position->position;
- first_time = false;
- }
-
- // Add position to the history.
- history[history_position_] = position->position;
- history_position_ = (history_position_ + 1) % kHistoryLength;
-
- loop_->Y << position->position;
- // Prevents the position from going out of the bounds
- // where the control loop will run the motor at full power.
- const double velocity_weight_scalar = 0.35;
- const double max_reference = (loop_->plant.U_max[0] - velocity_weight_scalar *
- (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
- / loop_->K[0] + loop_->X_hat[0];
- const double min_reference = (loop_->plant.U_min[0] - velocity_weight_scalar *
- (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
- / loop_->K[0] + loop_->X_hat[0];
- position_goal_ = std::max(std::min(position_goal_, max_reference), min_reference);
- loop_->R << position_goal_, velocity_goal;
- position_goal_ += velocity_goal * dt;
+ // 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_->plant.U_max[0] - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[0];
+ const double min_reference =
+ (loop_->plant.U_min[0] - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[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_->Update(!bad_pos, bad_pos || (output == NULL));
- // There is no need to make the motors actively assist the spin-down.
+ loop_->Update(position, output == NULL);
+
+ // Kill power at low velocity goals.
if (velocity_goal < 1.0) {
- output_voltage = 0.0;
- // Also, reset the position incrementer to avoid accumulating error.
- position_goal_ = position->position;
+ loop_->U[0] = 0.0;
} else {
- output_voltage = loop_->U[0] / 12.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, position->position,
- position->position / (2 * M_PI),
- (position->position - last_position) / dt,
+ 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]);
-// aos::DriverStationDisplay::Send(2, "RPS%3.0f(%3.0f) RPM%4.0f",
-// (position->position - last_position) * 100.0, goal->goal,
-// (position->position - last_position) / (2.0 * M_PI) * 6000.0);
-
- // Calculates the velocity over the last kHistoryLength*.001 seconds
+ // 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);
+ average_velocity_ = (history_[old_history_position] -
+ history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+
status->average_velocity = average_velocity_;
- // Determines if the velocity is close enough to the goal to be ready.
+
+ // 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: ");
@@ -110,34 +98,12 @@
}
LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
- // Deal with motor controller deadbands.
- if (output_voltage > 0) {
- output_voltage += positive_deadband_power;
- }
- if (output_voltage < 0) {
- output_voltage -= negative_deadband_power;
- }
+ last_position_ = current_position;
- if (bad_pos) {
- last_position = position->position;
- } else {
- // use the predicted position
- last_position = loop_->X_hat[0];
- }
if (output) {
output->voltage = output_voltage;
}
-
- // If anything is happening, record it. Otherwise, reset the time.
- // This should be removed once we are done with testing.
- if (output_voltage != 0 && average_velocity_ != 0 && velocity_goal != 0) {
- loop_->RecordDatum("shooter.csv", time_);
- time_ += dt;
- }
- else {
- time_ = 0;
- }
-} // RunIteration
+}
} // namespace control_loops
} // namespace frc971