Tune flywheels
Switch to hybrid kalman filter, plot voltage error
Change-Id: I9ade9a741aae58bdb3818c23bfe91b18786235f3
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 1746589..48c9098 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -28,20 +28,22 @@
name='Accelerator',
motor=control_loop.Falcon(),
G=G,
- J=J,
- q_pos=0.08,
- q_vel=4.00,
- q_voltage=0.4,
+ J=J + 0.0015,
+ q_pos=0.01,
+ q_vel=40.0,
+ q_voltage=2.0,
r_pos=0.05,
- controller_poles=[.84])
+ controller_poles=[.86])
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[0.0], [500.0], [0.0]])
- flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
+ flywheel.PlotSpinup(kAccelerator, goal=R, iterations=400)
return 0
+ glog.debug("J is %f" % J)
+
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
@@ -53,4 +55,5 @@
if __name__ == '__main__':
argv = FLAGS(sys.argv)
+ glog.init()
sys.exit(main(argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 5067a16..9381630 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -14,7 +14,7 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
# Inertia for a single 4" diameter, 2" wide neopreme wheel.
-J_wheel = 0.000319 * 2.0
+J_wheel = 0.000319 * 2.0 * 6.0
# Gear ratio to the final wheel.
# 40 tooth on the flywheel
# 48 for the falcon.
@@ -29,17 +29,17 @@
motor=control_loop.Falcon(),
G=G,
J=J,
- q_pos=0.08,
- q_vel=4.00,
- q_voltage=0.4,
+ q_pos=0.01,
+ q_vel=100.0,
+ q_voltage=6.0,
r_pos=0.05,
- controller_poles=[.84])
+ controller_poles=[.92])
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[0.0], [500.0], [0.0]])
- flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
+ flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=400)
return 0
if len(argv) != 5:
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 630d223..451788a 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -30,7 +30,7 @@
self.controller_poles = controller_poles
-class VelocityFlywheel(control_loop.ControlLoop):
+class VelocityFlywheel(control_loop.HybridControlLoop):
def __init__(self, params, name="Flywheel"):
super(VelocityFlywheel, self).__init__(name=name)
self.params = params
@@ -121,26 +121,44 @@
self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
# states
# [position, velocity, voltage_error]
self.C_unaugmented = self.C
self.C = numpy.matrix(numpy.zeros((1, 3)))
self.C[0:1, 0:2] = self.C_unaugmented
+ glog.debug('A_continuous %s' % str(self.A_continuous))
+ glog.debug('B_continuous %s' % str(self.B_continuous))
+ glog.debug('C %s' % str(self.C))
+
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
+ glog.debug('A %s' % str(self.A))
+ glog.debug('B %s' % str(self.B))
+
q_pos = self.params.q_pos
q_vel = self.params.q_vel
q_voltage = self.params.q_voltage
- self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.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 = self.params.r_pos
- 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)
+
+ glog.debug('Q_discrete %s' % (str(self.Q)))
+ glog.debug('R_discrete %s' % (str(self.R)))
+
+ 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
@@ -155,7 +173,7 @@
self.InitializeState()
-def PlotSpinup(params, goal, iterations=200):
+def PlotSpinup(params, goal, iterations=400):
"""Runs the flywheel plant with an initial condition and goal.
Args:
@@ -210,21 +228,20 @@
if observer_flywheel is not None:
observer_flywheel.Y = flywheel.Y
- observer_flywheel.CorrectObserver(U)
+ observer_flywheel.CorrectHybridObserver(U)
offset.append(observer_flywheel.X_hat[2, 0])
applied_U = U.copy()
- if i > 100:
+ if i > 200:
applied_U += 2
flywheel.Update(applied_U)
if observer_flywheel is not None:
- observer_flywheel.PredictObserver(U)
+ observer_flywheel.PredictHybridObserver(U, flywheel.dt)
t.append(initial_t + i * flywheel.dt)
u.append(U[0, 0])
- glog.debug('Time: %f', t[-1])
pylab.subplot(3, 1, 1)
pylab.plot(t, v, label='x')
pylab.plot(t, x_hat, label='x_hat')
@@ -281,5 +298,9 @@
loop_writer.Write(plant_files[0], plant_files[1])
integral_loop_writer = control_loop.ControlLoopWriter(
- 'Integral' + name, integral_flywheels, namespaces=namespace)
+ 'Integral' + name,
+ integral_flywheels,
+ namespaces=namespace,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index 98ddfe4..7e75dce 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -47,6 +47,8 @@
angular_system.PlotKick(kHood, R)
angular_system.PlotMotion(kHood, R)
+ glog.info("Radians per turn: %f\n", radians_per_turn)
+
# Write the generated constants out to a file.
if len(argv) != 5:
glog.fatal(