Run yapf on all python files in the repo
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I221e04c3f517fab8535b22551553799e0fee7a80
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
index 0122b58..7573c16 100755
--- a/y2017/control_loops/python/column.py
+++ b/y2017/control_loops/python/column.py
@@ -37,12 +37,12 @@
self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
- self.A_continuous[0, 0] = -(
- self.indexer.Kt / self.indexer.Kv /
- (self.indexer.J * self.indexer.resistance * self.indexer.G *
- self.indexer.G) + self.turret.Kt / self.turret.Kv /
- (self.indexer.J * self.turret.resistance * self.turret.G *
- self.turret.G))
+ self.A_continuous[0, 0] = -(self.indexer.Kt / self.indexer.Kv /
+ (self.indexer.J * self.indexer.resistance *
+ self.indexer.G * self.indexer.G) +
+ self.turret.Kt / self.turret.Kv /
+ (self.indexer.J * self.turret.resistance *
+ self.turret.G * self.turret.G))
self.A_continuous[0, 2] = self.turret.Kt / self.turret.Kv / (
self.indexer.J * self.turret.resistance * self.turret.G *
self.turret.G)
@@ -142,8 +142,11 @@
r_pos = 0.05
self.R = numpy.matrix([[(r_pos**2.0), 0.0], [0.0, (r_pos**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.L = self.A * self.KalmanGain
self.InitializeState()
@@ -209,8 +212,11 @@
r_pos = 0.05
self.R = numpy.matrix([[(r_pos**2.0), 0.0], [0.0, (r_pos**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.L = self.A * self.KalmanGain
self.InitializeState()
@@ -282,12 +288,13 @@
self.x_hat.append(observer_column.X_hat[0, 0])
next_goal = numpy.concatenate(
- (end_goal[0:2, :], profile.Update(
- end_goal[2, 0], end_goal[3, 0]), end_goal[4:6, :]),
+ (end_goal[0:2, :],
+ profile.Update(end_goal[2, 0],
+ end_goal[3, 0]), end_goal[4:6, :]),
axis=0)
- ff_U = controller_column.Kff * (
- next_goal - observer_column.A * goal)
+ ff_U = controller_column.Kff * (next_goal -
+ observer_column.A * goal)
fb_U = controller_column.K * (goal - observer_column.X_hat)
self.turret_error.append((goal[2, 0] - column.X[2, 0]) * 100.0)
self.ui_fb.append(fb_U[0, 0])
@@ -373,12 +380,11 @@
initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
- scenario_plotter.run_test(
- column,
- end_goal=R,
- controller_column=column_controller,
- observer_column=observer_column,
- iterations=400)
+ scenario_plotter.run_test(column,
+ end_goal=R,
+ controller_column=column_controller,
+ observer_column=observer_column,
+ iterations=400)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -388,8 +394,8 @@
else:
namespaces = ['y2017', 'control_loops', 'superstructure', 'column']
column = Column('Column')
- loop_writer = control_loop.ControlLoopWriter(
- 'Column', [column], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Column', [column],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kIndexerFreeSpeed', '%f',
column.indexer.free_speed))
@@ -405,15 +411,15 @@
# IntegralColumn controller 1 will disable the indexer.
integral_column = IntegralColumn('IntegralColumn')
- disabled_integral_column = IntegralColumn(
- 'DisabledIntegralColumn', disable_indexer=True)
+ disabled_integral_column = IntegralColumn('DisabledIntegralColumn',
+ disable_indexer=True)
integral_loop_writer = control_loop.ControlLoopWriter(
'IntegralColumn', [integral_column, disabled_integral_column],
namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
- stuck_integral_column = IntegralColumn(
- 'StuckIntegralColumn', voltage_error_noise=8.0)
+ stuck_integral_column = IntegralColumn('StuckIntegralColumn',
+ voltage_error_noise=8.0)
stuck_integral_loop_writer = control_loop.ControlLoopWriter(
'StuckIntegralColumn', [stuck_integral_column],
namespaces=namespaces)
diff --git a/y2017/control_loops/python/drivetrain.py b/y2017/control_loops/python/drivetrain.py
index 652fcf4..b1ebcd8 100755
--- a/y2017/control_loops/python/drivetrain.py
+++ b/y2017/control_loops/python/drivetrain.py
@@ -11,17 +11,17 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-kDrivetrain = drivetrain.DrivetrainParams(
- J=6.0,
- mass=52,
- robot_radius=0.59055 / 2.0,
- wheel_radius=0.08255 / 2.0,
- G=11.0 / 60.0,
- q_pos_low=0.12,
- q_pos_high=0.14,
- q_vel_low=1.0,
- q_vel_high=0.95,
- has_imu=False)
+kDrivetrain = drivetrain.DrivetrainParams(J=6.0,
+ mass=52,
+ robot_radius=0.59055 / 2.0,
+ wheel_radius=0.08255 / 2.0,
+ G=11.0 / 60.0,
+ q_pos_low=0.12,
+ q_pos_high=0.14,
+ q_vel_low=1.0,
+ q_vel_high=0.95,
+ has_imu=False)
+
def main(argv):
argv = FLAGS(argv)
@@ -35,5 +35,6 @@
# Write the generated constants out to a file.
drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2017', kDrivetrain)
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index c405bb2..c77d134 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -118,8 +118,11 @@
r_volts = 0.025
self.R = numpy.matrix([[(r_volts**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
glog.debug('Kal %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
@@ -165,8 +168,11 @@
r_pos = 0.001
self.R = numpy.matrix([[(r_pos**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.L = self.A * self.KalmanGain
self.K_unaugmented = self.K
@@ -244,7 +250,8 @@
ff_U = controller_hood.Kff * (next_goal - observer_hood.A * goal)
- U_uncapped = controller_hood.K * (goal - observer_hood.X_hat) + ff_U
+ U_uncapped = controller_hood.K * (goal -
+ observer_hood.X_hat) + ff_U
U = U_uncapped.copy()
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(hood.X[0, 0])
@@ -310,12 +317,11 @@
# Test moving the hood with constant separation.
initial_X = numpy.matrix([[0.0], [0.0]])
R = numpy.matrix([[numpy.pi / 4.0], [0.0], [0.0]])
- scenario_plotter.run_test(
- hood,
- end_goal=R,
- controller_hood=hood_controller,
- observer_hood=observer_hood,
- iterations=200)
+ scenario_plotter.run_test(hood,
+ end_goal=R,
+ controller_hood=hood_controller,
+ observer_hood=observer_hood,
+ iterations=200)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -328,8 +334,8 @@
else:
namespaces = ['y2017', 'control_loops', 'superstructure', 'hood']
hood = Hood('Hood')
- loop_writer = control_loop.ControlLoopWriter(
- 'Hood', [hood], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Hood', [hood],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f', hood.free_speed))
loop_writer.AddConstant(
@@ -340,7 +346,8 @@
integral_loop_writer = control_loop.ControlLoopWriter(
'IntegralHood', [integral_hood], namespaces=namespaces)
integral_loop_writer.AddConstant(
- control_loop.Constant('kLastReduction', '%f', integral_hood.last_G))
+ control_loop.Constant('kLastReduction', '%f',
+ integral_hood.last_G))
integral_loop_writer.Write(argv[3], argv[4])
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 7312e57..d4c3fe6 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -12,188 +12,203 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
gflags.DEFINE_bool('stall', False, 'If true, stall the indexer.')
+
class VelocityIndexer(control_loop.ControlLoop):
- def __init__(self, name='VelocityIndexer'):
- super(VelocityIndexer, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.71
- # Stall Current in Amps
- self.stall_current = 134
- self.free_speed_rpm = 18730.0
- # Free Speed in rotations/second.
- self.free_speed = self.free_speed_rpm / 60.0
- # Free Current in Amps
- self.free_current = 0.7
- # Moment of inertia of the indexer halves in kg m^2
- # This is measured as Iyy in CAD (the moment of inertia around the Y axis).
- # Inner part of indexer -> Iyy = 59500 lb * mm * mm
- # Inner spins with 12 / 48 * 18 / 48 * 24 / 36 * 16 / 72
- # Outer part of indexer -> Iyy = 210000 lb * mm * mm
- # 1 775 pro -> 12 / 48 * 18 / 48 * 30 / 422
- self.J_inner = 0.0269
- self.J_outer = 0.0952
- # Gear ratios for the inner and outer parts.
- self.G_inner = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (12.0 / 84.0)
- self.G_outer = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (24.0 / 420.0)
+ def __init__(self, name='VelocityIndexer'):
+ super(VelocityIndexer, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.71
+ # Stall Current in Amps
+ self.stall_current = 134
+ self.free_speed_rpm = 18730.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
+ # Free Current in Amps
+ self.free_current = 0.7
+ # Moment of inertia of the indexer halves in kg m^2
+ # This is measured as Iyy in CAD (the moment of inertia around the Y axis).
+ # Inner part of indexer -> Iyy = 59500 lb * mm * mm
+ # Inner spins with 12 / 48 * 18 / 48 * 24 / 36 * 16 / 72
+ # Outer part of indexer -> Iyy = 210000 lb * mm * mm
+ # 1 775 pro -> 12 / 48 * 18 / 48 * 30 / 422
- # Motor inertia in kg m^2
- self.motor_inertia = 0.00001187
+ self.J_inner = 0.0269
+ self.J_outer = 0.0952
+ # Gear ratios for the inner and outer parts.
+ self.G_inner = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (12.0 /
+ 84.0)
+ self.G_outer = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (24.0 /
+ 420.0)
- # The output coordinate system is in radians for the inner part of the
- # indexer.
- # Compute the effective moment of inertia assuming all the mass is in that
- # coordinate system.
- self.J = (
- self.J_inner * self.G_inner * self.G_inner +
- self.J_outer * self.G_outer * self.G_outer) / (self.G_inner * self.G_inner) + \
- self.motor_inertia * ((1.0 / self.G_inner) ** 2.0)
- glog.debug('Indexer J is %f', self.J)
- self.G = self.G_inner
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.00001187
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
+ # The output coordinate system is in radians for the inner part of the
+ # indexer.
+ # Compute the effective moment of inertia assuming all the mass is in that
+ # coordinate system.
+ self.J = (
+ self.J_inner * self.G_inner * self.G_inner +
+ self.J_outer * self.G_outer * self.G_outer) / (self.G_inner * self.G_inner) + \
+ self.motor_inertia * ((1.0 / self.G_inner) ** 2.0)
+ glog.debug('Indexer J is %f', self.J)
+ self.G = self.G_inner
- # State feedback matrices
- # [angular velocity]
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.G * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Control loop time step
+ self.dt = 0.005
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [angular velocity]
+ self.A_continuous = numpy.matrix([[
+ -self.Kt / self.Kv / (self.J * self.G * self.G * self.resistance)
+ ]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.G * self.resistance)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([.75])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.PlaceObserverPoles([0.3])
+ self.PlaceControllerPoles([.75])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ self.InitializeState()
class Indexer(VelocityIndexer):
- def __init__(self, name='Indexer'):
- super(Indexer, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='Indexer'):
+ super(Indexer, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
- self.A_continuous[0, 1] = 1
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.rpl = .45
- self.ipl = 0.07
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.K[0, 1:2] = self.K_unaugmented
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 2)))
- self.Kff[0, 1:2] = self.Kff_unaugmented
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl])
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class IntegralIndexer(Indexer):
- def __init__(self, name="IntegralIndexer", voltage_error_noise=None):
- super(IntegralIndexer, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralIndexer", voltage_error_noise=None):
+ super(IntegralIndexer, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.01
- q_vel = 2.0
- q_voltage = 0.6
- if voltage_error_noise is not None:
- q_voltage = voltage_error_noise
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.Q = 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)]])
+ q_pos = 0.01
+ q_vel = 2.0
+ q_voltage = 0.6
+ if voltage_error_noise is not None:
+ q_voltage = voltage_error_noise
- r_pos = 0.001
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ self.Q = 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)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.001
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.stall_ratio = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, indexer, goal, iterations=200, controller_indexer=None,
- observer_indexer=None):
- """Runs the indexer plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.stall_ratio = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self,
+ indexer,
+ goal,
+ iterations=200,
+ controller_indexer=None,
+ observer_indexer=None):
+ """Runs the indexer plant with an initial condition and goal.
Args:
indexer: Indexer object to use.
@@ -205,141 +220,148 @@
should use the actual state.
"""
- if controller_indexer is None:
- controller_indexer = indexer
+ if controller_indexer is None:
+ controller_indexer = indexer
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + indexer.dt
- else:
- initial_t = 0
-
- for i in range(iterations):
- X_hat = indexer.X
-
- if observer_indexer is not None:
- X_hat = observer_indexer.X_hat
- observer_indexer.Y = indexer.Y
- observer_indexer.CorrectObserver(numpy.matrix([[0.0]]))
- self.x_hat.append(observer_indexer.X_hat[1, 0])
- self.offset.append(observer_indexer.X_hat[2, 0])
-
- ff_U = controller_indexer.Kff * (goal - observer_indexer.A * goal)
-
- U = controller_indexer.K * (goal - X_hat) + ff_U
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(indexer.X[0, 0])
-
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
-
- self.v.append(indexer.X[1, 0])
- self.a.append((self.v[-1] - last_v) / indexer.dt)
-
- applied_U = U.copy()
- if i >= 40:
- applied_U -= 2
-
- if FLAGS.stall and i >= 40:
- indexer.X[1, 0] = 0.0
- else:
- indexer.Update(applied_U)
-
- if observer_indexer is not None:
- clipped_u = U[0, 0]
- clip_u_value = 3.0
- if clipped_u < 0:
- clipped_u = min(clipped_u, -clip_u_value)
+ if self.t:
+ initial_t = self.t[-1] + indexer.dt
else:
- clipped_u = max(clipped_u, clip_u_value)
+ initial_t = 0
- self.stall_ratio.append(10 * (-self.offset[-1] / clipped_u))
+ for i in range(iterations):
+ X_hat = indexer.X
- observer_indexer.PredictObserver(U)
+ if observer_indexer is not None:
+ X_hat = observer_indexer.X_hat
+ observer_indexer.Y = indexer.Y
+ observer_indexer.CorrectObserver(numpy.matrix([[0.0]]))
+ self.x_hat.append(observer_indexer.X_hat[1, 0])
+ self.offset.append(observer_indexer.X_hat[2, 0])
- self.t.append(initial_t + i * indexer.dt)
- self.u.append(U[0, 0])
+ ff_U = controller_indexer.Kff * (goal - observer_indexer.A * goal)
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.v, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ U = controller_indexer.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(indexer.X[0, 0])
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.plot(self.t, self.stall_ratio, label='stall_ratio')
- pylab.plot(self.t,
- [10.0 if x > 6.0 else 0.0 for x in self.stall_ratio],
- label='is_stalled')
- pylab.legend()
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ self.v.append(indexer.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / indexer.dt)
- pylab.show()
+ applied_U = U.copy()
+ if i >= 40:
+ applied_U -= 2
+
+ if FLAGS.stall and i >= 40:
+ indexer.X[1, 0] = 0.0
+ else:
+ indexer.Update(applied_U)
+
+ if observer_indexer is not None:
+ clipped_u = U[0, 0]
+ clip_u_value = 3.0
+ if clipped_u < 0:
+ clipped_u = min(clipped_u, -clip_u_value)
+ else:
+ clipped_u = max(clipped_u, clip_u_value)
+
+ self.stall_ratio.append(10 * (-self.offset[-1] / clipped_u))
+
+ observer_indexer.PredictObserver(U)
+
+ self.t.append(initial_t + i * indexer.dt)
+ self.u.append(U[0, 0])
+
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.plot(self.t, self.stall_ratio, label='stall_ratio')
+ pylab.plot(self.t,
+ [10.0 if x > 6.0 else 0.0 for x in self.stall_ratio],
+ label='is_stalled')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- indexer = Indexer()
- indexer_controller = IntegralIndexer()
- observer_indexer = IntegralIndexer()
+ indexer = Indexer()
+ indexer_controller = IntegralIndexer()
+ observer_indexer = IntegralIndexer()
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [20.0], [0.0]])
- scenario_plotter.run_test(indexer, goal=R, controller_indexer=indexer_controller,
- observer_indexer=observer_indexer, iterations=200)
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [20.0], [0.0]])
+ scenario_plotter.run_test(indexer,
+ goal=R,
+ controller_indexer=indexer_controller,
+ observer_indexer=observer_indexer,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- indexer = Indexer()
- indexer_controller = IntegralIndexer(voltage_error_noise=1.5)
- observer_indexer = IntegralIndexer(voltage_error_noise=1.5)
+ indexer = Indexer()
+ indexer_controller = IntegralIndexer(voltage_error_noise=1.5)
+ observer_indexer = IntegralIndexer(voltage_error_noise=1.5)
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [20.0], [0.0]])
- scenario_plotter.run_test(indexer, goal=R, controller_indexer=indexer_controller,
- observer_indexer=observer_indexer, iterations=200)
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [20.0], [0.0]])
+ scenario_plotter.run_test(indexer,
+ goal=R,
+ controller_indexer=indexer_controller,
+ observer_indexer=observer_indexer,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- if len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file names')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'indexer']
- indexer = Indexer('Indexer')
- loop_writer = control_loop.ControlLoopWriter('Indexer', [indexer],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', indexer.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', indexer.G))
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file names')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'indexer']
+ indexer = Indexer('Indexer')
+ loop_writer = control_loop.ControlLoopWriter('Indexer', [indexer],
+ namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', indexer.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', indexer.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_indexer = IntegralIndexer('IntegralIndexer')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralIndexer', [integral_indexer], namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_indexer = IntegralIndexer('IntegralIndexer')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralIndexer', [integral_indexer], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
- stuck_integral_indexer = IntegralIndexer('StuckIntegralIndexer',
- voltage_error_noise=1.5)
- stuck_integral_loop_writer = control_loop.ControlLoopWriter(
- 'StuckIntegralIndexer', [stuck_integral_indexer], namespaces=namespaces)
- stuck_integral_loop_writer.Write(argv[5], argv[6])
+ stuck_integral_indexer = IntegralIndexer('StuckIntegralIndexer',
+ voltage_error_noise=1.5)
+ stuck_integral_loop_writer = control_loop.ControlLoopWriter(
+ 'StuckIntegralIndexer', [stuck_integral_indexer],
+ namespaces=namespaces)
+ stuck_integral_loop_writer.Write(argv[5], argv[6])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/polydrivetrain.py b/y2017/control_loops/python/polydrivetrain.py
index 498a2c3..e181ef5 100755
--- a/y2017/control_loops/python/polydrivetrain.py
+++ b/y2017/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2017',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2017', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/polydrivetrain_test.py b/y2017/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2017/control_loops/python/polydrivetrain_test.py
+++ b/y2017/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
class TestVelocityDrivetrain(unittest.TestCase):
- def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
- H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- K = numpy.matrix([[x1_max],
- [-x1_min],
- [x2_max],
- [-x2_min]])
- return polytope.HPolytope(H, K)
- def test_coerce_inside(self):
- """Tests coercion when the point is inside the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+ return polytope.HPolytope(H, K)
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
- numpy.matrix([[1.5], [1.5]])),
- numpy.matrix([[1.5], [1.5]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_intersect(self):
- """Tests coercion when the line intersects the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_no_intersect(self):
- """Tests coercion when the line does not intersect the box."""
- box = self.MakeBox(3, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[3.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_middle_of_edge(self):
- """Tests coercion when the line intersects the middle of an edge."""
- box = self.MakeBox(0, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[-1, 1]])
- w = 0
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
- def test_coerce_perpendicular_line(self):
- """Tests coercion when the line does not intersect and is in quadrant 2."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = -x2
- K = numpy.matrix([[1, 1]])
- w = 0
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[1.0], [1.0]]))
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index be4fb81..47b7217 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -204,8 +204,8 @@
glog.debug('A: \n%s', repr(self.A_continuous))
glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
- glog.debug('schur(A): \n%s', repr(
- scipy.linalg.schur(self.A_continuous)))
+ glog.debug('schur(A): \n%s',
+ repr(scipy.linalg.schur(self.A_continuous)))
glog.debug('A_dt(A): \n%s', repr(self.A))
q_pos = 0.01
@@ -220,15 +220,17 @@
r_pos = 0.0003
self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
- _, _, 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)
+ _, _, 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)
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=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
self.K_unaugmented = self.K
@@ -363,13 +365,12 @@
shooter_controller = IntegralShooter()
observer_shooter_hybrid = IntegralShooter()
- scenario_plotter_int.run_test(
- shooter,
- goal=R,
- controller_shooter=shooter_controller,
- observer_shooter=observer_shooter_hybrid,
- iterations=iterations,
- hybrid_obs=True)
+ scenario_plotter_int.run_test(shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter_hybrid,
+ iterations=iterations,
+ hybrid_obs=True)
scenario_plotter_int.Plot()
@@ -380,8 +381,8 @@
else:
namespaces = ['y2017', 'control_loops', 'superstructure', 'shooter']
shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter(
- 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f', shooter.free_speed))
loop_writer.AddConstant(
diff --git a/y2017/control_loops/python/turret.py b/y2017/control_loops/python/turret.py
index e67904d..6407133 100755
--- a/y2017/control_loops/python/turret.py
+++ b/y2017/control_loops/python/turret.py
@@ -12,166 +12,176 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Turret(control_loop.ControlLoop):
- def __init__(self, name='Turret'):
- super(Turret, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.71
- # Stall Current in Amps
- self.stall_current = 134
- self.free_speed_rpm = 18730.0
- # Free Speed in rotations/second.
- self.free_speed = self.free_speed_rpm / 60.0
- # Free Current in Amps
- self.free_current = 0.7
- # Resistance of the motor
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = (12.0 / 60.0) * (11.0 / 94.0)
+ def __init__(self, name='Turret'):
+ super(Turret, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.71
+ # Stall Current in Amps
+ self.stall_current = 134
+ self.free_speed_rpm = 18730.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
+ # Free Current in Amps
+ self.free_current = 0.7
- # Motor inertia in kg * m^2
- self.motor_inertia = 0.00001187
+ # Resistance of the motor
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = (12.0 / 60.0) * (11.0 / 94.0)
- # Moment of inertia, measured in CAD.
- # Extra mass to compensate for friction is added on.
- self.J = 0.06 + self.motor_inertia * ((1.0 / self.G) ** 2.0)
- glog.debug('Turret J is: %f', self.J)
+ # Motor inertia in kg * m^2
+ self.motor_inertia = 0.00001187
- # Control loop time step
- self.dt = 0.005
+ # Moment of inertia, measured in CAD.
+ # Extra mass to compensate for friction is added on.
+ self.J = 0.06 + self.motor_inertia * ((1.0 / self.G)**2.0)
+ glog.debug('Turret J is: %f', self.J)
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.Kt / (self.resistance * self.J * self.Kv * self.G * self.G)
- C2 = self.Kt / (self.J * self.resistance * self.G)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.Kt / (self.resistance * self.J * self.Kv * self.G * self.G)
+ C2 = self.Kt / (self.J * self.resistance * self.G)
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0],
- [C2]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix([[0], [C2]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- controllability = controls.ctrb(self.A, self.B)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- glog.debug('Free speed is %f',
- -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
+ controllability = controls.ctrb(self.A, self.B)
- # Calculate the LQR controller gain
- q_pos = 0.20
- q_vel = 5.0
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel ** 2.0))]])
+ glog.debug('Free speed is %f',
+ -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ # Calculate the LQR controller gain
+ q_pos = 0.20
+ q_vel = 5.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel**2.0))]])
- # Calculate the feed forwards gain.
- q_pos_ff = 0.005
- q_vel_ff = 1.0
- self.Qff = numpy.matrix([[(1.0 / (q_pos_ff ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel_ff ** 2.0))]])
+ self.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ # Calculate the feed forwards gain.
+ q_pos_ff = 0.005
+ q_vel_ff = 1.0
+ self.Qff = numpy.matrix([[(1.0 / (q_pos_ff**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel_ff**2.0))]])
- q_pos = 0.10
- q_vel = 1.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.10
+ q_vel = 1.65
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0], [0.0, (q_vel**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
class IntegralTurret(Turret):
- def __init__(self, name='IntegralTurret'):
- super(IntegralTurret, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='IntegralTurret'):
+ super(IntegralTurret, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.12
- q_vel = 2.00
- q_voltage = 3.0
- self.Q = 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)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.12
+ q_vel = 2.00
+ q_voltage = 3.0
+ self.Q = 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)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
+ self.L = self.A * self.KalmanGain
- self.Kff = numpy.concatenate((self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
- self.InitializeState()
+ self.Kff = numpy.concatenate(
+ (self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+
+ self.InitializeState()
+
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, turret, end_goal,
- controller_turret,
- observer_turret=None,
- iterations=200):
- """Runs the turret plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self,
+ turret,
+ end_goal,
+ controller_turret,
+ observer_turret=None,
+ iterations=200):
+ """Runs the turret plant with an initial condition and goal.
Args:
turret: turret object to use.
@@ -183,130 +193,138 @@
iterations: Number of timesteps to run the model for.
"""
- if controller_turret is None:
- controller_turret = turret
+ if controller_turret is None:
+ controller_turret = turret
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + turret.dt
- else:
- initial_t = 0
+ if self.t:
+ initial_t = self.t[-1] + turret.dt
+ else:
+ initial_t = 0
- goal = numpy.concatenate((turret.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((turret.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
- profile = TrapezoidProfile(turret.dt)
- profile.set_maximum_acceleration(100.0)
- profile.set_maximum_velocity(7.0)
- profile.SetGoal(goal[0, 0])
+ profile = TrapezoidProfile(turret.dt)
+ profile.set_maximum_acceleration(100.0)
+ profile.set_maximum_velocity(7.0)
+ profile.SetGoal(goal[0, 0])
- U_last = numpy.matrix(numpy.zeros((1, 1)))
- for i in range(iterations):
- observer_turret.Y = turret.Y
- observer_turret.CorrectObserver(U_last)
+ U_last = numpy.matrix(numpy.zeros((1, 1)))
+ for i in range(iterations):
+ observer_turret.Y = turret.Y
+ observer_turret.CorrectObserver(U_last)
- self.offset.append(observer_turret.X_hat[2, 0])
- self.x_hat.append(observer_turret.X_hat[0, 0])
+ self.offset.append(observer_turret.X_hat[2, 0])
+ self.x_hat.append(observer_turret.X_hat[0, 0])
- next_goal = numpy.concatenate(
- (profile.Update(end_goal[0, 0], end_goal[1, 0]),
- numpy.matrix(numpy.zeros((1, 1)))),
- axis=0)
+ next_goal = numpy.concatenate(
+ (profile.Update(end_goal[0, 0], end_goal[1, 0]),
+ numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
- ff_U = controller_turret.Kff * (next_goal - observer_turret.A * goal)
+ ff_U = controller_turret.Kff * (next_goal -
+ observer_turret.A * goal)
- U_uncapped = controller_turret.K * (goal - observer_turret.X_hat) + ff_U
- U_uncapped = controller_turret.K * (end_goal - observer_turret.X_hat)
- U = U_uncapped.copy()
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(turret.X[0, 0])
+ U_uncapped = controller_turret.K * (goal -
+ observer_turret.X_hat) + ff_U
+ U_uncapped = controller_turret.K * (end_goal -
+ observer_turret.X_hat)
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(turret.X[0, 0])
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- self.v.append(turret.X[1, 0])
- self.a.append((self.v[-1] - last_v) / turret.dt)
+ self.v.append(turret.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / turret.dt)
- offset = 0.0
- if i > 100:
- offset = 2.0
- turret.Update(U + offset)
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ turret.Update(U + offset)
- observer_turret.PredictObserver(U)
+ observer_turret.PredictObserver(U)
- self.t.append(initial_t + i * turret.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * turret.dt)
+ self.u.append(U[0, 0])
- ff_U -= U_uncapped - U
- goal = controller_turret.A * goal + controller_turret.B * ff_U
+ ff_U -= U_uncapped - U
+ goal = controller_turret.A * goal + controller_turret.B * ff_U
- if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ if U[0, 0] != U_uncapped[0, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
- glog.debug('Time: %f', self.t[-1])
- glog.debug('goal_error %s', repr(end_goal - goal))
- glog.debug('error %s', repr(observer_turret.X_hat - end_goal))
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer_turret.X_hat - end_goal))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- turret = Turret()
- turret_controller = IntegralTurret()
- observer_turret = IntegralTurret()
+ turret = Turret()
+ turret_controller = IntegralTurret()
+ observer_turret = IntegralTurret()
- # Test moving the turret with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[numpy.pi/2.0], [0.0], [0.0]])
- scenario_plotter.run_test(turret, end_goal=R,
- controller_turret=turret_controller,
- observer_turret=observer_turret, iterations=200)
+ # Test moving the turret with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0], [0.0]])
+ scenario_plotter.run_test(turret,
+ end_goal=R,
+ controller_turret=turret_controller,
+ observer_turret=observer_turret,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the turret and integral turret.')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'turret']
- turret = Turret('Turret')
- loop_writer = control_loop.ControlLoopWriter('Turret', [turret],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', turret.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', turret.G))
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the turret and integral turret.'
+ )
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'turret']
+ turret = Turret('Turret')
+ loop_writer = control_loop.ControlLoopWriter('Turret', [turret],
+ namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', turret.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', turret.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_turret = IntegralTurret('IntegralTurret')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralTurret', [integral_turret],
- namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_turret = IntegralTurret('IntegralTurret')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralTurret', [integral_turret], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))