Switched y2014 control loops over to gflags and glog.
Change-Id: I88a4beda7a3e59516badd5403cb2de1383ba7ef4
diff --git a/y2014/control_loops/python/BUILD b/y2014/control_loops/python/BUILD
index f900152..84e73a5 100644
--- a/y2014/control_loops/python/BUILD
+++ b/y2014/control_loops/python/BUILD
@@ -19,9 +19,9 @@
'drivetrain.py',
],
deps = [
- '//frc971/control_loops/python:controls',
'//external:python-gflags',
'//external:python-glog',
+ '//frc971/control_loops/python:controls',
],
)
@@ -32,6 +32,8 @@
'drivetrain.py',
],
deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
'//frc971/control_loops/python:controls',
],
)
@@ -43,6 +45,8 @@
],
deps = [
':polydrivetrain_lib',
+ '//external:python-gflags',
+ '//external:python-glog',
'//frc971/control_loops/python:controls',
]
)
@@ -53,6 +57,8 @@
'shooter.py',
],
deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
'//frc971/control_loops/python:controls',
]
)
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index e6242ec..f53a8fa 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -7,6 +7,15 @@
import numpy
import sys
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
class Claw(control_loop.ControlLoop):
def __init__(self, name="RawClaw"):
@@ -72,7 +81,7 @@
[self.motor_feedforward_bottom, 0],
[-self.motor_feedforward_bottom, self.motor_feedforward_difference]])
- print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
+ glog.debug('Cont X_ss %f', self.motor_feedforward / -self.common_motor_constant)
self.B_bottom_cont = numpy.matrix(
[[0],
@@ -100,13 +109,13 @@
self.K_diff = controls.dplace(self.A_diff, self.B_diff,
[0.85 + 0.05j, 0.85 - 0.05j])
- print "K_diff", self.K_diff
- print "K_bottom", self.K_bottom
+ glog.debug('K_diff %s', str(self.K_diff))
+ glog.debug('K_bottom %s', str(self.K_bottom))
- print "A"
- print self.A
- print "B"
- print self.B
+ glog.debug('A')
+ glog.debug(self.A)
+ glog.debug('B')
+ glog.debug(self.B)
self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
@@ -127,24 +136,24 @@
lstsq_A = numpy.identity(2)
lstsq_A[0, :] = self.B[1, :]
lstsq_A[1, :] = self.B[3, :]
- print "System of Equations coefficients:"
- print lstsq_A
- print "det", numpy.linalg.det(lstsq_A)
+ glog.debug('System of Equations coefficients:')
+ glog.debug(str(lstsq_A))
+ glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
out_x = numpy.linalg.lstsq(
lstsq_A,
numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
- print "K unaugmented"
- print self.K
- print "B * K unaugmented"
- print self.B * self.K
+ glog.debug('K unaugmented')
+ glog.debug(str(self.K))
+ glog.debug('B * K unaugmented')
+ glog.debug(str(self.B * self.K))
F = self.A - self.B * self.K
- print "A - B * K unaugmented"
- print F
- print "eigenvalues"
- print numpy.linalg.eig(F)[0]
+ glog.debug('A - B * K unaugmented')
+ glog.debug(str(F))
+ glog.debug('eigenvalues')
+ glog.debug(str(numpy.linalg.eig(F)[0]))
self.rpl = .09
self.ipl = 0.030
@@ -180,7 +189,7 @@
X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
- print "X_ss", X_ss
+ glog.debug('X_ss %s', str(X_ss))
self.InitializeState()
@@ -225,17 +234,17 @@
self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
[50.0, 0.0, 10.0, 0.0, 1.0]])
- #self.K = numpy.matrix([[50.0, -100.0, 0, -10, 0],
- # [50.0, 100.0, 0, 10, 0]])
controlability = controls.ctrb(self.A, self.B)
- print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
+ glog.debug('Rank of augmented controlability matrix: %d',
+ numpy.linalg.matrix_rank(controlability))
- print "K"
- print self.K
- print "Placed controller poles are"
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
- print [numpy.abs(x) for x in numpy.linalg.eig(self.A - self.B * self.K)[0]]
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug(str([numpy.abs(x) for x in
+ numpy.linalg.eig(self.A - self.B * self.K)[0]]))
self.rpl = .05
self.ipl = 0.008
@@ -355,7 +364,7 @@
return numpy.matrix([[bottom_u], [top_u]])
-def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=False, iterations=200):
+def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=True, iterations=200):
"""Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
The tests themselves are not terribly sophisticated; I just test for
@@ -396,8 +405,8 @@
if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
tests_passed = False
- print "Claw separation was", claw.X[1, 0]
- print "Should have been between", lower_bound, "and", upper_bound
+ glog.info('Claw separation was %f', claw.X[1, 0])
+ glog.info("Should have been between", lower_bound, "and", upper_bound)
if claw.hard_pos_limits and \
(claw.X[0, 0] > claw.hard_pos_limits[1] or
@@ -405,9 +414,9 @@
claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
tests_passed = False
- print "Claws at %f and %f" % (claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
- print "Both should be in %s, definitely %s" % \
- (claw.pos_limits, claw.hard_pos_limits)
+ glog.info('Claws at %f and %f', claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
+ glog.info("Both should be in %s, definitely %s",
+ claw.pos_limits, claw.hard_pos_limits)
t.append(i * claw.dt)
x_bottom.append(claw.X[0, 0] * 10.0)
@@ -428,70 +437,69 @@
# Test to make sure that we are near the goal.
if numpy.max(abs(claw.X - goal)) > 1e-4:
tests_passed = False
- print "X was", claw.X, "Expected", goal
+ glog.error('X was %s Expected %s', str(claw.X), str(goal))
return tests_passed
def main(argv):
+ argv = FLAGS(argv)
+
claw = Claw()
+ if FLAGS.plot:
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test moving the claw with constant separation.
- initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test just changing separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test just changing separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test changing both separation and position at once.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test changing both separation and position at once.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test opening with the top claw at the limit.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
+ claw.hard_pos_limits = (-1.6, 0.1)
+ claw.pos_limits = (-1.5, 0.0)
+ run_test(claw, initial_X, R)
+ claw.pos_limits = None
- # Test opening with the top claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-1.6, 0.1)
- claw.pos_limits = (-1.5, 0.0)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
-
- # Test opening with the bottom claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-0.1, 1.6)
- claw.pos_limits = (0.0, 1.6)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
+ # Test opening with the bottom claw at the limit.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
+ claw.hard_pos_limits = (-0.1, 1.6)
+ claw.pos_limits = (0.0, 1.6)
+ run_test(claw, initial_X, R)
+ claw.pos_limits = None
# Write the generated constants out to a file.
if len(argv) != 3:
- print "Expected .h file name and .cc file name for the claw."
+ glog.fatal('Expected .h file name and .cc file name for the claw.')
else:
namespaces = ['y2014', 'control_loops', 'claw']
- claw = Claw("Claw")
- loop_writer = control_loop.ControlLoopWriter("Claw", [claw],
+ claw = Claw('Claw')
+ loop_writer = control_loop.ControlLoopWriter('Claw', [claw],
namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant("kClawMomentOfInertiaRatio",
- "%f", claw.J_top / claw.J_bottom))
- loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ loop_writer.AddConstant(control_loop.Constant('kClawMomentOfInertiaRatio',
+ '%f', claw.J_top / claw.J_bottom))
+ loop_writer.AddConstant(control_loop.Constant('kDt', '%f',
claw.dt))
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index b6fecc6..19fdb5e 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -8,8 +8,17 @@
from frc971.control_loops.python import controls
from matplotlib import pylab
+import gflags
+import glog
+
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
def CoerceGoal(region, K, w, R):
"""Intersects a line with a region, and finds the closest point to R.
@@ -250,16 +259,16 @@
#goal_gear_is_high = True
if not self.IsInGear(current_gear):
- print gear_name, 'Not in gear.'
+ glog.debug('%s Not in gear.', gear_name)
return current_gear
else:
is_high = current_gear is VelocityDrivetrain.HIGH
if is_high != goal_gear_is_high:
if goal_gear_is_high:
- print gear_name, 'Shifting up.'
+ glog.debug('%s Shifting up.', gear_name)
return VelocityDrivetrain.SHIFTING_UP
else:
- print gear_name, 'Shifting down.'
+ glog.debug('%s Shifting down.', gear_name)
return VelocityDrivetrain.SHIFTING_DOWN
else:
return current_gear
@@ -361,7 +370,7 @@
FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
else:
- print 'Not all in gear'
+ glog.debug('Not all in gear')
if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
# TODO(austin): Use battery volts here.
R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
@@ -390,15 +399,19 @@
self.right_gear, self.right_shifter_position = self.SimShifter(
self.right_gear, self.right_shifter_position)
- print "U is", self.U[0, 0], self.U[1, 0]
- print "Left shifter", self.left_gear, self.left_shifter_position, "Right shifter", self.right_gear, self.right_shifter_position
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d',
+ self.left_gear, self.left_shifter_position,
+ self.right_gear, self.right_shifter_position)
def main(argv):
+ argv = FLAGS(argv)
+
vdrivetrain = VelocityDrivetrain()
if len(argv) != 5:
- print "Expected .h file name and .cc file name"
+ glog.fatal('Expected .h file name and .cc file name')
else:
namespaces = ['y2014', 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
@@ -408,18 +421,12 @@
vdrivetrain.drivetrain_high_high],
namespaces=namespaces)
- if argv[1][-3:] == '.cc':
- dog_loop_writer.Write(argv[2], argv[1])
- else:
- dog_loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
"CIM", [drivetrain.CIM()])
- if argv[3][-3:] == '.cc':
- cim_writer.Write(argv[4], argv[3])
- else:
- cim_writer.Write(argv[3], argv[4])
+ cim_writer.Write(argv[3], argv[4])
return
vl_plot = []
@@ -435,16 +442,16 @@
vdrivetrain.left_gear = VelocityDrivetrain.LOW
vdrivetrain.right_gear = VelocityDrivetrain.LOW
- print "K is", vdrivetrain.CurrentDrivetrain().K
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
- print "Left is high"
+ glog.debug('Left is high')
else:
- print "Left is low"
+ glog.debug('Left is low')
if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
- print "Right is high"
+ glog.debug('Right is high')
else:
- print "Right is low"
+ glog.debug('Right is low')
for t in numpy.arange(0, 1.7, vdrivetrain.dt):
if t < 0.5:
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index aab539f..a4767d1 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -1,13 +1,20 @@
#!/usr/bin/python
-from frc971.control_loops.python import control_loop
+import gflags
+import glog
import argparse
import numpy
import sys
from matplotlib import pylab
+from frc971.control_loops.python import control_loop
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class SprungShooter(control_loop.ControlLoop):
- def __init__(self, name="RawSprungShooter", verbose=False):
+ def __init__(self, name="RawSprungShooter"):
super(SprungShooter, self).__init__(name)
# Stall Torque in N m
self.stall_torque = .4982
@@ -68,7 +75,7 @@
class Shooter(SprungShooter):
- def __init__(self, name="RawShooter", verbose=False):
+ def __init__(self, name="RawShooter"):
super(Shooter, self).__init__(name)
# State feedback matrices
@@ -96,7 +103,7 @@
class SprungShooterDeltaU(SprungShooter):
- def __init__(self, name="SprungShooter", verbose=False):
+ def __init__(self, name="SprungShooter"):
super(SprungShooterDeltaU, self).__init__(name)
A_unaugmented = self.A
B_unaugmented = self.B
@@ -116,19 +123,17 @@
self.PlaceControllerPoles([0.50, 0.35, 0.80])
- if verbose:
- print "K"
- print self.K
- print "Placed controller poles are"
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl, 0.90])
- if verbose:
- print "Placed observer poles are"
- print numpy.linalg.eig(self.A - self.L * self.C)[0]
+ glog.debug('Placed observer poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
@@ -137,7 +142,7 @@
class ShooterDeltaU(Shooter):
- def __init__(self, name="Shooter", verbose=False):
+ def __init__(self, name="Shooter"):
super(ShooterDeltaU, self).__init__(name)
A_unaugmented = self.A
B_unaugmented = self.B
@@ -157,19 +162,17 @@
self.PlaceControllerPoles([0.55, 0.45, 0.80])
- if verbose:
- print "K"
- print self.K
- print "Placed controller poles are"
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl, 0.90])
- if verbose:
- print "Placed observer poles are"
- print numpy.linalg.eig(self.A - self.L * self.C)[0]
+ glog.debug('Placed observer poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
@@ -183,18 +186,11 @@
return new_u - old_u
def main(argv):
- parser = argparse.ArgumentParser(description='Calculate shooter.')
- parser.add_argument('--plot', action='store_true', default=False, help='If true, plot')
- parser.add_argument('shootercc')
- parser.add_argument('shooterh')
- parser.add_argument('unaugmented_shootercc')
- parser.add_argument('unaugmented_shooterh')
-
- args = parser.parse_args(argv[1:])
+ argv = FLAGS(argv)
# Simulate the response of the system to a goal.
- sprung_shooter = SprungShooterDeltaU(verbose=args.plot)
- raw_sprung_shooter = SprungShooter(verbose=args.plot)
+ sprung_shooter = SprungShooterDeltaU()
+ raw_sprung_shooter = SprungShooter()
close_loop_x = []
close_loop_u = []
goal_position = -0.3
@@ -213,13 +209,13 @@
close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
close_loop_u.append(voltage[0, 0])
- if args.plot:
+ if FLAGS.plot:
pylab.plot(range(500), close_loop_x)
pylab.plot(range(500), close_loop_u)
pylab.show()
- shooter = ShooterDeltaU(verbose=args.plot)
- raw_shooter = Shooter(verbose=args.plot)
+ shooter = ShooterDeltaU()
+ raw_shooter = Shooter()
close_loop_x = []
close_loop_u = []
goal_position = -0.3
@@ -235,24 +231,23 @@
close_loop_x.append(raw_shooter.X[0, 0] * 10)
close_loop_u.append(voltage[0, 0])
- if args.plot:
+ if FLAGS.plot:
pylab.plot(range(500), close_loop_x)
pylab.plot(range(500), close_loop_u)
pylab.show()
# Write the generated constants out to a file.
- unaug_sprung_shooter = SprungShooter("RawSprungShooter", verbose=args.plot)
- unaug_shooter = Shooter("RawShooter", verbose=args.plot)
+ unaug_sprung_shooter = SprungShooter("RawSprungShooter")
+ unaug_shooter = Shooter("RawShooter")
namespaces = ['y2014', 'control_loops', 'shooter']
unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
[unaug_sprung_shooter,
unaug_shooter],
namespaces=namespaces)
- unaug_loop_writer.Write(args.unaugmented_shooterh,
- args.unaugmented_shootercc)
+ unaug_loop_writer.Write(argv[4], argv[3])
- sprung_shooter = SprungShooterDeltaU(verbose=args.plot)
- shooter = ShooterDeltaU(verbose=args.plot)
+ sprung_shooter = SprungShooterDeltaU()
+ shooter = ShooterDeltaU()
loop_writer = control_loop.ControlLoopWriter("Shooter",
[sprung_shooter, shooter],
namespaces=namespaces)
@@ -263,7 +258,7 @@
sprung_shooter.Ks))
loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
sprung_shooter.dt))
- loop_writer.Write(args.shooterh, args.shootercc)
+ loop_writer.Write(argv[2], argv[1])
if __name__ == '__main__':
sys.exit(main(sys.argv))