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;
}
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
index 63c194f..efc1f1b 100644
--- a/bot3/control_loops/shooter/shooter.h
+++ b/bot3/control_loops/shooter/shooter.h
@@ -33,18 +33,10 @@
private:
// The state feedback control loop to talk to.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> loop_;
- // History array and stuff for determining average velocity and whether
- // we are ready to shoot.
- static const int kHistoryLength = 5;
- double history_[kHistoryLength];
- ptrdiff_t history_position_;
double average_velocity_;
- double position_goal_;
- double last_position_;
-
// For making sure it keeps spinning if we're shooting.
double last_velocity_goal_;
diff --git a/bot3/control_loops/shooter/shooter_lib_test.cc b/bot3/control_loops/shooter/shooter_lib_test.cc
index 062e507..fddc415 100644
--- a/bot3/control_loops/shooter/shooter_lib_test.cc
+++ b/bot3/control_loops/shooter/shooter_lib_test.cc
@@ -22,7 +22,7 @@
// Constructs a shooter simulation. I'm not sure how the construction of
// the queue (my_shooter_loop_) actually works (same format as wrist).
ShooterMotorSimulation()
- : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+ : shooter_plant_(new StateFeedbackPlant<1, 1, 1>(MakeShooterPlant())),
my_shooter_loop_(".bot3.control_loops.shooter",
0x78d8e372, ".bot3.control_loops.shooter.goal",
".bot3.control_loops.shooter.position",
@@ -45,7 +45,7 @@
shooter_plant_->Update();
}
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<1, 1, 1>> shooter_plant_;
private:
ShooterLoop my_shooter_loop_;
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index aacf9b1..ab5dbc7 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.cc
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -7,40 +7,40 @@
namespace bot3 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00994518845675, 0.0, 0.989057756738;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.00267091861198, 0.533205953514;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
+StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 1, 1> A;
+ A << 0.989057756738;
+ Eigen::Matrix<double, 1, 1> B;
+ B << 0.533205953514;
+ Eigen::Matrix<double, 1, 1> C;
+ C << 0;
Eigen::Matrix<double, 1, 1> D;
D << 0;
Eigen::Matrix<double, 1, 1> U_max;
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<1, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 1, 1> MakeShooterController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.08905775674, 29.7111780621;
- Eigen::Matrix<double, 1, 2> K;
- K << 1.42534042426, 0.758151303088;
- return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
+StateFeedbackController<1, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 1, 1> L;
+ L << 29.7111780621;
+ Eigen::Matrix<double, 1, 1> K;
+ K << 0.758151303088;
+ return StateFeedbackController<1, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
+StateFeedbackPlant<1, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<1, 1, 1>(plants);
}
-StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
+StateFeedbackLoop<1, 1, 1> MakeShooterLoop() {
+ ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<1, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<1, 1, 1>(controllers);
}
} // namespace control_loops
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
index fa2237d..1ab9702 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.h
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -6,13 +6,13 @@
namespace bot3 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
+StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients();
-StateFeedbackController<2, 1, 1> MakeShooterController();
+StateFeedbackController<1, 1, 1> MakeShooterController();
-StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
+StateFeedbackPlant<1, 1, 1> MakeShooterPlant();
-StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
+StateFeedbackLoop<1, 1, 1> MakeShooterLoop();
} // namespace control_loops
} // namespace bot3