State feedback loop now supports gain scheduling.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 79907c3..57e53fa 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -59,7 +59,7 @@
const double kPracticeAngleAdjustLowerLimit = 0.32;
const double kCompAngleAdjustLowerLimit = 0.32;
-const double kAngleAdjustZeroingSpeed = -0.04;
+const double kAngleAdjustZeroingSpeed = -0.2;
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index f8a7c63..d9eb1f8 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -183,7 +183,7 @@
// Tests that the angle_adjust zeros correctly and goes to a position.
TEST_F(AngleAdjustTest, ZerosCorrectly) {
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
for (int i = 0; i < 400; ++i) {
angle_adjust_motor_plant_.SendPositionMessage();
angle_adjust_motor_.Iterate();
@@ -195,8 +195,8 @@
// Tests that the angle_adjust zeros correctly starting on the sensor.
TEST_F(AngleAdjustTest, ZerosStartingOn) {
- angle_adjust_motor_plant_.Reinitialize(0.25);
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ angle_adjust_motor_plant_.Reinitialize(0.30);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
for (int i = 0; i < 500; ++i) {
angle_adjust_motor_plant_.SendPositionMessage();
angle_adjust_motor_.Iterate();
@@ -208,7 +208,7 @@
// Tests that missing positions are correctly handled.
TEST_F(AngleAdjustTest, HandleMissingPosition) {
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
for (int i = 0; i < 400; ++i) {
if (i % 23) {
angle_adjust_motor_plant_.SendPositionMessage();
@@ -222,7 +222,7 @@
// Tests that loosing the encoder for a second triggers a re-zero.
TEST_F(AngleAdjustTest, RezeroWithMissingPos) {
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
for (int i = 0; i < 800; ++i) {
// After 3 seconds, simulate the encoder going missing.
// This should trigger a re-zero. To make sure it works, change the goal as
@@ -234,7 +234,7 @@
// Should be re-zeroing now.
EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
}
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.5).Send();
}
if (i == 430) {
EXPECT_TRUE(angle_adjust_motor_.is_zeroing() ||
@@ -251,7 +251,7 @@
// Tests that disabling while zeroing sends the state machine into the
// uninitialized state.
TEST_F(AngleAdjustTest, DisableGoesUninitialized) {
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
for (int i = 0; i < 800; ++i) {
angle_adjust_motor_plant_.SendPositionMessage();
// After 0.5 seconds, disable the robot.
@@ -276,10 +276,12 @@
VerifyNearGoal();
}
+/*
+// TODO(aschuh): Enable these tests if we install a second hall effect sensor.
// Tests that the angle_adjust zeros correctly from above the second sensor.
TEST_F(AngleAdjustTest, ZerosCorrectlyAboveSecond) {
angle_adjust_motor_plant_.Reinitialize(1.75);
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(2.0).Send();
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(1.0).Send();
for (int i = 0; i < 400; ++i) {
angle_adjust_motor_plant_.SendPositionMessage();
angle_adjust_motor_.Iterate();
@@ -293,7 +295,7 @@
// the second hall effect sensor.
TEST_F(AngleAdjustTest, ZerosStartingOnSecond) {
angle_adjust_motor_plant_.Reinitialize(1.25);
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(2.0).Send();
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(1.0).Send();
for (int i = 0; i < 500; ++i) {
angle_adjust_motor_plant_.SendPositionMessage();
angle_adjust_motor_.Iterate();
@@ -302,6 +304,7 @@
}
VerifyNearGoal();
}
+*/
} // namespace testing
} // namespace control_loops
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 9ecc1da..4053eae 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -217,10 +217,10 @@
}
for (int i = 0; i < kNumOutputs; ++i) {
- if (U[i] > plant.U_max[i]) {
- U[i] = plant.U_max[i];
- } else if (U[i] < plant.U_min[i]) {
- U[i] = plant.U_min[i];
+ if (U(i, 0) > U_max(i, 0)) {
+ U(i, 0) = U_max(i, 0);
+ } else if (U(i, 0) < U_min(i, 0)) {
+ U(i, 0) = U_min(i, 0);
}
}
}
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index cecf10f..ec08ec8 100644
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -29,8 +29,8 @@
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status) {
const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
- const double current_position =
- (position == NULL ? loop_->X_hat[0] : position->position);
+ const double current_position =
+ (position == NULL ? loop_->X_hat(0, 0) : position->position);
double output_voltage = 0.0;
// Track the current position if the velocity goal is small.
@@ -48,19 +48,19 @@
// 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];
+ (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_->plant.U_min[0] - kVelocityWeightScalar *
- (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
- / loop_->K[0] + loop_->X_hat[0];
+ (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_->Update(position, output == NULL);
// Kill power at low velocity goals.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 894487b..f955796 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -80,13 +80,65 @@
static const int kNumInputs = number_of_inputs;
};
+// A Controller is a structure which holds a plant and the K and L matrices.
+// This is designed such that multiple controllers can share one set of state to
+// support gain scheduling easily.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct StateFeedbackController {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> plant;
+
+ StateFeedbackController(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
+ : L(L),
+ K(K),
+ plant(plant) {
+ }
+};
+
template <int number_of_states, int number_of_inputs, int number_of_outputs>
class StateFeedbackLoop {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
- const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ return controller().plant.A;
+ }
+ double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ return controller().plant.B;
+ }
+ double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ return controller().plant.C;
+ }
+ double C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ return controller().plant.D;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K() const {
+ return controller().K;
+ }
+ double K(int i, int j) const { return K()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ return controller().L;
+ }
+ double L(int i, int j) const { return L()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return controller().plant.U_min;
+ }
+ double U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return controller().plant.U_max;
+ }
+ double U_max(int i, int j) const { return U_max()(i, j); }
Eigen::Matrix<double, number_of_states, 1> X_hat;
Eigen::Matrix<double, number_of_states, 1> R;
@@ -95,17 +147,16 @@
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
Eigen::Matrix<double, number_of_outputs, 1> Y;
- StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> plant;
+ ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> *> controllers;
- StateFeedbackLoop(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
- const StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> &plant)
- : L(L),
- K(K),
- plant(plant) {
+ const StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs>
+ &controller() const {
+ return *controllers[controller_index_];
+ }
+
+ void Reset() {
X_hat.setZero();
R.setZero();
U.setZero();
@@ -113,6 +164,36 @@
U_ff.setZero();
Y.setZero();
}
+
+ StateFeedbackLoop(const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &controller)
+ : controller_index_(0) {
+ controllers.push_back(
+ new StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>(controller));
+ Reset();
+ }
+
+ StateFeedbackLoop(
+ const ::std::vector<StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *> &controllers)
+ : controllers(controllers),
+ controller_index_(0) {
+ Reset();
+ }
+
+ StateFeedbackLoop(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
+ : controller_index_(0) {
+ controllers.push_back(
+ new StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>(L, K, plant));
+
+ Reset();
+ }
virtual ~StateFeedbackLoop() {}
virtual void FeedForward() {
@@ -125,10 +206,10 @@
// it.
virtual void CapU() {
for (int i = 0; i < kNumOutputs; ++i) {
- if (U[i] > plant.U_max[i]) {
- U[i] = plant.U_max[i];
- } else if (U[i] < plant.U_min[i]) {
- U[i] = plant.U_min[i];
+ if (U(i, 0) > U_max(i, 0)) {
+ U(i, 0) = U_max(i, 0);
+ } else if (U(i, 0) < U_min(i, 0)) {
+ U(i, 0) = U_min(i, 0);
}
}
}
@@ -137,26 +218,36 @@
// stop_motors is whether or not to output all 0s.
void Update(bool update_observer, bool stop_motors) {
if (stop_motors) {
- for (int i = 0; i < number_of_outputs; ++i) {
- U[i] = 0.0;
- }
+ U.setZero();
} else {
- U = U_uncapped = K * (R - X_hat);
+ U = U_uncapped = K() * (R - X_hat);
CapU();
}
if (update_observer) {
- X_hat = (plant.A - L * plant.C) * X_hat + L * Y + plant.B * U;
+ X_hat = (A() - L() * C()) * X_hat + L() * Y + B() * U;
} else {
- X_hat = plant.A * X_hat + plant.B * U;
+ X_hat = A() * X_hat + B() * U;
}
}
+ // Sets the current controller to be index and verifies that it isn't out of
+ // range.
+ void set_controller_index(int index) {
+ if (index >= 0 && index < controllers.size()) {
+ controller_index_ = index;
+ }
+ }
+
+ void controller_index() const { return controller_index_; }
+
protected:
// these are accessible from non-templated subclasses
static const int kNumStates = number_of_states;
static const int kNumOutputs = number_of_outputs;
static const int kNumInputs = number_of_inputs;
+
+ int controller_index_;
};
#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_