Add gain scheduling to linear and angular systems.
Change-Id: Ib7092c2ee2192305b6f9f395ea73d24a1bda1d78
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])