Add gain scheduling to linear and angular systems.
Change-Id: Ib7092c2ee2192305b6f9f395ea73d24a1bda1d78
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index d98b5c9..ef5ffde 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -10,12 +10,12 @@
#include "aos/controls/control_loop.h"
#include "aos/util/trapezoid_profile.h"
+#include "frc971/constants.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/profiled_subsystem.q.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "frc971/constants.h"
namespace frc971 {
namespace control_loops {
@@ -59,13 +59,15 @@
}
// Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs> &
- controller() const {
+ const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs>
+ &controller() const {
return *loop_;
}
int controller_index() const { return loop_->index(); }
+ void set_controller_index(int index) { loop_->set_index(index); }
+
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
@@ -140,7 +142,8 @@
template <typename ZeroingEstimator =
::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
class SingleDOFProfiledSubsystem
- : public ::frc971::control_loops::ProfiledSubsystem<3, 1, ZeroingEstimator> {
+ : public ::frc971::control_loops::ProfiledSubsystem<3, 1,
+ ZeroingEstimator> {
public:
SingleDOFProfiledSubsystem(
::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index a0a07d6..36da41a 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -375,26 +375,42 @@
"""Writes out the constants for a angular system to a file.
Args:
- params: AngularSystemParams, the parameters defining the system.
+ params: list of AngularSystemParams or AngularSystemParams, the
+ parameters defining the system.
plant_files: list of strings, the cc and h files for the plant.
controller_files: list of strings, the cc and h files for the integral
controller.
year_namespaces: list of strings, the namespace list to use.
"""
# Write the generated constants out to a file.
- angular_system = AngularSystem(params, params.name)
+ angular_systems = []
+ integral_angular_systems = []
+
+ if type(params) is list:
+ name = params[0].name
+ for index, param in enumerate(params):
+ angular_systems.append(
+ AngularSystem(param, param.name + str(index)))
+ integral_angular_systems.append(
+ IntegralAngularSystem(param, 'Integral' + param.name + str(
+ index)))
+ else:
+ name = params.name
+ angular_systems.append(AngularSystem(params, params.name))
+ integral_angular_systems.append(
+ IntegralAngularSystem(params, 'Integral' + params.name))
+
loop_writer = control_loop.ControlLoopWriter(
- angular_system.name, [angular_system], namespaces=year_namespaces)
+ name, angular_systems, namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f', angular_system.G))
+ control_loop.Constant('kOutputRatio', '%f', angular_systems[0].G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f',
- angular_system.motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f', angular_systems[0]
+ .motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
- integral_angular_system = IntegralAngularSystem(params,
- 'Integral' + params.name)
integral_loop_writer = control_loop.ControlLoopWriter(
- integral_angular_system.name, [integral_angular_system],
+ 'Integral' + name,
+ integral_angular_systems,
namespaces=year_namespaces)
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index fe8dd85..105093b 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -381,29 +381,45 @@
"""Writes out the constants for a linear system to a file.
Args:
- params: LinearSystemParams, the parameters defining the system.
+ params: list of LinearSystemParams or LinearSystemParams, the
+ parameters defining the system.
plant_files: list of strings, the cc and h files for the plant.
controller_files: list of strings, the cc and h files for the integral
controller.
year_namespaces: list of strings, the namespace list to use.
"""
# Write the generated constants out to a file.
- linear_system = LinearSystem(params, params.name)
+ linear_systems = []
+ integral_linear_systems = []
+
+ if type(params) is list:
+ name = params[0].name
+ for index, param in enumerate(params):
+ linear_systems.append(
+ LinearSystem(param, param.name + str(index)))
+ integral_linear_systems.append(
+ IntegralLinearSystem(param, 'Integral' + param.name + str(
+ index)))
+ else:
+ name = params.name
+ linear_systems.append(LinearSystem(params, params.name))
+ integral_linear_systems.append(
+ IntegralLinearSystem(params, 'Integral' + params.name))
+
loop_writer = control_loop.ControlLoopWriter(
- linear_system.name, [linear_system], namespaces=year_namespaces)
+ name, linear_systems, namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f',
- linear_system.motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f', linear_systems[0]
+ .motor.free_speed))
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f',
- linear_system.G * linear_system.radius))
+ control_loop.Constant('kOutputRatio', '%f', linear_systems[0].G *
+ linear_systems[0].radius))
loop_writer.AddConstant(
- control_loop.Constant('kRadius', '%f', linear_system.radius))
+ control_loop.Constant('kRadius', '%f', linear_systems[0].radius))
loop_writer.Write(plant_files[0], plant_files[1])
- integral_linear_system = IntegralLinearSystem(params,
- 'Integral' + params.name)
integral_loop_writer = control_loop.ControlLoopWriter(
- integral_linear_system.name, [integral_linear_system],
+ 'Integral' + name,
+ integral_linear_systems,
namespaces=year_namespaces)
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 2951f1a..7e8f4a7 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -74,6 +74,10 @@
void TriggerEstimatorError() { profiled_subsystem_.TriggerEstimatorError(); }
+ void set_controller_index(int index) {
+ profiled_subsystem_.set_controller_index(index);
+ }
+
enum class State : int32_t {
UNINITIALIZED,
DISABLED_INITIALIZED,