Merge "Checking in target_sender.cc. Renaming vision_data to avoid naming collision."
diff --git a/NO_BUILD_AMD64 b/NO_BUILD_AMD64
index 0a82064..69ace36 100644
--- a/NO_BUILD_AMD64
+++ b/NO_BUILD_AMD64
@@ -26,3 +26,4 @@
-//y2016_bot4:download_stripped
-//y2017:wpilib_interface
-//y2017:download_stripped
+-//y2017:download
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 8f8b381..7272d48 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -109,9 +109,10 @@
'state_feedback_loop.h',
],
deps = [
- '//third_party/eigen',
+ '//aos/common/controls:control_loop',
'//aos/common/logging',
'//aos/common:macros',
+ '//third_party/eigen',
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6a2822b..aa6cd60 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -112,14 +112,14 @@
break;
}
- kf_.set_controller_index(ControllerIndexFromGears());
+ kf_.set_index(ControllerIndexFromGears());
{
GearLogging gear_logging;
gear_logging.left_state = static_cast<uint32_t>(left_gear_);
gear_logging.right_state = static_cast<uint32_t>(right_gear_);
gear_logging.left_loop_high = MaybeHigh(left_gear_);
gear_logging.right_loop_high = MaybeHigh(right_gear_);
- gear_logging.controller_index = kf_.controller_index();
+ gear_logging.controller_index = kf_.index();
LOG_STRUCT(DEBUG, "state", gear_logging);
}
const bool is_latest_imu_values = ::frc971::imu_values.FetchLatest();
@@ -151,7 +151,7 @@
rate, angle, down_estimator_.X_hat(0, 0), down_estimator_.X_hat(1, 0));
down_U_(0, 0) = rate;
}
- down_estimator_.UpdateObserver(down_U_);
+ down_estimator_.UpdateObserver(down_U_, ::aos::controls::kLoopFrequency);
// TODO(austin): Signal the current gear to both loops.
@@ -292,7 +292,7 @@
last_left_voltage_ = left_voltage;
last_right_voltage_ = right_voltage;
- kf_.UpdateObserver(U);
+ kf_.UpdateObserver(U, ::aos::controls::kLoopFrequency);
}
void DrivetrainLoop::Zero(
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 41a4f3c..c66a4e3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -166,15 +166,15 @@
if (left_gear_high_) {
if (right_gear_high_) {
- drivetrain_plant_->set_plant_index(3);
+ drivetrain_plant_->set_index(3);
} else {
- drivetrain_plant_->set_plant_index(2);
+ drivetrain_plant_->set_index(2);
}
} else {
if (right_gear_high_) {
- drivetrain_plant_->set_plant_index(1);
+ drivetrain_plant_->set_index(1);
} else {
- drivetrain_plant_->set_plant_index(0);
+ drivetrain_plant_->set_index(0);
}
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index d3a220b..91c2fc9 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -1,16 +1,16 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/controls/polytope.h"
#include "aos/common/commonmath.h"
-#include "aos/common/logging/queue_logging.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/logging/logging.h"
#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "aos/common/messages/robot_state.q.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
@@ -22,13 +22,16 @@
U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
/*[*/ -1, 0 /*]*/,
/*[*/ 0, 1 /*]*/,
- /*[*/ 0, -1 /*]]*/).finished(),
+ /*[*/ 0, -1 /*]]*/)
+ .finished(),
(Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
- /*[*/ 12 /*]]*/).finished(),
+ /*[*/ 12 /*]]*/)
+ .finished(),
(Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
- /*[*/ -12, 12, 12, -12 /*]*/).finished()),
+ /*[*/ -12, 12, 12, -12 /*]*/)
+ .finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
ttrust_(1.1),
wheel_(0.0),
@@ -148,19 +151,19 @@
double PolyDrivetrain::FilterVelocity(double throttle) const {
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B.inverse() *
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A);
+ loop_->plant().coefficients(kHighGearController).A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const double min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
const double high_min_FF_sum = FF_high.col(0).sum();
const double adjusted_ff_voltage =
@@ -173,14 +176,14 @@
double PolyDrivetrain::MaxVelocity() {
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B.inverse() *
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A);
+ loop_->plant().coefficients(kHighGearController).A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
@@ -206,8 +209,8 @@
if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
// FF * X = U (steady state)
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
// Invert the plant to figure out how the velocity filter would have to
// work
@@ -241,11 +244,13 @@
// Construct a constraint on R by manipulating the constraint on U
::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
- U_Poly_.static_H() * (loop_->K() + FF),
- U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
- (loop_->K() + FF).inverse() *
- ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
- loop_->K() * loop_->X_hat()));
+ U_Poly_.static_H() * (loop_->controller().K() + FF),
+ U_Poly_.static_k() +
+ U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
+ (loop_->controller().K() + FF).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(
+ U_Poly_.StaticVertices(),
+ loop_->controller().K() * loop_->X_hat()));
// Limit R back inside the box.
loop_->mutable_R() =
@@ -254,7 +259,7 @@
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
const Eigen::Matrix<double, 2, 1> U_ideal =
- loop_->K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+ loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
for (int i = 0; i < 2; i++) {
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
@@ -262,7 +267,7 @@
if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
loop_->mutable_X_hat() =
- loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
+ loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
}
} else {
const double current_left_velocity =
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 20f1a49..570b87f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -17,7 +17,7 @@
PolyDrivetrain(const DrivetrainConfig &dt_config,
StateFeedbackLoop<7, 2, 3> *kf);
- int controller_index() const { return loop_->controller_index(); }
+ int controller_index() const { return loop_->index(); }
// Computes the speed of the motor given the hall effect position and the
// speed of the robot.
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index b7a68a9..4aeb74b 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -35,9 +35,11 @@
LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
- position_K << kf_->K(0, 0), kf_->K(0, 2), kf_->K(1, 0), kf_->K(1, 2);
+ position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
+ kf_->controller().K(1, 0), kf_->controller().K(1, 2);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << kf_->K(0, 1), kf_->K(0, 3), kf_->K(1, 1), kf_->K(1, 3);
+ velocity_K << kf_->controller().K(0, 1), kf_->controller().K(0, 3),
+ kf_->controller().K(1, 1), kf_->controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(2, 0);
@@ -144,7 +146,7 @@
goal.right_velocity_goal, 0.0, 0.0, 0.0;
use_profile_ =
- !kf_->Kff().isZero(0) &&
+ !kf_->controller().Kff().isZero(0) &&
(goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
goal.angular.max_velocity != 0.0 &&
goal.angular.max_acceleration != 0.0);
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 031795a..4d09bbc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -60,7 +60,7 @@
return *loop_;
}
- int controller_index() const { return loop_->controller_index(); }
+ int controller_index() const { return loop_->index(); }
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
@@ -259,8 +259,10 @@
status->estimator_state = this->EstimatorState(0);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- status->position_power = this->controller().K(0, 0) * error(0, 0);
- status->velocity_power = this->controller().K(0, 1) * error(1, 0);
+ status->position_power =
+ this->controller().controller().K(0, 0) * error(0, 0);
+ status->velocity_power =
+ this->controller().controller().K(0, 1) * error(1, 0);
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index fba883c..7301390 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -16,7 +16,9 @@
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
+ def __init__(self, gain_schedule_name, loops, namespaces=None,
+ write_constants=False, plant_type='StateFeedbackPlant',
+ observer_type='StateFeedbackObserver'):
"""Constructs a control loop writer.
Args:
@@ -25,6 +27,8 @@
in order.
namespaces: array[string], a list of names of namespaces to nest in
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.
"""
self._gain_schedule_name = gain_schedule_name
self._loops = loops
@@ -40,6 +44,8 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
self._constant_list = []
+ self._plant_type = plant_type
+ self._observer_type = observer_type
def AddConstant(self, constant):
"""Adds a constant to write.
@@ -62,29 +68,46 @@
self.WriteHeader(header_file)
self.WriteCC(os.path.basename(header_file), cc_file)
- def _GenericType(self, typename):
+ def _GenericType(self, typename, extra_args=None):
"""Returns a loop template using typename for the type."""
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 '%s<%d, %d, %d>' % (
- typename, num_states, num_inputs, num_outputs)
+ if extra_args is not None:
+ extra_args = ', ' + extra_args
+ else:
+ extra_args = ''
+ return '%s<%d, %d, %d%s>' % (
+ typename, num_states, num_inputs, num_outputs, extra_args)
def _ControllerType(self):
- """Returns a template name for StateFeedbackControllerConstants."""
- return self._GenericType('StateFeedbackControllerConstants')
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
+
+ def _ObserverType(self):
+ """Returns a template name for StateFeedbackObserver."""
+ return self._GenericType(self._observer_type)
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
- return self._GenericType('StateFeedbackLoop')
+ extra_args = '%s, %s' % (self._PlantType(), self._ObserverType())
+ return self._GenericType('StateFeedbackLoop', extra_args)
def _PlantType(self):
"""Returns a template name for StateFeedbackPlant."""
- return self._GenericType('StateFeedbackPlant')
+ return self._GenericType(self._plant_type)
- def _CoeffType(self):
+ def _PlantCoeffType(self):
"""Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType('StateFeedbackPlantCoefficients')
+ return self._GenericType(self._plant_type + 'Coefficients')
+
+ def _ControllerCoeffType(self):
+ """Returns a template name for StateFeedbackControllerCoefficients."""
+ return self._GenericType('StateFeedbackControllerCoefficients')
+
+ def _ObserverCoeffType(self):
+ """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.
@@ -104,14 +127,22 @@
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlantHeader())
+ fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
fd.write('\n')
fd.write(loop.DumpControllerHeader())
fd.write('\n')
+ fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
+ fd.write('\n')
fd.write('%s Make%sPlant();\n\n' %
(self._PlantType(), self._gain_schedule_name))
+ fd.write('%s Make%sController();\n\n' %
+ (self._ControllerType(), self._gain_schedule_name))
+
+ fd.write('%s Make%sObserver();\n\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+
fd.write('%s Make%sLoop();\n\n' %
(self._LoopType(), self._gain_schedule_name))
@@ -132,33 +163,55 @@
fd.write(self._namespace_start)
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlant())
+ fd.write(loop.DumpPlant(self._PlantCoeffType()))
fd.write('\n')
for loop in self._loops:
fd.write(loop.DumpController())
fd.write('\n')
+ for loop in self._loops:
+ fd.write(loop.DumpObserver(self._ObserverCoeffType()))
+ fd.write('\n')
+
fd.write('%s Make%sPlant() {\n' %
(self._PlantType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._CoeffType(), len(self._loops)))
+ self._PlantCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._CoeffType(), self._CoeffType(),
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
loop.PlantFunction()))
fd.write(' return %s(&plants);\n' % self._PlantType())
fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write('%s Make%sController() {\n' %
+ (self._ControllerType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerType(), len(self._loops)))
+ self._ControllerCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerType(), self._ControllerType(),
+ (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._LoopType())
+ fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sObserver() {\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
+ self._ObserverCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ObserverCoeffType(), self._ObserverCoeffType(),
+ loop.ObserverFunction()))
+ fd.write(' return %s(&observers);\n' % self._ObserverType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
+ fd.write(' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
+ (self._LoopType(), self._gain_schedule_name,
+ self._gain_schedule_name, self._gain_schedule_name))
fd.write('}\n\n')
fd.write(self._namespace_end)
@@ -250,43 +303,45 @@
return ''.join(ans)
- def DumpPlantHeader(self):
+ def DumpPlantHeader(self, plant_coefficient_type):
"""Writes out a c++ header declaration which will create a Plant object.
Returns:
string, The header declaration for the function.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
- num_states, num_inputs, num_outputs, self._name)
+ return '%s Make%sPlantCoefficients();\n' % (
+ plant_coefficient_type, self._name)
- def DumpPlant(self):
+ def DumpPlant(self, plant_coefficient_type):
"""Writes out a c++ function which will create a PlantCoefficients object.
Returns:
string, The function which will create the object.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
- ' Make%sPlantCoefficients() {\n' % (
- num_states, num_inputs, num_outputs, self._name)]
+ ans = ['%s Make%sPlantCoefficients() {\n' % (
+ plant_coefficient_type, self._name)]
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
- ans.append(self._DumpMatrix('B', self.B))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
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(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
- num_states, num_inputs, num_outputs))
+ 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(' 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(' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
+ plant_coefficient_type))
+ else:
+ glog.fatal('Unsupported plant type %s', plant_coefficient_type)
+
ans.append('}\n')
return ''.join(ans)
@@ -296,7 +351,11 @@
def ControllerFunction(self):
"""Returns the name of the controller function."""
- return 'Make%sController()' % self._name
+ return 'Make%sControllerCoefficients()' % self._name
+
+ def ObserverFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sObserverCoefficients()' % self._name
def DumpControllerHeader(self):
"""Writes out a c++ header declaration which will create a Controller object.
@@ -307,7 +366,7 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return 'StateFeedbackControllerConstants<%d, %d, %d> %s;\n' % (
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())
def DumpController(self):
@@ -319,19 +378,77 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ['StateFeedbackControllerConstants<%d, %d, %d> %s {\n' % (
+ ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())]
- ans.append(self._DumpMatrix('L', self.L))
ans.append(self._DumpMatrix('K', self.K))
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('A_inv', numpy.linalg.inv(self.A)))
- ans.append(' return StateFeedbackControllerConstants<%d, %d, %d>'
- '(L, K, Kff, A_inv, Make%sPlantCoefficients());\n' % (
- num_states, num_inputs, num_outputs, self._name))
+ ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d>'
+ '(K, Kff);\n' % (
+ num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
+
+ def DumpObserverHeader(self, observer_coefficient_type):
+ """Writes out a c++ header declaration which will create a Observer object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s %s;\n' % (
+ observer_coefficient_type, self.ObserverFunction())
+
+ def DumpObserver(self, observer_coefficient_type):
+ """Returns a c++ function which will create a Observer object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = ['%s %s {\n' % (
+ observer_coefficient_type, self.ObserverFunction())]
+
+ if observer_coefficient_type.startswith('StateFeedbackObserver'):
+ ans.append(self._DumpMatrix('L', self.L))
+ 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(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
+ observer_coefficient_type,))
+
+ ans.append('}\n')
+ return ''.join(ans)
+
+class HybridControlLoop(ControlLoop):
+ def __init__(self, name):
+ super(HybridControlLoop, self).__init__(name=name)
+
+ def Discritize(self, dt):
+ [self.A, self.B, self.Q, self.R] = \
+ controls.kalmd(self.A_continuous, self.B_continuous,
+ self.Q_continuous, self.R_continuous, dt)
+
+ def PredictHybridObserver(self, U, dt):
+ self.Discritize(dt)
+ self.X_hat = self.A * self.X_hat + self.B * U
+ self.P = (self.A * self.P * self.A.T + self.Q)
+
+ def CorrectHybridObserver(self, U):
+ Y_bar = self.Y - self.C * self.X_hat
+ C_t = self.C.T
+ S = self.C * self.P * C_t + self.R
+ self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
+ self.X_hat = self.X_hat + self.KalmanGain * Y_bar
+ self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+
+ def InitializeState(self):
+ super(HybridControlLoop, self).InitializeState()
+ if hasattr(self, 'Q_steady_state'):
+ self.P = self.Q_steady_state
+ else:
+ self.P = numpy.matrix(numpy.zeros((self.A.shape[0], self.A.shape[0])))
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 211b478..7b0317d 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -151,6 +151,49 @@
return K, P
+
+def kalmd(A_continuous, B_continuous, Q_continuous, R_continuous, dt):
+ """Converts a continuous time kalman filter to discrete time.
+
+ Args:
+ A_continuous: The A continuous matrix
+ B_continuous: the B continuous matrix
+ Q_continuous: The continuous cost matrix
+ R_continuous: The R continuous matrix
+ dt: Timestep
+
+ The math for this is from:
+ https://www.mathworks.com/help/control/ref/kalmd.html
+
+ Returns:
+ The discrete matrices of A, B, Q, and R.
+ """
+ # TODO(austin): Verify that the dimensions make sense.
+ number_of_states = A_continuous.shape[0]
+ number_of_inputs = B_continuous.shape[1]
+ M = numpy.zeros((len(A_continuous) + number_of_inputs,
+ len(A_continuous) + number_of_inputs))
+ M[0:number_of_states, 0:number_of_states] = A_continuous
+ M[0:number_of_states, number_of_states:] = B_continuous
+ M_exp = scipy.linalg.expm(M * dt)
+ A_discrete = M_exp[0:number_of_states, 0:number_of_states]
+ B_discrete = numpy.matrix(M_exp[0:number_of_states, number_of_states:])
+ Q_continuous = (Q_continuous + Q_continuous.T) / 2.0
+ R_continuous = (R_continuous + R_continuous.T) / 2.0
+ M = numpy.concatenate((-A_continuous, Q_continuous), axis=1)
+ M = numpy.concatenate(
+ (M, numpy.concatenate((numpy.matrix(
+ numpy.zeros((number_of_states, number_of_states))),
+ numpy.transpose(A_continuous)), axis = 1)), axis = 0)
+ phi = numpy.matrix(scipy.linalg.expm(M*dt))
+ 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]
+ Q_discrete = phi22.T * phi12
+ Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
+ R_discrete = R_continuous / dt
+ return (A_discrete, B_discrete, Q_discrete, R_discrete)
+
+
def TwoStateFeedForwards(B, Q):
"""Computes the feed forwards constant for a 2 state controller.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 3b935e2..6d05444 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -3,15 +3,23 @@
#include <assert.h>
-#include <vector>
-#include <memory>
#include <iostream>
+#include <memory>
+#include <utility>
+#include <vector>
+#include <chrono>
#include "Eigen/Dense"
+#include "unsupported/Eigen/MatrixFunctions"
+#include "aos/common/controls/control_loop.h"
#include "aos/common/logging/logging.h"
#include "aos/common/macros.h"
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename PlantType, typename ObserverType>
+class StateFeedbackLoop;
+
// For everything in this file, "inputs" and "outputs" are defined from the
// perspective of the plant. This means U is an input and Y is an output
// (because you give the plant U (powers) and it gives you back a Y (sensor
@@ -26,9 +34,8 @@
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A(other.A),
- A_continuous(other.A_continuous),
+ A_inv(other.A_inv),
B(other.B),
- B_continuous(other.B_continuous),
C(other.C),
D(other.D),
U_min(other.U_min),
@@ -36,28 +43,17 @@
StateFeedbackPlantCoefficients(
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_states> &A_inv,
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_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_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_continuous;
+ 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_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;
@@ -73,12 +69,12 @@
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>>
*coefficients)
- : coefficients_(::std::move(*coefficients)), plant_index_(0) {
+ : coefficients_(::std::move(*coefficients)), index_(0) {
Reset();
}
StateFeedbackPlant(StateFeedbackPlant &&other)
- : plant_index_(other.plant_index_) {
+ : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
@@ -90,6 +86,10 @@
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 {
+ 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 {
return coefficients().B;
}
@@ -122,16 +122,22 @@
double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> &
- coefficients() const {
- return *coefficients_[plant_index_];
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
}
- int plant_index() const { return plant_index_; }
- void set_plant_index(int plant_index) {
- assert(plant_index >= 0);
- assert(plant_index < static_cast<int>(coefficients_.size()));
- plant_index_ = plant_index;
+ const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ int index() const { return index_; }
+ void set_index(int index) {
+ assert(index >= 0);
+ assert(index < static_cast<int>(coefficients_.size()));
+ index_ = index;
}
void Reset() {
@@ -153,10 +159,16 @@
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
- X_ = A() * X() + B() * U;
+ X_ = Update(X(), 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 {
+ return A() * X + B() * U;
+ }
+
protected:
// these are accessible from non-templated subclasses
static const int kNumStates = number_of_states;
@@ -171,107 +183,609 @@
number_of_states, number_of_inputs, number_of_outputs>>>
coefficients_;
- int plant_index_;
+ int index_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackPlant);
};
-// 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.
+// A container for all the controller coefficients.
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-struct StateFeedbackControllerConstants final {
+struct StateFeedbackControllerCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- 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_inputs, number_of_states> Kff;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
- StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
- plant;
- StateFeedbackControllerConstants(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ 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<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(Kff), A_inv(A_inv), plant(plant) {}
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
+ : K(K), Kff(Kff) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-class StateFeedbackLoop {
+struct StateFeedbackHybridPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- StateFeedbackLoop(
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
- number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
- : controllers_(::std::move(*controllers)), controller_index_(0) {
+ StateFeedbackHybridPlantCoefficients(
+ const StateFeedbackHybridPlantCoefficients &other)
+ : A_continuous(other.A_continuous),
+ B_continuous(other.B_continuous),
+ C(other.C),
+ D(other.D),
+ U_min(other.U_min),
+ U_max(other.U_max) {}
+
+ StateFeedbackHybridPlantCoefficients(
+ 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_max,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+ : A_continuous(A_continuous),
+ 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_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;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackHybridPlant {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ StateFeedbackHybridPlant(
+ ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ *coefficients)
+ : coefficients_(::std::move(*coefficients)), index_(0) {
Reset();
}
- StateFeedbackLoop(StateFeedbackLoop &&other) {
- X_hat_.swap(other.X_hat_);
- R_.swap(other.R_);
- next_R_.swap(other.next_R_);
- U_.swap(other.U_);
- U_uncapped_.swap(other.U_uncapped_);
- ff_U_.swap(other.ff_U_);
- ::std::swap(controllers_, other.controllers_);
- controller_index_ = other.controller_index_;
+ StateFeedbackHybridPlant(StateFeedbackHybridPlant &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ X_.swap(other.X_);
+ Y_.swap(other.Y_);
}
- virtual ~StateFeedbackLoop() {}
+ virtual ~StateFeedbackHybridPlant() {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return controller().plant.A;
+ 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 {
- return controller().plant.B;
+ return B_;
}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv()
- const {
- return controller().A_inv;
- }
- 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 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 controller().plant.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 controller().plant.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 controller().plant.U_max;
+ return coefficients().U_max;
}
double 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); }
+
+ 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); }
+
+ const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ int index() const { return index_; }
+ void set_index(int index) {
+ assert(index >= 0);
+ assert(index < static_cast<int>(coefficients_.size()));
+ index_ = index;
+ }
+
+ void Reset() {
+ X_.setZero();
+ Y_.setZero();
+ A_.setZero();
+ B_.setZero();
+ UpdateAB(::aos::controls::kLoopFrequency);
+ }
+
+ // Assert that U is within the hardware range.
+ virtual void CheckU(const Eigen::Matrix<double, 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) {
+ 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,
+ ::std::chrono::nanoseconds dt) {
+ // Powers outside of the range are more likely controller bugs than things
+ // that the plant should deal with.
+ CheckU(U);
+ X_ = Update(X(), U, dt);
+ 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,
+ ::std::chrono::nanoseconds dt) {
+ UpdateAB(dt);
+ return A() * X + B() * U;
+ }
+
+ 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;
+
+ private:
+ void UpdateAB(::std::chrono::nanoseconds dt) {
+ Eigen::Matrix<double, 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)
+ .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)
+ .count();
+
+ Eigen::Matrix<double, 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);
+ B_ = M_state.template block<number_of_states, number_of_inputs>(
+ 0, number_of_states);
+ }
+
+ Eigen::Matrix<double, number_of_states, 1> X_;
+ Eigen::Matrix<double, 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_;
+
+
+ ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+
+ int index_;
+
+ DISALLOW_COPY_AND_ASSIGN(StateFeedbackHybridPlant);
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+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)
+ : coefficients_(::std::move(*controllers)) {}
+
+ StateFeedbackController(StateFeedbackController &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
- return controller().K;
+ 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 {
- return controller().Kff;
+ return coefficients().Kff;
}
double Kff(int i, int j) const { return Kff()(i, j); }
+
+ void Reset() {}
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+// A container for all the observer coefficients.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct StateFeedbackObserverCoefficients final {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+
+ StateFeedbackObserverCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L)
+ : L(L) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+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)
+ : coefficients_(::std::move(*observers)) {}
+
+ StateFeedbackObserver(StateFeedbackObserver &&other)
+ : X_hat_(other.X_hat_), index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
- return controller().L;
+ return coefficients().L;
}
double L(int i, int j) const { return L()(i, j); }
const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
return X_hat_;
}
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+
+ void Reset(
+ StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> * /*loop*/) {
+ X_hat_.setZero();
+ }
+
+ void Predict(
+ StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> *loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds /*dt*/) {
+ mutable_X_hat() = loop->plant().Update(X_hat(), new_u);
+ }
+
+ void Correct(const StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> &loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ mutable_X_hat() += loop.plant().A_inv() * L() *
+ (Y - loop.plant().C() * X_hat() - loop.plant().D() * U);
+ }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ // Internal state estimate.
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+// A container for all the observer coefficients.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+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;
+
+ HybridKalmanCoefficients(
+ 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)
+ : 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>
+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)
+ : coefficients_(::std::move(*observers)) {}
+
+ HybridKalman(HybridKalman &&other)
+ : X_hat_(other.X_hat_), index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ // Getters for Q
+ const Eigen::Matrix<double, number_of_states, number_of_states> &Q() const {
+ return Q_;
+ }
+ double 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 {
+ return R_;
+ }
+ double 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 {
+ return P_;
+ }
+ double 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 {
+ return X_hat_;
+ }
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+
+ void Reset(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop) {
+ X_hat_.setZero();
+ P_ = coefficients().P_steady_state;
+ UpdateQR(loop, ::aos::controls::kLoopFrequency);
+ }
+
+ void Predict(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop,
+ const Eigen::Matrix<double, 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() = loop->mutable_plant()->Update(X_hat(), new_u, dt);
+
+ UpdateQR(loop, dt);
+ P_ = loop->plant().A() * P_ * loop->plant().A().transpose() + Q_;
+ }
+
+ void Correct(const StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> &loop,
+ 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 =
+ Y - (loop.plant().C() * X_hat_ + loop.plant().D() * U);
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> S =
+ loop.plant().C() * P_ * loop.plant().C().transpose() + R_;
+ Eigen::Matrix<double, number_of_states, number_of_outputs> KalmanGain;
+ KalmanGain = (S.transpose().ldlt().solve(
+ (P() * loop.plant().C().transpose()).transpose()))
+ .transpose();
+ X_hat_ = X_hat_ + KalmanGain * Y_bar;
+ P_ = (loop.plant().coefficients().A_continuous.Identity() -
+ KalmanGain * loop.plant().C()) *
+ P();
+ }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const HybridKalmanCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const HybridKalmanCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ void UpdateQR(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop,
+ ::std::chrono::nanoseconds dt) {
+ // Now, compute the discrete time Q and R coefficients.
+ Eigen::Matrix<double, 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 =
+ (coefficients().R_continuous +
+ coefficients().R_continuous.transpose()) /
+ 2.0;
+
+ Eigen::Matrix<double, 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) =
+ -loop->plant().coefficients().A_continuous;
+ M_gain.template block<number_of_states, number_of_states>(
+ 0, number_of_states) = Qtemp;
+ M_gain.template block<number_of_states, number_of_states>(
+ number_of_states, number_of_states) =
+ loop->plant().coefficients().A_continuous.transpose();
+
+ Eigen::Matrix<double, 2 * number_of_states, 2 *number_of_states> phi =
+ (M_gain *
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(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 =
+ phi.block(0, number_of_states, number_of_states, number_of_states);
+ Eigen::Matrix<double, 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;
+ R_ = Rtemp /
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count();
+ }
+
+ // Internal state estimate.
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ // Internal covariance estimate.
+ Eigen::Matrix<double, 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_;
+
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename PlantType = StateFeedbackPlant<
+ number_of_states, number_of_inputs, number_of_outputs>,
+ typename ObserverType = StateFeedbackObserver<
+ number_of_states, number_of_inputs, number_of_outputs>>
+class StateFeedbackLoop {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit StateFeedbackLoop(
+ PlantType &&plant,
+ StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &&controller,
+ ObserverType &&observer)
+ : plant_(::std::move(plant)),
+ controller_(::std::move(controller)),
+ observer_(::std::move(observer)) {
+ Reset();
+ }
+
+ StateFeedbackLoop(StateFeedbackLoop &&other)
+ : plant_(::std::move(other.plant_)),
+ controller_(::std::move(other.controller_)),
+ observer_(::std::move(other.observer_)) {
+ R_.swap(other.R_);
+ next_R_.swap(other.next_R_);
+ U_.swap(other.U_);
+ U_uncapped_.swap(other.U_uncapped_);
+ ff_U_.swap(other.ff_U_);
+ }
+
+ virtual ~StateFeedbackLoop() {}
+
+ const Eigen::Matrix<double, 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); }
@@ -290,7 +804,9 @@
}
double ff_U(int i, int j) const { return ff_U()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Eigen::Matrix<double, 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); }
@@ -307,42 +823,44 @@
return mutable_U_uncapped()(i, j);
}
- const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
- number_of_outputs>
+ const PlantType &plant() const { return plant_; }
+ PlantType *mutable_plant() { return &plant_; }
+
+ const StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>
&controller() const {
- return *controllers_[controller_index_];
+ return controller_;
}
- const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
- number_of_outputs>
- &controller(int index) const {
- return *controllers_[index];
- }
+ const ObserverType &observer() const { return observer_; }
void Reset() {
- X_hat_.setZero();
R_.setZero();
next_R_.setZero();
U_.setZero();
U_uncapped_.setZero();
ff_U_.setZero();
+
+ plant_.Reset();
+ controller_.Reset();
+ observer_.Reset(this);
}
// If U is outside the hardware range, limit it before the plant tries to use
// it.
virtual void CapU() {
for (int i = 0; i < kNumInputs; ++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);
+ if (U(i, 0) > plant().U_max(i, 0)) {
+ U_(i, 0) = plant().U_max(i, 0);
+ } else if (U(i, 0) < plant().U_min(i, 0)) {
+ U_(i, 0) = plant().U_min(i, 0);
}
}
}
// Corrects X_hat given the observation in Y.
void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
- X_hat_ += A_inv() * L() * (Y - C() * X_hat_ - D() * U());
+ observer_.Correct(*this, U(), Y);
}
const Eigen::Matrix<double, number_of_states, 1> error() const {
@@ -351,17 +869,20 @@
// Returns the calculated controller power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
+ // TODO(austin): Should this live in StateSpaceController?
ff_U_ = FeedForward();
- return K() * error() + ff_U_;
+ return controller().K() * error() + ff_U_;
}
// Calculates the feed forwards power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
- return Kff() * (next_R() - A() * R());
+ // TODO(austin): Should this live in StateSpaceController?
+ return controller().Kff() * (next_R() - plant().A() * R());
}
// stop_motors is whether or not to output all 0s.
- void Update(bool stop_motors) {
+ void Update(bool stop_motors,
+ ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(5)) {
if (stop_motors) {
U_.setZero();
U_uncapped_.setZero();
@@ -371,7 +892,7 @@
CapU();
}
- UpdateObserver(U_);
+ UpdateObserver(U_, dt);
UpdateFFReference();
}
@@ -379,32 +900,32 @@
// Updates R() after any CapU operations happen on U().
void UpdateFFReference() {
ff_U_ -= U_uncapped() - U();
- if (!Kff().isZero(0)) {
- R_ = A() * R() + B() * ff_U_;
+ if (!controller().Kff().isZero(0)) {
+ R_ = plant().A() * R() + plant().B() * ff_U_;
}
}
- void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
- X_hat_ = A() * X_hat() + B() * new_u;
+ void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt) {
+ observer_.Predict(this, new_u, dt);
}
- // Sets the current controller to be index, clamped to be within range.
- void set_controller_index(int index) {
- if (index < 0) {
- controller_index_ = 0;
- } else if (index >= static_cast<int>(controllers_.size())) {
- controller_index_ = static_cast<int>(controllers_.size()) - 1;
- } else {
- controller_index_ = index;
- }
+ // Sets the current controller to be index.
+ void set_index(int index) {
+ plant_.set_index(index);
+ controller_.set_index(index);
+ observer_.set_index(index);
}
- int controller_index() const { return controller_index_; }
+ int index() const { return plant_.index(); }
protected:
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
- number_of_states, number_of_inputs, number_of_outputs>>>
- controllers_;
+ PlantType plant_;
+
+ StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs>
+ controller_;
+
+ ObserverType observer_;
// These are accessible from non-templated subclasses.
static constexpr int kNumStates = number_of_states;
@@ -415,8 +936,6 @@
Eigen::Matrix<double, number_of_inputs, 1> ff_U_;
private:
- // Internal state estimate.
- Eigen::Matrix<double, number_of_states, 1> X_hat_;
// Current goal (Used by the feed-back controller).
Eigen::Matrix<double, number_of_states, 1> R_;
// Goal to go to in the next cycle (Used by Feed-Forward controller.)
@@ -426,8 +945,6 @@
// Computed output before being capped.
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
- int controller_index_;
-
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 47434e4..072d044 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -6,6 +6,114 @@
namespace control_loops {
namespace testing {
+StateFeedbackHybridPlantCoefficients<3, 1, 1>
+MakeIntegralShooterPlantCoefficients() {
+ Eigen::Matrix<double, 1, 3> C;
+ C(0, 0) = 1.0;
+ C(0, 1) = 0.0;
+ C(0, 2) = 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D(0, 0) = 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max(0, 0) = 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min(0, 0) = -12.0;
+ Eigen::Matrix<double, 3, 3> A_continuous;
+ A_continuous(0, 0) = 0.0;
+ A_continuous(0, 1) = 1.0;
+ A_continuous(0, 2) = 0.0;
+ A_continuous(1, 0) = 0.0;
+ A_continuous(1, 1) = -8.1021414789556374;
+ A_continuous(1, 2) = 443.75;
+ A_continuous(2, 0) = 0.0;
+ A_continuous(2, 1) = 0.0;
+ A_continuous(2, 2) = 0.0;
+ Eigen::Matrix<double, 3, 1> B_continuous;
+ B_continuous(0, 0) = 0.0;
+ B_continuous(1, 0) = 443.75;
+ B_continuous(2, 0) = 0.0;
+ return StateFeedbackHybridPlantCoefficients<3, 1, 1>(
+ A_continuous, B_continuous, C, D, U_max, U_min);
+}
+
+StateFeedbackControllerCoefficients<3, 1, 1>
+MakeIntegralShooterControllerCoefficients() {
+ Eigen::Matrix<double, 1, 3> K;
+ K(0, 0) = 0.0;
+ K(0, 1) = 0.027731156542808996;
+ K(0, 2) = 1.0;
+ Eigen::Matrix<double, 1, 3> Kff;
+ Kff(0, 0) = 0.0;
+ Kff(0, 1) = 0.45989503537638587;
+ Kff(0, 2) = 0.0;
+ return StateFeedbackControllerCoefficients<3, 1, 1>(K, Kff);
+}
+
+HybridKalmanCoefficients<3, 1, 1> MakeIntegralShooterObserverCoefficients() {
+ Eigen::Matrix<double, 3, 3> Q_continuous;
+ Q_continuous(0, 0) = 0.0001;
+ Q_continuous(0, 1) = 0.0;
+ Q_continuous(0, 2) = 0.0;
+ Q_continuous(1, 0) = 0.0;
+ Q_continuous(1, 1) = 4.0;
+ Q_continuous(1, 2) = 0.0;
+ Q_continuous(2, 0) = 0.0;
+ Q_continuous(2, 1) = 0.0;
+ Q_continuous(2, 2) = 0.040000000000000008;
+ Eigen::Matrix<double, 1, 1> R_continuous;
+ R_continuous(0, 0) = 9.9999999999999995e-07;
+ Eigen::Matrix<double, 3, 3> P_steady_state;
+ P_steady_state(0, 0) = 7.1645559451160497e-05;
+ P_steady_state(0, 1) = 0.0031205034236441768;
+ P_steady_state(0, 2) = 0.00016022137220036598;
+ P_steady_state(1, 0) = 0.0031205034236441768;
+ P_steady_state(1, 1) = 0.25313549121689616;
+ P_steady_state(1, 2) = 0.015962850974712596;
+ P_steady_state(2, 0) = 0.00016022137220036598;
+ P_steady_state(2, 1) = 0.015962850974712596;
+ P_steady_state(2, 2) = 0.0019821816120708254;
+ return HybridKalmanCoefficients<3, 1, 1>(Q_continuous, R_continuous,
+ P_steady_state);
+}
+
+StateFeedbackHybridPlant<3, 1, 1> MakeIntegralShooterPlant() {
+ ::std::vector<
+ ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>>
+ plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>(
+ new StateFeedbackHybridPlantCoefficients<3, 1, 1>(
+ MakeIntegralShooterPlantCoefficients()));
+ return StateFeedbackHybridPlant<3, 1, 1>(&plants);
+}
+
+StateFeedbackController<3, 1, 1> MakeIntegralShooterController() {
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>>
+ controllers(1);
+ controllers[0] =
+ ::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>(
+ new StateFeedbackControllerCoefficients<3, 1, 1>(
+ MakeIntegralShooterControllerCoefficients()));
+ return StateFeedbackController<3, 1, 1>(&controllers);
+}
+
+HybridKalman<3, 1, 1> MakeIntegralShooterObserver() {
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>> observers(
+ 1);
+ observers[0] = ::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>(
+ new HybridKalmanCoefficients<3, 1, 1>(
+ MakeIntegralShooterObserverCoefficients()));
+ return HybridKalman<3, 1, 1>(&observers);
+}
+
+StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+MakeIntegralShooterLoop() {
+ return StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(
+ MakeIntegralShooterPlant(), MakeIntegralShooterController(),
+ MakeIntegralShooterObserver());
+}
+
// Tests that everything compiles and nothing crashes even if
// number_of_inputs!=number_of_outputs.
// There used to be lots of bugs in this area.
@@ -16,33 +124,66 @@
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 2, 4>::Identity(),
- Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
Eigen::Matrix<double, 4, 1>::Constant(1),
Eigen::Matrix<double, 4, 1>::Constant(-1));
- {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>> v;
- v.emplace_back(new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
- StateFeedbackPlant<2, 4, 7> plant(&v);
- plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
- plant.Reset();
- plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
- }
- {
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<2, 4, 7>>>
- v;
- v.emplace_back(new 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));
- StateFeedbackLoop<2, 4, 7> test_loop(&v);
- test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
- test_loop.Update(false);
- test_loop.CapU();
- }
+ // Build a plant.
+ ::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
+ v_plant;
+ v_plant.emplace_back(
+ new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
+ StateFeedbackPlant<2, 4, 7> plant(&v_plant);
+ plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
+ plant.Reset();
+ plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
+
+ // Now build a controller.
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<2, 4, 7>>>
+ v_controller;
+ v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity()));
+ StateFeedbackController<2, 4, 7> controller(&v_controller);
+
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
+ v_observer;
+ v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity()));
+ StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+
+ StateFeedbackLoop<2, 4, 7> test_loop(
+ ::std::move(plant), ::std::move(controller), ::std::move(observer));
+ test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
+ test_loop.Update(false);
+ test_loop.CapU();
+}
+
+// Tests that the continuous to discrete calculation for the kalman filter
+// matches what was computed both in Python and in Matlab.
+TEST(StateFeedbackLoopTest, PythonMatch) {
+ auto test_loop = MakeIntegralShooterLoop();
+ test_loop.Update(false, ::std::chrono::milliseconds(5));
+
+ Eigen::Matrix<double, 3, 3> A_discrete;
+ A_discrete << 1, 0.00490008, 0.00547272, 0, 0.96029888, 2.17440921, 0, 0, 1;
+
+ Eigen::Matrix<double, 3, 1> B_discrete;
+ B_discrete << 0.00547272, 2.17440921, 0;
+
+ Eigen::Matrix<double, 3, 3> Q_discrete;
+ Q_discrete << 6.62900602e-07, 4.86205253e-05, 3.66076676e-07, 4.86205253e-05,
+ 1.95296358e-02, 2.18908995e-04, 3.66076676e-07, 2.18908995e-04,
+ 2.00000000e-04;
+
+ Eigen::Matrix<double, 1, 1> R_discrete;
+ R_discrete << 0.0002;
+
+ EXPECT_TRUE(A_discrete.isApprox(test_loop.plant().A(), 0.001));
+ EXPECT_TRUE(B_discrete.isApprox(test_loop.plant().B(), 0.001));
+ EXPECT_TRUE(Q_discrete.isApprox(test_loop.observer().Q(), 0.001));
+ EXPECT_TRUE(R_discrete.isApprox(test_loop.observer().R(), 0.001));
}
} // namespace testing
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 7ab4f3d..418b591 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -24,11 +24,11 @@
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
loop_(constants::GetValues().make_drivetrain_loop()) {
- loop_.set_controller_index(3);
+ loop_.set_index(3);
}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = loop_.K();
+ static const auto &K = loop_.controller().K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 9320e6b..86ec084 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -106,11 +106,11 @@
// (H * position_K) * position_error <= k - H * UVel
Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 1),
- K(1, 0), K(1, 1);
+ position_K << controller().K(0, 0), controller().K(0, 1),
+ controller().K(1, 0), controller().K(1, 1);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 2), K(0, 3),
- K(1, 2), K(1, 3);
+ velocity_K << controller().K(0, 2), controller().K(0, 3),
+ controller().K(1, 2), controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(1, 0);
@@ -924,19 +924,19 @@
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::max(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup."
" Uncapped is %f, max is %f, difference is %f\n",
@@ -946,19 +946,19 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::min(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index d51049d..8444cea 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -542,7 +542,8 @@
claw_motor_.top_claw_goal_ - claw_motor_.bottom_claw_goal_, 0.0,
0.0;
Eigen::Matrix<double, 2, 1> uncapped_voltage =
- claw_motor_.claw_.K() * (R - claw_motor_.claw_.X_hat());
+ claw_motor_.claw_.controller().K() *
+ (R - claw_motor_.claw_.X_hat());
// Use a factor of 1.8 because so long as it isn't actually running
// away, the CapU function will deal with getting the actual output
// down.
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index af310f0..70a6cad 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -56,13 +56,14 @@
void ZeroedStateFeedbackLoop::CapGoal() {
if (uncapped_voltage() > max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() - max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
@@ -70,13 +71,14 @@
::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() + max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
@@ -88,10 +90,11 @@
}
void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
- if (controller_index() == 0) {
- mutable_R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
+ if (index() == 0) {
+ mutable_R(2, 0) = (-plant().A(1, 0) / plant().A(1, 2) * R(0, 0) -
+ plant().A(1, 1) / plant().A(1, 2) * R(1, 0));
} else {
- mutable_R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
+ mutable_R(2, 0) = -plant().A(1, 1) / plant().A(1, 2) * R(1, 0);
}
}
@@ -104,8 +107,8 @@
mutable_X_hat(0, 0) += doffset;
// Offset the goal so we don't move.
mutable_R(0, 0) += doffset;
- if (controller_index() == 0) {
- mutable_R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
+ if (index() == 0) {
+ mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
LOG_STRUCT(DEBUG, "sensor edge (fake?)",
::y2014::control_loops::ShooterChangeCalibration(
@@ -257,16 +260,16 @@
// Probably not needed yet.
if (position) {
- int last_controller_index = shooter_.controller_index();
+ int last_index = shooter_.index();
if (position->plunger && position->latch) {
// Use the controller without the spring if the latch is set and the
// plunger is back
- shooter_.set_controller_index(1);
+ shooter_.set_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(0);
+ shooter_.set_index(0);
}
- if (shooter_.controller_index() != last_controller_index) {
+ if (shooter_.index() != last_index) {
shooter_.RecalculatePowerGoal();
}
}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index fb991e3..f2ddbc8 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -83,8 +83,9 @@
desired_velocity);
mutable_R() << desired_position - kPositionOffset, desired_velocity,
- (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
- A(1, 1) / A(1, 2) * desired_velocity);
+ (-plant().A(1, 0) / plant().A(1, 2) *
+ (desired_position - kPositionOffset) -
+ plant().A(1, 1) / plant().A(1, 2) * desired_velocity);
}
double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index 5463265..df3f5fd 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -102,12 +102,12 @@
// Only disengage the spring if we are greater than 0, which is where the
// latch will take the load off the pusher.
if (GetAbsolutePosition() > 0.0) {
- shooter_plant_->set_plant_index(1);
+ shooter_plant_->set_index(1);
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
}
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
@@ -228,7 +228,7 @@
U << last_voltage_;
shooter_plant_->Update(U);
}
- LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
+ LOG(DEBUG, "Plant index is %d\n", shooter_plant_->index());
// Handle latch hall effect
if (!latch_piston_state_ && latch_delay_count_ > 0) {
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 8772af7..91873eb 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -25,7 +25,8 @@
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = constants::GetValues().make_drivetrain_loop().K();
+ static const auto K =
+ constants::GetValues().make_drivetrain_loop().controller().K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index 25b5b9d..6a56105 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -25,7 +25,7 @@
double ClawCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
// Compute K matrix to compensate for position errors.
- double Kp = K(0, 0);
+ double Kp = controller().K(0, 0);
// Compute how much we need to change R in order to achieve the change in U
// that was observed.
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index a6acae7..d34cd33 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -39,8 +39,8 @@
// Compute the K matrix used to compensate for position errors.
Eigen::Matrix<double, 2, 2> Kp;
Kp.setZero();
- Kp.col(0) = this->K().col(0);
- Kp.col(1) = this->K().col(2);
+ Kp.col(0) = this->controller().K().col(0);
+ Kp.col(1) = this->controller().K().col(2);
Eigen::Matrix<double, 2, 2> Kp_inv = Kp.inverse();
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 29a55a6..b601853 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -112,7 +112,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -129,7 +129,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceControllerPoles([0.7, 0.7])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -178,7 +178,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 313a818..f9587a0 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -30,7 +30,9 @@
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
static const auto K =
- ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop().K();
+ ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop()
+ .controller()
+ .K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2015_bot3/control_loops/elevator/elevator.cc b/y2015_bot3/control_loops/elevator/elevator.cc
index d8b1368..eb3d870 100644
--- a/y2015_bot3/control_loops/elevator/elevator.cc
+++ b/y2015_bot3/control_loops/elevator/elevator.cc
@@ -20,7 +20,7 @@
double SimpleCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
// Compute K matrix to compensate for position errors.
- double Kp = K(0, 0);
+ double Kp = controller().K(0, 0);
// Compute how much we need to change R in order to achieve the change in U
// that was observed.
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index 1d2a74e..8efe374 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -112,7 +112,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -129,7 +129,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceControllerPoles([0.7, 0.7])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -178,7 +178,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 3bf0966..2366fb7 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -670,16 +670,22 @@
// Calculate the loops for a cycle.
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power = intake_.controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power = intake_.controller().K(0, 1) * error(1, 0);
+ status->intake.position_power =
+ intake_.controller().controller().K(0, 0) * error(0, 0);
+ status->intake.velocity_power =
+ intake_.controller().controller().K(0, 1) * error(1, 0);
}
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power = arm_.controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power = arm_.controller().K(0, 1) * error(1, 0);
- status->wrist.position_power = arm_.controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power = arm_.controller().K(0, 3) * error(3, 0);
+ status->shoulder.position_power =
+ arm_.controller().controller().K(0, 0) * error(0, 0);
+ status->shoulder.velocity_power =
+ arm_.controller().controller().K(0, 1) * error(1, 0);
+ status->wrist.position_power =
+ arm_.controller().controller().K(0, 2) * error(2, 0);
+ status->wrist.velocity_power =
+ arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 6f853d6..5d8c85a 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -28,14 +28,16 @@
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().coefficients(0).Kff *
+ (next_R() - plant().coefficients(0).A * R());
const Eigen::Matrix<double, 2, 1> accelerating_controller =
- controller(0).K * error() + accelerating_ff;
+ controller().coefficients(0).K * error() + accelerating_ff;
const Eigen::Matrix<double, 2, 1> decelerating_ff =
- controller(1).Kff * (next_R() - controller(1).plant.A * R());
+ controller().coefficients(1).Kff *
+ (next_R() - plant().coefficients(1).A * R());
const Eigen::Matrix<double, 2, 1> decelerating_controller =
- controller(1).K * error() + decelerating_ff;
+ controller().coefficients(1).K * error() + decelerating_ff;
const double bemf_voltage = X_hat(1, 0) / kV_shoulder;
bool use_accelerating_controller = true;
@@ -48,11 +50,11 @@
use_accelerating_controller = false;
}
if (use_accelerating_controller) {
+ set_index(0);
ff_U_ = accelerating_ff;
- set_controller_index(0);
return accelerating_controller;
} else {
- set_controller_index(1);
+ set_index(1);
ff_U_ = decelerating_ff;
return decelerating_controller;
}
@@ -66,18 +68,18 @@
if (U(0, 0) > max_voltage(0)) {
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
- overage_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
if (U(0, 0) < min_voltage(0)) {
const double under_amount = U(0, 0) - min_voltage(0);
mutable_U(0, 0) = min_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
- under_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ under_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index b583078..b3e980d 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -207,9 +207,9 @@
is_accelerating = arm_U(0, 0) < bemf_voltage;
}
if (is_accelerating) {
- arm_plant_->set_plant_index(0);
+ arm_plant_->set_index(0);
} else {
- arm_plant_->set_plant_index(1);
+ arm_plant_->set_index(1);
}
}
arm_plant_->Update(arm_U);
diff --git a/y2017/BUILD b/y2017/BUILD
index 249f5f3..4e66d6f 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -25,6 +25,25 @@
)
cc_binary(
+ name = 'joystick_reader',
+ srcs = [
+ 'joystick_reader.cc',
+ ],
+ deps = [
+ ':constants',
+ '//aos/common/actions:action_lib',
+ '//aos/common/logging',
+ '//aos/common/util:log_interval',
+ '//aos/common:time',
+ '//aos/input:joystick_input',
+ '//aos/linux_code:init',
+ '//frc971/autonomous:auto_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ ],
+)
+
+cc_binary(
name = 'wpilib_interface',
srcs = [
'wpilib_interface.cc',
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 6c0f2f3..0858e45 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -14,7 +14,14 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class VelocityShooter(control_loop.ControlLoop):
+
+def PlotDiff(list1, list2, time):
+ pylab.subplot(1, 1, 1)
+ pylab.plot(time, numpy.subtract(list1, list2), label='diff')
+ pylab.legend()
+
+
+class VelocityShooter(control_loop.HybridControlLoop):
def __init__(self, name='VelocityShooter'):
super(VelocityShooter, self).__init__(name)
# Number of motors
@@ -141,14 +148,19 @@
q_pos = 0.01
q_vel = 2.0
q_voltage = 0.2
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (q_voltage ** 2.0)]])
r_pos = 0.001
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous, B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
+ dt=self.dt)
+
+ self.KalmanGain, self.P_steady_state = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
@@ -173,9 +185,10 @@
self.x_hat = []
self.u = []
self.offset = []
+ self.diff = []
def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
- observer_shooter=None):
+ observer_shooter=None, hybrid_obs = False):
"""Runs the shooter plant with an initial condition and goal.
Args:
@@ -211,6 +224,7 @@
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(shooter.X[0, 0])
+ self.diff.append(shooter.X[1, 0] - observer_shooter.X_hat[1, 0])
if self.v:
last_v = self.v[-1]
@@ -221,8 +235,9 @@
self.a.append((self.v[-1] - last_v) / shooter.dt)
if observer_shooter is not None:
- observer_shooter.Y = shooter.Y
- observer_shooter.CorrectObserver(U)
+ if i != 0:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
self.offset.append(observer_shooter.X_hat[2, 0])
applied_U = U.copy()
@@ -231,7 +246,11 @@
shooter.Update(applied_U)
if observer_shooter is not None:
- observer_shooter.PredictObserver(U)
+ if hybrid_obs:
+ observer_shooter.PredictHybridObserver(U, shooter.dt)
+ else:
+ observer_shooter.PredictObserver(U)
+
self.t.append(initial_t + i * shooter.dt)
self.u.append(U[0, 0])
@@ -251,24 +270,42 @@
pylab.plot(self.t, self.a, label='a')
pylab.legend()
- pylab.show()
+ pylab.figure()
+ pylab.draw()
def main(argv):
scenario_plotter = ScenarioPlotter()
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter = IntegralShooter()
-
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [0.0]])
- scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter, iterations=200)
-
if FLAGS.plot:
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter = IntegralShooter()
+ iterations = 200
+
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter, iterations=iterations)
+
scenario_plotter.Plot()
+ scenario_plotter_int = ScenarioPlotter()
+
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter_hybrid = IntegralShooter()
+
+ scenario_plotter_int.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter_hybrid, iterations=iterations,
+ hybrid_obs = True)
+
+ scenario_plotter_int.Plot()
+ PlotDiff(scenario_plotter.x_hat, scenario_plotter_int.x_hat,
+ scenario_plotter.t)
+
+ pylab.show()
+
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
@@ -284,7 +321,9 @@
integral_shooter = IntegralShooter('IntegralShooter')
integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralShooter', [integral_shooter], namespaces=namespaces)
+ 'IntegralShooter', [integral_shooter], namespaces=namespaces,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
integral_loop_writer.Write(argv[3], argv[4])
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
index 6186f7f..0560ec4 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -116,7 +116,8 @@
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
loop_->Update(disabled);
- stuck_indexer_detector_->UpdateObserver(loop_->U());
+ stuck_indexer_detector_->UpdateObserver(loop_->U(),
+ ::aos::controls::kLoopFrequency);
}
void IndexerController::SetStatus(IndexerStatus *status) {
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index c4d1630..8991658 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<3, 1, 1>(
+ : loop_(new StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(
superstructure::shooter::MakeIntegralShooterLoop())) {
history_.fill(0);
Y_.setZero();
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index 41e24c0..7dc44bf 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -49,7 +49,9 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<
+ 3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>>>
+ loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 5;
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
new file mode 100644
index 0000000..179fac6
--- /dev/null
+++ b/y2017/joystick_reader.cc
@@ -0,0 +1,244 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
+#include "aos/common/actions/actions.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+#include "y2017/constants.h"
+#include "frc971/autonomous/auto.q.h"
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::y2017::control_loops::superstructure_queue;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
+
+namespace y2017 {
+namespace input {
+namespace joysticks {
+
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kQuickTurn(1, 5);
+
+const ButtonLocation kTurn1(1, 7);
+const ButtonLocation kTurn2(1, 11);
+
+const ButtonLocation kIntakeDown(3, 9);
+const ButtonLocation kIntakeIn(3, 12);
+const ButtonLocation kIntakeOut(3, 8);
+const POVLocation kHang(3, 90);
+const ButtonLocation kFire(3, 3);
+const ButtonLocation kCloseShot(3, 7);
+const ButtonLocation kMiddleShot(3, 6);
+const POVLocation kFarShot(3, 270);
+
+const ButtonLocation kVisionAlign(3, 5);
+
+const ButtonLocation kReverseIndexer(3, 4);
+const ButtonLocation kExtra1(3, 11);
+const ButtonLocation kExtra2(3, 10);
+const ButtonLocation kExtra3(3, 2);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() {}
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override {
+ bool last_auto_running = auto_running_;
+ auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+ if (auto_running_ != last_auto_running) {
+ if (auto_running_) {
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ vision_valid_ = false;
+
+ if (!auto_running_) {
+ HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ // Process any pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ drivetrain_queue.status.FetchLatest();
+
+ if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
+ if (drivetrain_queue.status.get()) {
+ left_goal_ = drivetrain_queue.status->estimated_left_position;
+ right_goal_ = drivetrain_queue.status->estimated_right_position;
+ }
+ }
+ if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
+ is_control_loop_driving = true;
+ }
+ if (!drivetrain_queue.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal_ - wheel * 0.5 + throttle * 0.3)
+ .right_goal(right_goal_ + wheel * 0.5 + throttle * 0.3)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
+ .Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ // Default the intake to in.
+ intake_goal_ = constants::Values::kIntakeRange.lower;
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+
+ superstructure_queue.status.FetchLatest();
+ if (!superstructure_queue.status.get()) {
+ LOG(ERROR, "Got no superstructure status packet.\n");
+ return;
+ }
+
+ if (data.IsPressed(kIntakeDown)) {
+ intake_goal_ = 0.23;
+ }
+
+ if (data.IsPressed(kVisionAlign)) {
+ // Align shot using vision
+ // TODO(campbell): Add vision aligning.
+ shooter_velocity_ = 100.0;
+ } else if (data.IsPressed(kCloseShot)) {
+ // Close shot
+ hood_goal_ = 0.5;
+ shooter_velocity_ = 350.0;
+ } else if (data.IsPressed(kMiddleShot)) {
+ // Medium distance shot
+ hood_goal_ = 0.4;
+ shooter_velocity_ = 350.0;
+ } else if (data.IsPressed(kFarShot)) {
+ // Far shot
+ hood_goal_ = 0.6;
+ shooter_velocity_ = 250.0;
+ } else {
+ hood_goal_ = 0.15;
+ shooter_velocity_ = 0.0;
+ }
+
+ if (data.IsPressed(kExtra1)) {
+ turret_goal_ += -0.1;
+ }
+ if (data.IsPressed(kExtra2)) {
+ turret_goal_ = 0.0;
+ }
+ if (data.IsPressed(kExtra3)) {
+ turret_goal_ += 0.1;
+ }
+
+ fire_ = data.IsPressed(kFire) && shooter_velocity_ != 0.0;
+
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ new_superstructure_goal->intake.distance = intake_goal_;
+ new_superstructure_goal->turret.angle = turret_goal_;
+ new_superstructure_goal->hood.angle = hood_goal_;
+ new_superstructure_goal->shooter.angular_velocity = shooter_velocity_;
+
+ new_superstructure_goal->intake.profile_params.max_velocity = 0.50;
+ new_superstructure_goal->turret.profile_params.max_velocity = 6.0;
+ new_superstructure_goal->hood.profile_params.max_velocity = 5.0;
+
+ new_superstructure_goal->intake.profile_params.max_acceleration = 5.0;
+ new_superstructure_goal->turret.profile_params.max_acceleration = 15.0;
+ new_superstructure_goal->hood.profile_params.max_acceleration = 25.0;
+
+ if (data.IsPressed(kHang)) {
+ new_superstructure_goal->intake.voltage_rollers = -12.0;
+ } else if (data.IsPressed(kIntakeIn)) {
+ new_superstructure_goal->intake.voltage_rollers = 12.0;
+ } else if (data.IsPressed(kIntakeOut)) {
+ new_superstructure_goal->intake.voltage_rollers = -8.0;
+ } else {
+ new_superstructure_goal->intake.voltage_rollers = 0.0;
+ }
+
+ if (data.IsPressed(kReverseIndexer)) {
+ new_superstructure_goal->indexer.voltage_rollers = -4.0;
+ new_superstructure_goal->indexer.angular_velocity = -4.0;
+ new_superstructure_goal->indexer.angular_velocity = -1.0;
+ } else if (fire_) {
+ new_superstructure_goal->indexer.voltage_rollers = 2.0;
+ new_superstructure_goal->indexer.angular_velocity = 3.0 * M_PI;
+ new_superstructure_goal->indexer.angular_velocity = 1.0;
+ } else {
+ new_superstructure_goal->indexer.voltage_rollers = 0.0;
+ new_superstructure_goal->indexer.angular_velocity = 0.0;
+ }
+
+ LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
+
+ private:
+ void StartAuto() { LOG(INFO, "Starting auto mode\n"); }
+
+ void StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+ }
+
+ // Current goals to send to the robot.
+ double intake_goal_ = 0.0;
+ double turret_goal_ = 0.0;
+ double hood_goal_ = 0.3;
+ double shooter_velocity_ = 0.0;
+
+ // Goals to send to the drivetrain in closed loop mode.
+ double left_goal_;
+ double right_goal_;
+
+ bool was_running_ = false;
+ bool auto_running_ = false;
+
+ bool vision_valid_ = false;
+
+ bool fire_ = false;
+
+ ::aos::common::actions::ActionQueue action_queue_;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2017
+
+int main() {
+ ::aos::Init(-1);
+ ::y2017::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}