Merge "Update LED and current limit resistors"
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 27ad4fb..971f0b8 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -17,7 +17,8 @@
#include "aos/common/macros.h"
#include "frc971/control_loops/state_feedback_loop.h"
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackHybridPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -32,14 +33,14 @@
U_max(other.U_max) {}
StateFeedbackHybridPlantCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_states>
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&A_continuous,
- const Eigen::Matrix<double, number_of_states, number_of_inputs>
+ const Eigen::Matrix<Scalar, 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_max,
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
: A_continuous(A_continuous),
B_continuous(B_continuous),
C(C),
@@ -47,15 +48,16 @@
U_min(U_min),
U_max(U_max) {}
- const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous;
- 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;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_continuous;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_continuous;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackHybridPlant {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -77,49 +79,49 @@
virtual ~StateFeedbackHybridPlant() {}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ const Eigen::Matrix<Scalar, 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_inputs> &B() const {
+ Scalar A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<Scalar, 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_outputs, number_of_states> &C() const {
+ Scalar B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C() const {
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 {
+ Scalar C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D() const {
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 {
+ Scalar D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min() const {
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 {
+ Scalar U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max() const {
return coefficients().U_max;
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ Scalar U_max(int i, int j) const { return U_max()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
- double X(int i, int j) const { return X()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
- double Y(int i, int j) const { return Y()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
+ Scalar X(int i, int j) const { return X()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
+ Scalar Y(int i, int j) const { return Y()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
- double &mutable_X(int i, int j) { return mutable_X()(i, j); }
- Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
- double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
+ Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
+ Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
@@ -141,36 +143,38 @@
}
// Assert that U is within the hardware range.
- virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ virtual void CheckU(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
for (int i = 0; i < kNumInputs; ++i) {
- if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
+ U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
LOG(FATAL, "U out of range\n");
}
}
}
// Computes the new X and Y given the control input.
- void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
::aos::robot_state.FetchLatest();
- Eigen::Matrix<double, number_of_inputs, 1> current_U =
- DelayedU_ * (::aos::robot_state.get()
- ? ::aos::robot_state->voltage_battery / 12.0
- : 1.0);
+ Eigen::Matrix<Scalar, number_of_inputs, 1> current_U =
+ DelayedU_ *
+ (::aos::robot_state.get()
+ ? ::aos::robot_state->voltage_battery / static_cast<Scalar>(12.0)
+ : static_cast<Scalar>(1.0));
X_ = Update(X(), current_U, dt);
Y_ = C() * X() + D() * current_U;
DelayedU_ = U;
}
- Eigen::Matrix<double, number_of_inputs, 1> DelayedU_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
- Eigen::Matrix<double, number_of_states, 1> Update(
- const Eigen::Matrix<double, number_of_states, 1> X,
- const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ Eigen::Matrix<Scalar, number_of_states, 1> Update(
+ const Eigen::Matrix<Scalar, number_of_states, 1> X,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt) {
UpdateAB(dt);
return A() * X + B() * U;
@@ -184,22 +188,22 @@
private:
void UpdateAB(::std::chrono::nanoseconds dt) {
- Eigen::Matrix<double, number_of_states + number_of_inputs,
+ Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
number_of_states + number_of_inputs>
M_state_continuous;
M_state_continuous.setZero();
M_state_continuous.template block<number_of_states, number_of_states>(0,
0) =
coefficients().A_continuous *
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count();
M_state_continuous.template block<number_of_states, number_of_inputs>(
0, number_of_states) =
coefficients().B_continuous *
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count();
- Eigen::Matrix<double, number_of_states + number_of_inputs,
+ Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
number_of_states + number_of_inputs>
M_state = M_state_continuous.exp();
A_ = M_state.template block<number_of_states, number_of_states>(0, 0);
@@ -207,11 +211,11 @@
0, number_of_states);
}
- Eigen::Matrix<double, number_of_states, 1> X_;
- Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_;
+ Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
- Eigen::Matrix<double, number_of_states, number_of_states> A_;
- Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> A_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_;
::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
@@ -225,34 +229,36 @@
// A container for all the observer coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct HybridKalmanCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_states> Q_continuous;
- const Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_continuous;
- const Eigen::Matrix<double, number_of_states, number_of_states> P_steady_state;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> P_steady_state;
HybridKalmanCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_states>
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&Q_continuous,
- const Eigen::Matrix<double, number_of_outputs, number_of_outputs>
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
&R_continuous,
- const Eigen::Matrix<double, number_of_states, number_of_states>
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&P_steady_state)
: Q_continuous(Q_continuous),
R_continuous(R_continuous),
P_steady_state(P_steady_state) {}
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class HybridKalman {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
explicit HybridKalman(
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
: coefficients_(::std::move(*observers)) {}
HybridKalman(HybridKalman &&other)
@@ -261,27 +267,27 @@
}
// Getters for Q
- const Eigen::Matrix<double, number_of_states, number_of_states> &Q() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q() const {
return Q_;
}
- double Q(int i, int j) const { return Q()(i, j); }
+ Scalar Q(int i, int j) const { return Q()(i, j); }
// Getters for R
- const Eigen::Matrix<double, number_of_outputs, number_of_outputs> &R() const {
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R() const {
return R_;
}
- double R(int i, int j) const { return R()(i, j); }
+ Scalar R(int i, int j) const { return R()(i, j); }
// Getters for P
- const Eigen::Matrix<double, number_of_states, number_of_states> &P() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &P() const {
return P_;
}
- double P(int i, int j) const { return P()(i, j); }
+ Scalar P(int i, int j) const { return P()(i, j); }
// Getters for X_hat
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
return X_hat_;
}
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs> *plant) {
@@ -291,8 +297,8 @@
}
void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs> *plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ number_of_outputs, Scalar> *plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
// Trigger the predict step. This will update A() and B() in the plant.
mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
@@ -303,14 +309,14 @@
void Correct(
const StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs> &plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &U,
- const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
- Eigen::Matrix<double, number_of_outputs, 1> Y_bar =
+ number_of_outputs, Scalar> &plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
+ Eigen::Matrix<Scalar, number_of_outputs, 1> Y_bar =
Y - (plant.C() * X_hat_ + plant.D() * U);
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> S =
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> S =
plant.C() * P_ * plant.C().transpose() + R_;
- Eigen::Matrix<double, number_of_states, number_of_outputs> KalmanGain;
+ Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
KalmanGain =
(S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
.transpose();
@@ -350,16 +356,16 @@
number_of_outputs> *plant,
::std::chrono::nanoseconds dt) {
// Now, compute the discrete time Q and R coefficients.
- Eigen::Matrix<double, number_of_states, number_of_states> Qtemp =
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Qtemp =
(coefficients().Q_continuous +
coefficients().Q_continuous.transpose()) /
- 2.0;
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> Rtemp =
+ static_cast<Scalar>(2.0);
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
(coefficients().R_continuous +
coefficients().R_continuous.transpose()) /
- 2.0;
+ static_cast<Scalar>(2.0);
- Eigen::Matrix<double, 2 * number_of_states, 2 * number_of_states> M_gain;
+ Eigen::Matrix<Scalar, 2 * number_of_states, 2 * number_of_states> M_gain;
M_gain.setZero();
// Set up the matrix M = [[-A, Q], [0, A.T]]
M_gain.template block<number_of_states, number_of_states>(0, 0) =
@@ -370,35 +376,35 @@
number_of_states, number_of_states) =
plant->coefficients().A_continuous.transpose();
- Eigen::Matrix<double, 2 * number_of_states, 2 *number_of_states> phi =
+ Eigen::Matrix<Scalar, 2 * number_of_states, 2 *number_of_states> phi =
(M_gain *
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count())
.exp();
// Phi12 = phi[0:number_of_states, number_of_states:2*number_of_states]
// Phi22 = phi[number_of_states:2*number_of_states,
// number_of_states:2*number_of_states]
- Eigen::Matrix<double, number_of_states, number_of_states> phi12 =
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> phi12 =
phi.block(0, number_of_states, number_of_states, number_of_states);
- Eigen::Matrix<double, number_of_states, number_of_states> phi22 = phi.block(
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> phi22 = phi.block(
number_of_states, number_of_states, number_of_states, number_of_states);
Q_ = phi22.transpose() * phi12;
- Q_ = (Q_ + Q_.transpose()) / 2.0;
+ Q_ = (Q_ + Q_.transpose()) / static_cast<Scalar>(2.0);
R_ = Rtemp /
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count();
}
// Internal state estimate.
- Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
// Internal covariance estimate.
- Eigen::Matrix<double, number_of_states, number_of_states> P_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> P_;
// Discretized Q and R for the kalman filter.
- Eigen::Matrix<double, number_of_states, number_of_states> Q_;
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_;
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_;
int index_ = 0;
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 229bbad..401be44 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -106,10 +106,10 @@
return HybridKalman<3, 1, 1>(&observers);
}
-StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
HybridKalman<3, 1, 1>>
MakeIntegralShooterLoop() {
- return StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ return StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
HybridKalman<3, 1, 1>>(
MakeIntegralShooterPlant(), MakeIntegralShooterController(),
MakeIntegralShooterObserver());
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 50e4314..805e079 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -18,7 +18,8 @@
class ControlLoopWriter(object):
def __init__(self, gain_schedule_name, loops, namespaces=None,
write_constants=False, plant_type='StateFeedbackPlant',
- observer_type='StateFeedbackObserver'):
+ observer_type='StateFeedbackObserver',
+ scalar_type='double'):
"""Constructs a control loop writer.
Args:
@@ -29,6 +30,7 @@
order. If None, the default will be used.
plant_type: string, The C++ type of the plant.
observer_type: string, The C++ type of the observer.
+ scalar_type: string, The C++ type of the base scalar.
"""
self._gain_schedule_name = gain_schedule_name
self._loops = loops
@@ -46,6 +48,7 @@
self._constant_list = []
self._plant_type = plant_type
self._observer_type = observer_type
+ self._scalar_type = scalar_type
def AddConstant(self, constant):
"""Adds a constant to write.
@@ -77,6 +80,8 @@
extra_args = ', ' + extra_args
else:
extra_args = ''
+ if self._scalar_type != 'double':
+ extra_args += ', ' + self._scalar_type
return '%s<%d, %d, %d%s>' % (
typename, num_states, num_inputs, num_outputs, extra_args)
@@ -90,8 +95,16 @@
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
- extra_args = '%s, %s' % (self._PlantType(), self._ObserverType())
- return self._GenericType('StateFeedbackLoop', extra_args)
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+
+ return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
+ num_states,
+ num_inputs,
+ num_outputs, self._scalar_type,
+ self._PlantType(), self._ObserverType())
+
def _PlantType(self):
"""Returns a template name for StateFeedbackPlant."""
@@ -109,10 +122,8 @@
"""Returns a template name for StateFeedbackObserverCoefficients."""
return self._GenericType(self._observer_type + 'Coefficients')
- def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
- """Writes the header file to the file named header_file.
- Set double_appendage to true in order to include a ratio of
- moments of inertia constant. Currently, only used for 2014 claw."""
+ def WriteHeader(self, header_file):
+ """Writes the header file to the file named header_file."""
with open(header_file, 'w') as fd:
header_guard = self._HeaderGuard(header_file)
fd.write('#ifndef %s\n'
@@ -133,7 +144,7 @@
for loop in self._loops:
fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
fd.write('\n')
- fd.write(loop.DumpControllerHeader())
+ fd.write(loop.DumpControllerHeader(self._scalar_type))
fd.write('\n')
fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
fd.write('\n')
@@ -167,15 +178,15 @@
fd.write(self._namespace_start)
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlant(self._PlantCoeffType()))
+ fd.write(loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
fd.write('\n')
for loop in self._loops:
- fd.write(loop.DumpController())
+ fd.write(loop.DumpController(self._scalar_type))
fd.write('\n')
for loop in self._loops:
- fd.write(loop.DumpObserver(self._ObserverCoeffType()))
+ fd.write(loop.DumpObserver(self._ObserverCoeffType(), self._scalar_type))
fd.write('\n')
fd.write('%s Make%sPlant() {\n' %
@@ -288,22 +299,26 @@
self.X_hat = (self.A * self.X_hat + self.B * U +
self.L * (self.Y - self.C * self.X_hat - self.D * U))
- def _DumpMatrix(self, matrix_name, matrix):
+ def _DumpMatrix(self, matrix_name, matrix, scalar_type):
"""Dumps the provided matrix into a variable called matrix_name.
Args:
matrix_name: string, The variable name to save the matrix to.
matrix: The matrix to dump.
+ scalar_type: The C++ type to use for the scalar in the matrix.
Returns:
string, The C++ commands required to populate a variable named matrix_name
with the contents of matrix.
"""
- ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
- matrix.shape[0], matrix.shape[1], matrix_name)]
+ ans = [' Eigen::Matrix<%s, %d, %d> %s;\n' % (
+ scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)]
for x in xrange(matrix.shape[0]):
for y in xrange(matrix.shape[1]):
- ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, repr(matrix[x, y])))
+ write_type = repr(matrix[x, y])
+ if scalar_type == 'float':
+ write_type += 'f'
+ ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
return ''.join(ans)
@@ -316,7 +331,7 @@
return '%s Make%sPlantCoefficients();\n' % (
plant_coefficient_type, self._name)
- def DumpPlant(self, plant_coefficient_type):
+ def DumpPlant(self, plant_coefficient_type, scalar_type):
"""Writes out a c++ function which will create a PlantCoefficients object.
Returns:
@@ -325,21 +340,21 @@
ans = ['%s Make%sPlantCoefficients() {\n' % (
plant_coefficient_type, self._name)]
- ans.append(self._DumpMatrix('C', self.C))
- ans.append(self._DumpMatrix('D', self.D))
- ans.append(self._DumpMatrix('U_max', self.U_max))
- ans.append(self._DumpMatrix('U_min', self.U_min))
+ ans.append(self._DumpMatrix('C', self.C, scalar_type))
+ ans.append(self._DumpMatrix('D', self.D, scalar_type))
+ ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
+ ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
if plant_coefficient_type.startswith('StateFeedbackPlant'):
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
- ans.append(self._DumpMatrix('B', self.B))
+ ans.append(self._DumpMatrix('A', self.A, scalar_type))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A), scalar_type))
+ ans.append(self._DumpMatrix('B', self.B, scalar_type))
ans.append(' return %s'
'(A, A_inv, B, C, D, U_max, U_min);\n' % (
plant_coefficient_type))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
+ ans.append(self._DumpMatrix('A_continuous', self.A_continuous, scalar_type))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous, scalar_type))
ans.append(' return %s'
'(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
plant_coefficient_type))
@@ -361,7 +376,7 @@
"""Returns the name of the controller function."""
return 'Make%sObserverCoefficients()' % self._name
- def DumpControllerHeader(self):
+ def DumpControllerHeader(self, scalar_type):
"""Writes out a c++ header declaration which will create a Controller object.
Returns:
@@ -370,10 +385,11 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
+ num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())
- def DumpController(self):
+ def DumpController(self, scalar_type):
"""Returns a c++ function which will create a Controller object.
Returns:
@@ -382,18 +398,19 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())]
+ ans = ['StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' % (
+ num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())]
- ans.append(self._DumpMatrix('K', self.K))
+ ans.append(self._DumpMatrix('K', self.K, scalar_type))
if not hasattr(self, 'Kff'):
self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
- ans.append(self._DumpMatrix('Kff', self.Kff))
+ ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
- ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d>'
+ ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
'(K, Kff);\n' % (
- num_states, num_inputs, num_outputs))
+ num_states, num_inputs, num_outputs, scalar_type))
ans.append('}\n')
return ''.join(ans)
@@ -406,7 +423,7 @@
return '%s %s;\n' % (
observer_coefficient_type, self.ObserverFunction())
- def DumpObserver(self, observer_coefficient_type):
+ def DumpObserver(self, observer_coefficient_type, scalar_type):
"""Returns a c++ function which will create a Observer object.
Returns:
@@ -416,12 +433,12 @@
observer_coefficient_type, self.ObserverFunction())]
if observer_coefficient_type.startswith('StateFeedbackObserver'):
- ans.append(self._DumpMatrix('L', self.L))
+ ans.append(self._DumpMatrix('L', self.L, scalar_type))
ans.append(' return %s(L);\n' % (observer_coefficient_type,))
elif observer_coefficient_type.startswith('HybridKalman'):
- ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous))
- ans.append(self._DumpMatrix('R_continuous', self.R_continuous))
- ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state))
+ ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
+ ans.append(self._DumpMatrix('R_continuous', self.R_continuous, scalar_type))
+ ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state, scalar_type))
ans.append(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
observer_coefficient_type,))
else:
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 34a0fcf..b9765fc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -16,7 +16,7 @@
#include "aos/common/macros.h"
template <int number_of_states, int number_of_inputs, int number_of_outputs,
- typename PlantType, typename ObserverType>
+ typename PlantType, typename ObserverType, typename Scalar>
class StateFeedbackLoop;
// For everything in this file, "inputs" and "outputs" are defined from the
@@ -26,7 +26,8 @@
// controller (U is an output because that's what goes to the motors and Y is an
// input because that's what comes back from the sensors).
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -41,32 +42,33 @@
U_max(other.U_max) {}
StateFeedbackPlantCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_states> &A,
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
- 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_max,
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
: A(A), A_inv(A_inv), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
- const Eigen::Matrix<double, number_of_states, number_of_states> A;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
- 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;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_inv;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackPlant {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
StateFeedbackPlant(
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
*coefficients)
: coefficients_(::std::move(*coefficients)), index_(0) {
Reset();
@@ -81,53 +83,53 @@
virtual ~StateFeedbackPlant() {}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A() const {
return coefficients().A;
}
- double A(int i, int j) const { return A()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv() const {
+ Scalar A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv() const {
return coefficients().A_inv;
}
- double A_inv(int i, int j) const { return A_inv()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ Scalar A_inv(int i, int j) const { return A_inv()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B() const {
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 {
+ Scalar B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C() const {
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 {
+ Scalar C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D() const {
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 {
+ Scalar D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min() const {
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 {
+ Scalar U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max() const {
return coefficients().U_max;
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ Scalar U_max(int i, int j) const { return U_max()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
- double X(int i, int j) const { return X()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
- double Y(int i, int j) const { return Y()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
+ Scalar X(int i, int j) const { return X()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
+ Scalar Y(int i, int j) const { return Y()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
- double &mutable_X(int i, int j) { return mutable_X()(i, j); }
- Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
- double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
+ Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
+ Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
@@ -145,16 +147,17 @@
}
// Assert that U is within the hardware range.
- virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ virtual void CheckU(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
for (int i = 0; i < kNumInputs; ++i) {
- if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
+ U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
LOG(FATAL, "U out of range\n");
}
}
}
// Computes the new X and Y given the control input.
- void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
@@ -163,13 +166,13 @@
}
// Computes the new Y given the control input.
- void UpdateY(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ void UpdateY(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
Y_ = C() * X() + D() * U;
}
- Eigen::Matrix<double, number_of_states, 1> Update(
- const Eigen::Matrix<double, number_of_states, 1> X,
- const Eigen::Matrix<double, number_of_inputs, 1> &U) const {
+ Eigen::Matrix<Scalar, number_of_states, 1> Update(
+ const Eigen::Matrix<Scalar, number_of_states, 1> X,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) const {
return A() * X + B() * U;
}
@@ -180,11 +183,11 @@
static const int kNumInputs = number_of_inputs;
private:
- Eigen::Matrix<double, number_of_states, 1> X_;
- Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_;
+ Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
coefficients_;
int index_;
@@ -193,27 +196,30 @@
};
// A container for all the controller coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackControllerCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
- const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> K;
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> Kff;
StateFeedbackControllerCoefficients(
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &K,
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &Kff)
: K(K), Kff(Kff) {}
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackController {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
explicit StateFeedbackController(
::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
+ *controllers)
: coefficients_(::std::move(*controllers)) {}
StateFeedbackController(StateFeedbackController &&other)
@@ -221,14 +227,14 @@
::std::swap(coefficients_, other.coefficients_);
}
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &K() const {
return coefficients().K;
}
- double K(int i, int j) const { return K()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
+ Scalar K(int i, int j) const { return K()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &Kff() const {
return coefficients().Kff;
}
- double Kff(int i, int j) const { return Kff()(i, j); }
+ Scalar Kff(int i, int j) const { return Kff()(i, j); }
void Reset() {}
@@ -246,13 +252,13 @@
int index() const { return index_; }
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
@@ -260,30 +266,32 @@
private:
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
coefficients_;
};
// A container for all the observer coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackObserverCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> L;
StateFeedbackObserverCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L)
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L)
: L(L) {}
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackObserver {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
explicit StateFeedbackObserver(
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
: coefficients_(::std::move(*observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
@@ -291,32 +299,32 @@
::std::swap(coefficients_, other.coefficients_);
}
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L() const {
return coefficients().L;
}
- double L(int i, int j) const { return L()(i, j); }
+ Scalar L(int i, int j) const { return L()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
return X_hat_;
}
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> * /*loop*/) {
+ number_of_outputs, Scalar> * /*loop*/) {
X_hat_.setZero();
}
void Predict(StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> *plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ number_of_outputs, Scalar> *plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds /*dt*/) {
mutable_X_hat() = plant->Update(X_hat(), new_u);
}
void Correct(const StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> &plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &U,
- const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ number_of_outputs, Scalar> &plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
mutable_X_hat() +=
plant.A_inv() * L() * (Y - plant.C() * X_hat() - plant.D() * U);
}
@@ -335,32 +343,33 @@
int index() const { return index_; }
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
private:
// Internal state estimate.
- Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
coefficients_;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double,
typename PlantType = StateFeedbackPlant<
- number_of_states, number_of_inputs, number_of_outputs>,
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>,
typename ObserverType = StateFeedbackObserver<
- number_of_states, number_of_inputs, number_of_outputs>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>
class StateFeedbackLoop {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -368,7 +377,7 @@
explicit StateFeedbackLoop(
PlantType &&plant,
StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> &&controller,
+ number_of_outputs, Scalar> &&controller,
ObserverType &&observer)
: plant_(::std::move(plant)),
controller_(::std::move(controller)),
@@ -389,43 +398,43 @@
virtual ~StateFeedbackLoop() {}
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
return observer().X_hat();
}
- double X_hat(int i, int j) const { return X_hat()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &R() const { return R_; }
- double R(int i, int j) const { return R()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &next_R() const {
+ Scalar X_hat(int i, int j) const { return X_hat()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, 1> &R() const { return R_; }
+ Scalar R(int i, int j) const { return R()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, 1> &next_R() const {
return next_R_;
}
- double next_R(int i, int j) const { return next_R()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
- double U(int i, int j) const { return U()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_uncapped() const {
+ Scalar next_R(int i, int j) const { return next_R()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U() const { return U_; }
+ Scalar U(int i, int j) const { return U()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_uncapped() const {
return U_uncapped_;
}
- double U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &ff_U() const {
+ Scalar U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &ff_U() const {
return ff_U_;
}
- double ff_U(int i, int j) const { return ff_U()(i, j); }
+ Scalar ff_U(int i, int j) const { return ff_U()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() {
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() {
return observer_.mutable_X_hat();
}
- double &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_R() { return R_; }
- double &mutable_R(int i, int j) { return mutable_R()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_next_R() {
+ Scalar &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_R() { return R_; }
+ Scalar &mutable_R(int i, int j) { return mutable_R()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_next_R() {
return next_R_;
}
- double &mutable_next_R(int i, int j) { return mutable_next_R()(i, j); }
- Eigen::Matrix<double, number_of_inputs, 1> &mutable_U() { return U_; }
- double &mutable_U(int i, int j) { return mutable_U()(i, j); }
- Eigen::Matrix<double, number_of_inputs, 1> &mutable_U_uncapped() {
+ Scalar &mutable_next_R(int i, int j) { return mutable_next_R()(i, j); }
+ Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U() { return U_; }
+ Scalar &mutable_U(int i, int j) { return mutable_U()(i, j); }
+ Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U_uncapped() {
return U_uncapped_;
}
- double &mutable_U_uncapped(int i, int j) {
+ Scalar &mutable_U_uncapped(int i, int j) {
return mutable_U_uncapped()(i, j);
}
@@ -433,7 +442,7 @@
PlantType *mutable_plant() { return &plant_; }
const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&controller() const {
return controller_;
}
@@ -465,23 +474,23 @@
}
// Corrects X_hat given the observation in Y.
- void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ void Correct(const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
observer_.Correct(this->plant(), U(), Y);
}
- const Eigen::Matrix<double, number_of_states, 1> error() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> error() const {
return R() - X_hat();
}
// Returns the calculated controller power.
- virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
+ virtual const Eigen::Matrix<Scalar, number_of_inputs, 1> ControllerOutput() {
// TODO(austin): Should this live in StateSpaceController?
ff_U_ = FeedForward();
return controller().K() * error() + ff_U_;
}
// Calculates the feed forwards power.
- virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
+ virtual const Eigen::Matrix<Scalar, number_of_inputs, 1> FeedForward() {
// TODO(austin): Should this live in StateSpaceController?
return controller().Kff() * (next_R() - plant().A() * R());
}
@@ -511,7 +520,7 @@
}
}
- void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ void UpdateObserver(const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
observer_.Predict(this->mutable_plant(), new_u, dt);
}
@@ -528,7 +537,8 @@
protected:
PlantType plant_;
- StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs>
+ StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs,
+ Scalar>
controller_;
ObserverType observer_;
@@ -539,17 +549,17 @@
static constexpr int kNumInputs = number_of_inputs;
// Portion of U which is based on the feed-forwards.
- Eigen::Matrix<double, number_of_inputs, 1> ff_U_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> ff_U_;
private:
// Current goal (Used by the feed-back controller).
- Eigen::Matrix<double, number_of_states, 1> R_;
+ Eigen::Matrix<Scalar, number_of_states, 1> R_;
// Goal to go to in the next cycle (Used by Feed-Forward controller.)
- Eigen::Matrix<double, number_of_states, 1> next_R_;
+ Eigen::Matrix<Scalar, number_of_states, 1> next_R_;
// Computed output after being capped.
- Eigen::Matrix<double, number_of_inputs, 1> U_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> U_;
// Computed output before being capped.
- Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> U_uncapped_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};
diff --git a/motors/big_schematic/gschem_file.rb b/motors/big_schematic/gschem_file.rb
index 5788f34..bff85fd 100644
--- a/motors/big_schematic/gschem_file.rb
+++ b/motors/big_schematic/gschem_file.rb
@@ -172,7 +172,10 @@
files.each do |file|
if file.end_with? '.gsch2pcb'
File.open(file, 'r') do |f|
- r += f.readline.split(' ')[1..-1]
+ dirname = File.dirname(file) + '/'
+ r += f.readline.split(' ')[1..-1].map do |filename|
+ dirname + filename
+ end
end
else
r.push file
diff --git a/motors/big_schematic/ordering.rb b/motors/big_schematic/ordering.rb
index 8f5a01a..1b7b389 100755
--- a/motors/big_schematic/ordering.rb
+++ b/motors/big_schematic/ordering.rb
@@ -59,7 +59,8 @@
puts ARGV
filenames = get_schematic_filenames ARGV
-$parts_yaml = YAML.load_file('parts.yaml')
+#$parts_yaml = YAML.load_file('parts.yaml')
+$parts_yaml = {}
module Unicode
NBSP = "\u00A0"
@@ -735,7 +736,8 @@
unit_price = nil
order_quantity = nil
if $bom_file
- footprint = $parts_yaml['bom_footprints'][part.footprint] || part.footprint
+ #footprint = $parts_yaml['bom_footprints'][part.footprint] || part.footprint
+ footprint = part.footprint
case options[:bom_style]
when :myro
line = [part.quantity,
diff --git a/motors/big_schematic/parts.rb b/motors/big_schematic/parts.rb
index 87650ed..d02b797 100644
--- a/motors/big_schematic/parts.rb
+++ b/motors/big_schematic/parts.rb
@@ -11,7 +11,7 @@
require 'yaml'
require 'net/http'
-require './html'
+#require './html'
def get_octopart_results(mpn)
query = [{:mpn => mpn, :limit => 20}]
diff --git a/motors/hid_schematic/board-main.sch b/motors/hid_schematic/board-main.sch
index 55fa40a..b28b51c 100644
--- a/motors/hid_schematic/board-main.sch
+++ b/motors/hid_schematic/board-main.sch
@@ -617,6 +617,8 @@
refdes=R1
T 51200 40900 5 10 1 1 270 0 1
value=1.3 k
+T 51400 41100 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 51500 41300 51500 41100 4
N 51500 40200 51500 40000 4
@@ -644,6 +646,8 @@
refdes=R2
T 52200 40900 5 10 1 1 270 0 1
value=3 k
+T 52400 41100 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
N 52500 41300 52500 41100 4
N 52500 40200 52500 40000 4
@@ -672,6 +676,8 @@
refdes=R3
T 80100 44600 5 10 1 1 270 0 1
value=1.3 k
+T 80300 44800 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 80400 45000 80400 44800 4
N 80400 43900 80400 43300 4
@@ -699,6 +705,8 @@
refdes=R4
T 81600 44600 5 10 1 1 270 0 1
value=1.3 k
+T 81800 44800 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 81900 45000 81900 44800 4
N 81900 43100 81900 43900 4
@@ -726,6 +734,8 @@
refdes=R5
T 83100 44600 5 10 1 1 270 0 1
value=1.3 k
+T 83300 44800 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 83400 45000 83400 44800 4
N 83400 43700 83400 43900 4
@@ -753,6 +763,8 @@
refdes=R6
T 84600 44600 5 10 1 1 270 0 1
value=1.3 k
+T 84800 44800 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 84900 45000 84900 44800 4
N 84900 43500 84900 43900 4
@@ -949,6 +961,8 @@
refdes=R11
T 61700 54100 5 10 1 1 270 0 1
value=3 k
+T 61900 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 61800 55600 1 0 0 5V-plus-1.sym
N 62400 52200 62400 56700 4
@@ -983,6 +997,8 @@
refdes=R12
T 62900 54100 5 10 1 1 270 0 1
value=3 k
+T 63100 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 63000 55600 1 0 0 5V-plus-1.sym
N 63200 52800 63200 53400 4
@@ -1015,6 +1031,8 @@
refdes=R13
T 64100 54100 5 10 1 1 270 0 1
value=3 k
+T 64300 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 64200 55600 1 0 0 5V-plus-1.sym
N 64400 53400 64400 53000 4
@@ -1047,6 +1065,8 @@
refdes=R14
T 65200 54100 5 10 1 1 270 0 1
value=3 k
+T 65400 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 65300 55600 1 0 0 5V-plus-1.sym
N 65500 53400 65500 53200 4
@@ -1082,6 +1102,8 @@
refdes=R15
T 66500 54100 5 10 1 1 270 0 1
value=3 k
+T 66700 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 66600 55600 1 0 0 5V-plus-1.sym
N 67200 52800 66800 52800 4
@@ -1115,6 +1137,8 @@
refdes=R16
T 67700 54100 5 10 1 1 270 0 1
value=3 k
+T 67900 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 67800 55600 1 0 0 5V-plus-1.sym
N 68000 52800 68000 53400 4
@@ -1146,6 +1170,8 @@
refdes=R17
T 68900 54100 5 10 1 1 270 0 1
value=3 k
+T 69100 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 69000 55600 1 0 0 5V-plus-1.sym
N 69200 53400 69200 53000 4
@@ -1177,6 +1203,8 @@
refdes=R18
T 70000 54100 5 10 1 1 270 0 1
value=3 k
+T 70200 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 70100 55600 1 0 0 5V-plus-1.sym
N 70300 53400 70300 53200 4
@@ -1219,6 +1247,8 @@
refdes=R19
T 71200 54100 5 10 1 1 270 0 1
value=3 k
+T 71400 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
N 72600 55600 72600 55400 4
C 72800 54500 1 90 0 led-3.sym
@@ -1245,6 +1275,8 @@
refdes=R20
T 72300 54100 5 10 1 1 270 0 1
value=3 k
+T 72500 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 71300 55600 1 0 0 5V-plus-1.sym
C 72400 55600 1 0 0 5V-plus-1.sym
@@ -1289,6 +1321,8 @@
refdes=R21
T 50500 54100 5 10 1 1 270 0 1
value=3 k
+T 50700 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 50600 55600 1 0 0 5V-plus-1.sym
N 51200 52200 51200 56700 4
@@ -1324,6 +1358,8 @@
refdes=R22
T 51700 54100 5 10 1 1 270 0 1
value=3 k
+T 51900 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 51800 55600 1 0 0 5V-plus-1.sym
N 52000 52800 52000 53400 4
@@ -1356,6 +1392,8 @@
refdes=R23
T 52900 54100 5 10 1 1 270 0 1
value=3 k
+T 53100 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 53000 55600 1 0 0 5V-plus-1.sym
N 53200 53400 53200 53000 4
@@ -1388,6 +1426,8 @@
refdes=R24
T 54000 54100 5 10 1 1 270 0 1
value=3 k
+T 54200 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 54100 55600 1 0 0 5V-plus-1.sym
N 54300 53400 54300 53200 4
@@ -1421,6 +1461,8 @@
refdes=R25
T 55300 54100 5 10 1 1 270 0 1
value=3 k
+T 55500 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 55400 55600 1 0 0 5V-plus-1.sym
N 56000 52800 55600 52800 4
@@ -1454,6 +1496,8 @@
refdes=R26
T 56500 54100 5 10 1 1 270 0 1
value=3 k
+T 56700 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 56600 55600 1 0 0 5V-plus-1.sym
N 56800 52800 56800 53400 4
@@ -1485,6 +1529,8 @@
refdes=R27
T 57700 54100 5 10 1 1 270 0 1
value=3 k
+T 57900 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 57800 55600 1 0 0 5V-plus-1.sym
N 58000 53400 58000 53000 4
@@ -1516,6 +1562,8 @@
refdes=R28
T 58800 54100 5 10 1 1 270 0 1
value=3 k
+T 59000 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 58900 55600 1 0 0 5V-plus-1.sym
N 59100 53400 59100 53200 4
@@ -1553,6 +1601,8 @@
refdes=R29
T 48100 54100 5 10 1 1 270 0 1
value=3 k
+T 48300 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
N 49600 55600 49600 55400 4
C 49800 54500 1 90 0 led-3.sym
@@ -1579,6 +1629,8 @@
refdes=R30
T 49300 54100 5 10 1 1 270 0 1
value=3 k
+T 49500 54300 5 10 0 0 0 0 1
+pn=RC0603JR-073KL
}
C 48200 55600 1 0 0 5V-plus-1.sym
C 49400 55600 1 0 0 5V-plus-1.sym
@@ -1606,6 +1658,8 @@
refdes=R31
T 75600 39000 5 10 1 1 270 0 1
value=1.3 k
+T 75800 39200 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 75900 39400 75900 39200 4
N 74800 40500 74800 40300 4
@@ -1632,6 +1686,8 @@
refdes=R32
T 74500 39000 5 10 1 1 270 0 1
value=1.3 k
+T 74700 39200 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 74800 39400 74800 39200 4
N 73600 40500 73600 40300 4
@@ -1658,6 +1714,8 @@
refdes=R33
T 73300 39000 5 10 1 1 270 0 1
value=1.3 k
+T 73500 39200 5 10 0 0 0 0 1
+pn=RC0603JR-071K3L
}
N 73600 39400 73600 39200 4
N 72800 38100 72800 41800 4
@@ -1940,6 +1998,8 @@
value=120
T 61700 40000 5 10 0 0 0 0 1
power=.25 W
+T 61700 40000 5 10 0 0 0 0 1
+pn=ESR10EZPJ121
}
N 61700 40100 61500 40100 4
N 61500 40100 61500 41300 4
diff --git a/motors/hid_schematic/ordering.rb b/motors/hid_schematic/ordering.rb
new file mode 120000
index 0000000..48325a4
--- /dev/null
+++ b/motors/hid_schematic/ordering.rb
@@ -0,0 +1 @@
+../big_schematic/ordering.rb
\ No newline at end of file
diff --git a/motors/hid_schematic/parts.rb b/motors/hid_schematic/parts.rb
new file mode 120000
index 0000000..99f7624
--- /dev/null
+++ b/motors/hid_schematic/parts.rb
@@ -0,0 +1 @@
+../big_schematic/parts.rb
\ No newline at end of file
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index 03abd99..f8bfecf 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -23,7 +23,8 @@
// TODO(austin): Pseudo current limit?
ShooterController::ShooterController()
- : loop_(new StateFeedbackLoop<4, 1, 1, StateFeedbackHybridPlant<4, 1, 1>,
+ : loop_(new StateFeedbackLoop<4, 1, 1, double,
+ StateFeedbackHybridPlant<4, 1, 1>,
HybridKalman<4, 1, 1>>(
superstructure::shooter::MakeIntegralShooterLoop())) {
history_.fill(0);
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index d13d94c..3a9f3f9 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -50,8 +50,9 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<
- 4, 1, 1, StateFeedbackHybridPlant<4, 1, 1>, HybridKalman<4, 1, 1>>>
+ ::std::unique_ptr<
+ StateFeedbackLoop<4, 1, 1, double, StateFeedbackHybridPlant<4, 1, 1>,
+ HybridKalman<4, 1, 1>>>
loop_;
// History array for calculating a filtered angular velocity.