Added A_continuous and B_continuous to StateFeedbackPlantCoefficients
Change-Id: I0c2649e0ef4986c6b9122bf59adc8ad1d45572f4
diff --git a/y2015/control_loops/python/arm.py b/y2015/control_loops/python/arm.py
index 9fa270e..d3d77c5 100755
--- a/y2015/control_loops/python/arm.py
+++ b/y2015/control_loops/python/arm.py
@@ -1,18 +1,22 @@
#!/usr/bin/python
-import control_loop
-import controls
-import polytope
-import polydrivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
import numpy
import math
import sys
-import matplotlib
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm", mass=None):
+ def __init__(self, name='Arm', mass=None):
super(Arm, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
@@ -63,12 +67,12 @@
[0, 0, 0, 1],
[0, 0, -self.C1 * 2.0, -self.C3]])
- print 'Full speed is', self.C2 / self.C3 * 12.0
+ glog.debug('Full speed is %f', self.C2 / self.C3 * 12.0)
- print 'Stall arm difference is', 12.0 * self.C2 / self.C1
- print 'Stall arm difference first principles is', self.stall_torque * self.G / self.spring
+ glog.debug('Stall arm difference is %f', 12.0 * self.C2 / self.C1)
+ glog.debug('Stall arm difference first principles is %f', self.stall_torque * self.G / self.spring)
- print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
+ glog.debug('5 degrees of arm error is %f', self.spring / self.r * (math.pi * 5.0 / 180.0))
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -86,8 +90,8 @@
self.A_continuous, self.B_continuous, self.dt)
controllability = controls.ctrb(self.A, self.B)
- print 'Rank of augmented controllability matrix.', numpy.linalg.matrix_rank(
- controllability)
+ glog.debug('Rank of augmented controllability matrix. %d', numpy.linalg.matrix_rank(
+ controllability))
q_pos = 0.02
q_vel = 0.300
@@ -101,11 +105,10 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'Controller'
- print self.K
+ glog.debug('Controller\n %s', repr(self.K))
- print 'Controller Poles'
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('Controller Poles\n %s',
+ numpy.linalg.eig(self.A - self.B * self.K)[0])
self.rpl = 0.20
self.ipl = 0.05
@@ -119,13 +122,14 @@
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
- print 'Observer (Converted to a KF)', numpy.linalg.inv(self.A) * self.L
+ glog.debug('Observer (Converted to a KF):\n%s',
+ repr(numpy.linalg.inv(self.A) * self.L))
self.InitializeState()
class IntegralArm(Arm):
- def __init__(self, name="IntegralArm", mass=None):
+ def __init__(self, name='IntegralArm', mass=None):
super(IntegralArm, self).__init__(name=name, mass=mass)
self.A_continuous_unaugmented = self.A_continuous
@@ -144,9 +148,9 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- print 'A cont', self.A_continuous
- print 'B cont', self.B_continuous
- print 'A discrete', self.A
+ glog.debug('A cont: %s', repr(self.A_continuous))
+ glog.debug('B cont %s', repr(self.B_continuous))
+ glog.debug('A discrete %s', repr(self.A))
q_pos = 0.08
q_vel = 0.40
@@ -175,7 +179,7 @@
self.K[0:2, 0:4] = self.K_unaugmented
self.K[0, 4] = 1
self.K[1, 4] = 1
- print 'Kal', self.KalmanGain
+ glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
self.InitializeState()
@@ -264,10 +268,10 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print numpy.linalg.inv(arm.A)
- print "delta time is ", arm.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+ glog.debug(repr(numpy.linalg.inv(arm.A)))
+ glog.debug('delta time is %f', arm.dt)
+ glog.debug('Velocity at t=0 is %f %f %f %f', x_avg[0], v_avg[0], x_sep[0], v_sep[0])
+ glog.debug('Velocity at t=1+dt is %f %f %f %f', x_avg[1], v_avg[1], x_sep[1], v_sep[1])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -350,7 +354,7 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print 'End is', observer_arm.X_hat[4, 0]
+ glog.debug('End is %f', observer_arm.X_hat[4, 0])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -370,40 +374,40 @@
def main(argv):
- loaded_mass = 25
- #loaded_mass = 0
- arm = Arm(mass=13 + loaded_mass)
- #arm_controller = Arm(mass=13 + 15)
- #observer_arm = Arm(mass=13 + 15)
- #observer_arm = None
+ if FLAGS.plot:
+ loaded_mass = 25
+ #loaded_mass = 0
+ arm = Arm(mass=13 + loaded_mass)
+ #arm_controller = Arm(mass=13 + 15)
+ #observer_arm = Arm(mass=13 + 15)
+ #observer_arm = None
- integral_arm = IntegralArm(mass=13 + loaded_mass)
- integral_arm.X_hat[0, 0] += 0.02
- integral_arm.X_hat[2, 0] += 0.02
- integral_arm.X_hat[4] = 0
+ integral_arm = IntegralArm(mass=13 + loaded_mass)
+ integral_arm.X_hat[0, 0] += 0.02
+ integral_arm.X_hat[2, 0] += 0.02
+ integral_arm.X_hat[4] = 0
- # Test moving the arm with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
+ # Test moving the arm with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
# Write the generated constants out to a file.
if len(argv) != 5:
- print "Expected .h file name and .cc file name for the arm and augmented arm."
+ glog.fatal('Expected .h file name and .cc file name for the arm and augmented arm.')
else:
- arm = Arm("Arm", mass=13)
- loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
+ namespaces = ['y2015', 'control_loops', 'fridge']
+ arm = Arm('Arm', mass=13)
+ loop_writer = control_loop.ControlLoopWriter('Arm', [arm],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_arm = IntegralArm("IntegralArm", mass=13)
- loop_writer = control_loop.ControlLoopWriter("IntegralArm", [integral_arm])
- if argv[3][-3:] == '.cc':
- loop_writer.Write(argv[4], argv[3])
- else:
- loop_writer.Write(argv[3], argv[4])
+ integral_arm = IntegralArm('IntegralArm', mass=13)
+ loop_writer = control_loop.ControlLoopWriter('IntegralArm', [integral_arm],
+ namespaces=namespaces)
+ loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/control_loops/python/claw.py b/y2015/control_loops/python/claw.py
index 86a261d..9987089 100755
--- a/y2015/control_loops/python/claw.py
+++ b/y2015/control_loops/python/claw.py
@@ -18,7 +18,7 @@
pass
class Claw(control_loop.ControlLoop):
- def __init__(self, name="Claw", mass=None):
+ def __init__(self, name='Claw', mass=None):
super(Claw, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
@@ -74,7 +74,7 @@
controllability = controls.ctrb(self.A, self.B)
- print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
+ glog.debug('Free speed is %f', self.free_speed * numpy.pi * 2.0 / 60.0 / self.G)
q_pos = 0.15
q_vel = 2.5
@@ -84,15 +84,15 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'K', self.K
- print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('K: %s', repr(self.K))
+ glog.debug('Poles are: %s', repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
- print 'L is', self.L
+ glog.debug('L is: %s', repr(self.L))
q_pos = 0.05
q_vel = 2.65
@@ -105,9 +105,9 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- print 'Kal', self.KalmanGain
+ glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
- print 'KalL is', self.L
+ glog.debug('KalL is: %s', repr(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
@@ -118,7 +118,7 @@
def run_test(claw, initial_X, goal, max_separation_error=0.01,
- show_graph=False, iterations=200, controller_claw=None,
+ iterations=200, controller_claw=None,
observer_claw=None):
"""Runs the claw plant with an initial condition and goal.
@@ -131,8 +131,6 @@
claw: claw object to use.
initial_X: starting state.
goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
iterations: Number of timesteps to run the model for.
controller_claw: claw object to get K from, or None if we should
use claw.
@@ -177,32 +175,32 @@
t.append(i * claw.dt)
u.append(U[0, 0])
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x, label='x')
- if observer_claw is not None:
- pylab.plot(t, x_hat, label='x_hat')
- pylab.legend()
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x, label='x')
+ if observer_claw is not None:
+ pylab.plot(t, x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u, label='u')
- pylab.legend()
- pylab.show()
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u, label='u')
+ pylab.legend()
+ pylab.show()
def main(argv):
- loaded_mass = 0
- #loaded_mass = 0
- claw = Claw(mass=4 + loaded_mass)
- claw_controller = Claw(mass=5 + 0)
- observer_claw = Claw(mass=5 + 0)
- #observer_claw = None
+ if FLAGS.plot:
+ loaded_mass = 0
+ #loaded_mass = 0
+ claw = Claw(mass=4 + loaded_mass)
+ claw_controller = Claw(mass=5 + 0)
+ observer_claw = Claw(mass=5 + 0)
+ #observer_claw = None
- # Test moving the claw with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0]])
- run_test(claw, initial_X, R, controller_claw=claw_controller,
- observer_claw=observer_claw)
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0]])
+ run_test(claw, initial_X, R, controller_claw=claw_controller,
+ observer_claw=observer_claw)
# Write the generated constants out to a file.
if len(argv) != 3:
@@ -215,4 +213,6 @@
loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/control_loops/python/elevator.py b/y2015/control_loops/python/elevator.py
index dc9caa1..d522034 100755
--- a/y2015/control_loops/python/elevator.py
+++ b/y2015/control_loops/python/elevator.py
@@ -8,6 +8,14 @@
import matplotlib
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
class Elevator(control_loop.ControlLoop):
def __init__(self, name="Elevator", mass=None):
super(Elevator, self).__init__(name)
@@ -57,7 +65,7 @@
[0, 0, 0, 1],
[0, 0, -C1 * 2.0, -C3]])
- print "Full speed is", C2 / C3 * 12.0
+ glog.debug('Full speed is', C2 / C3 * 12.0)
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -74,11 +82,11 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- print self.A
+ glog.debug(repr(self.A))
controllability = controls.ctrb(self.A, self.B)
- print "Rank of augmented controllability matrix.", numpy.linalg.matrix_rank(
- controllability)
+ glog.debug('Rank of augmented controllability matrix: %d',
+ numpy.linalg.matrix_rank(controllability))
q_pos = 0.02
q_vel = 0.400
@@ -92,9 +100,9 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
+ glog.debug(repr(self.K))
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug(repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.20
self.ipl = 0.05
@@ -194,10 +202,10 @@
u_left.append(U[0, 0])
u_right.append(U[1, 0])
- print numpy.linalg.inv(elevator.A)
- print "delta time is ", elevator.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+ glog.debug(repr(numpy.linalg.inv(elevator.A)))
+ glog.debug('delta time is %f', elevator.dt)
+ glog.debug('Velocity at t=0 is %f %f %f %f', x_avg[0], v_avg[0], x_sep[0], v_sep[0])
+ glog.debug('Velocity at t=1+dt is %f %f %f %f', x_avg[1], v_avg[1], x_sep[1], v_sep[1])
if show_graph:
pylab.subplot(2, 1, 1)
@@ -232,7 +240,7 @@
# Write the generated constants out to a file.
if len(argv) != 3:
- print "Expected .h file name and .cc file name for the elevator."
+ glog.fatal('Expected .h file name and .cc file name for the elevator.')
else:
namespaces = ['y2015', 'control_loops', 'fridge']
elevator = Elevator("Elevator")
@@ -244,4 +252,6 @@
loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))