Merge "Adding //aos/vision/debug:debug_framework."
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 654ca2c..d3a220b 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -153,9 +153,9 @@
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B().inverse() *
+ loop_->controller(kHighGearController).plant.B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A());
+ loop_->controller(kHighGearController).plant.A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
@@ -178,9 +178,9 @@
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B().inverse() *
+ loop_->controller(kHighGearController).plant.B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A());
+ loop_->controller(kHighGearController).plant.A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 3f4a6ad..bcf00d6 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -20,19 +20,19 @@
// input because that's what comes back from the sensors).
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-class StateFeedbackPlantCoefficients final {
+struct StateFeedbackPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
- : A_(other.A()),
- A_continuous_(other.A_continuous()),
- B_(other.B()),
- B_continuous_(other.B_continuous()),
- C_(other.C()),
- D_(other.D()),
- U_min_(other.U_min()),
- U_max_(other.U_max()) {}
+ : A(other.A),
+ A_continuous(other.A_continuous),
+ B(other.B),
+ B_continuous(other.B_continuous),
+ C(other.C),
+ D(other.D),
+ U_min(other.U_min),
+ U_max(other.U_max) {}
StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
@@ -45,61 +45,23 @@
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
- : A_(A),
- A_continuous_(A_continuous),
- B_(B),
- B_continuous_(B_continuous),
- C_(C),
- D_(D),
- U_min_(U_min),
- U_max_(U_max) {}
+ : A(A),
+ A_continuous(A_continuous),
+ B(B),
+ B_continuous(B_continuous),
+ C(C),
+ D(D),
+ U_min(U_min),
+ U_max(U_max) {}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return A_;
- }
- double A(int i, int j) const { return A()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_continuous()
- const {
- return A_continuous_;
- }
- double A_continuous(int i, int j) const { return A_continuous()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
- return B_;
- }
- double B(int i, int j) const { return B()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B_continuous() const {
- return B_continuous_;
- }
- double B_continuous(int i, int j) const { return B_continuous()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return 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 D_;
- }
- double D(int i, int j) const { return D()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return 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 U_max_;
- }
- double U_max(int i, int j) const { return U_max()(i, j); }
-
- private:
- const Eigen::Matrix<double, number_of_states, number_of_states> A_;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous_;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous_;
- const Eigen::Matrix<double, number_of_outputs, number_of_states> C_;
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D_;
- const Eigen::Matrix<double, number_of_inputs, 1> U_min_;
- const Eigen::Matrix<double, number_of_inputs, 1> U_max_;
-
- StateFeedbackPlantCoefficients &operator=(
- StateFeedbackPlantCoefficients other) = delete;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -125,27 +87,27 @@
virtual ~StateFeedbackPlant() {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return coefficients().A();
+ return coefficients().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 coefficients().B();
+ return coefficients().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 coefficients().C();
+ return coefficients().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 coefficients().D();
+ return coefficients().D;
}
double D(int i, int j) const { return D()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return coefficients().U_min();
+ return coefficients().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 coefficients().U_max();
+ return coefficients().U_max;
}
double U_max(int i, int j) const { return U_max()(i, j); }
@@ -206,7 +168,8 @@
Eigen::Matrix<double, number_of_outputs, 1> Y_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> coefficients_;
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
int plant_index_;
@@ -236,20 +199,6 @@
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: L(L), K(K), Kff(Kff), A_inv(A_inv), plant(plant) {}
-
- // TODO(Brian): Remove this overload once they're all converted.
- StateFeedbackControllerConstants(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
- const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> &plant)
- : L(L),
- K(K),
- Kff(::Eigen::Matrix<double, number_of_inputs,
- number_of_states>::Zero()),
- A_inv(A_inv),
- plant(plant) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -288,11 +237,11 @@
virtual ~StateFeedbackLoop() {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return controller().plant.A();
+ 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();
+ return controller().plant.B;
}
const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv()
const {
@@ -301,19 +250,19 @@
double A_inv(int i, int j) const { return A_inv()(i, j); }
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();
+ 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();
+ return controller().plant.D;
}
double D(int i, int j) const { return D()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return controller().plant.U_min();
+ 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();
+ return controller().plant.U_max;
}
double U_max(int i, int j) const { return U_max()(i, j); }
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 5b8803a..9077103 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -46,6 +46,7 @@
StateFeedbackControllerConstants<2, 4, 7>(
Eigen::Matrix<double, 2, 7>::Identity(),
Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
test_loop.Update(false);
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 8f3a59e..6f853d6 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -28,12 +28,12 @@
const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
const Eigen::Matrix<double, 2, 1> accelerating_ff =
- controller(0).Kff * (next_R() - controller(0).plant.A() * R());
+ controller(0).Kff * (next_R() - controller(0).plant.A * R());
const Eigen::Matrix<double, 2, 1> accelerating_controller =
controller(0).K * error() + accelerating_ff;
const Eigen::Matrix<double, 2, 1> decelerating_ff =
- controller(1).Kff * (next_R() - controller(1).plant.A() * R());
+ controller(1).Kff * (next_R() - controller(1).plant.A * R());
const Eigen::Matrix<double, 2, 1> decelerating_controller =
controller(1).K * error() + decelerating_ff;