Run yapf on all python files in the repo
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I221e04c3f517fab8535b22551553799e0fee7a80
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 3c47934..0b5d87c 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -44,8 +44,8 @@
self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
self.A_continuous[0:2, 0:2] = self._shoulder.A_continuous
- self.A_continuous[2:4, 0:2] = (
- self._shoulder.A_continuous - self._shooter.A_continuous)
+ self.A_continuous[2:4, 0:2] = (self._shoulder.A_continuous -
+ self._shooter.A_continuous)
self.A_continuous[2:4, 2:4] = self._shooter.A_continuous
self.B_continuous = numpy.matrix(numpy.zeros((4, 2)))
@@ -131,8 +131,11 @@
self.R[0, 0] = r_voltage**2.0
self.R[1, 1] = r_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.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.U_max = numpy.matrix([[12.0], [12.0]])
@@ -182,8 +185,11 @@
self.R[0, 0] = r_pos**2.0
self.R[1, 1] = 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
@@ -365,20 +371,19 @@
pylab.plot(self.t, self.x_shooter, label='x shooter')
pylab.plot(self.t, self.x_hat_shooter, label='x_hat shooter')
pylab.plot(self.t, self.goal_x_shooter, label='goal x shooter')
- pylab.plot(
- self.t,
- map(operator.add, self.x_shooter, self.x_shoulder),
- label='x shooter ground')
- pylab.plot(
- self.t,
- map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
- label='x_hat shooter ground')
+ pylab.plot(self.t,
+ map(operator.add, self.x_shooter, self.x_shoulder),
+ label='x shooter ground')
+ pylab.plot(self.t,
+ map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
+ label='x_hat shooter ground')
pylab.legend()
pylab.subplot(3, 1, 2)
pylab.plot(self.t, self.u_shoulder, label='u shoulder')
- pylab.plot(
- self.t, self.offset_shoulder, label='voltage_offset shoulder')
+ pylab.plot(self.t,
+ self.offset_shoulder,
+ label='voltage_offset shoulder')
pylab.plot(self.t, self.u_shooter, label='u shooter')
pylab.plot(self.t, self.offset_shooter, label='voltage_offset shooter')
pylab.legend()
@@ -401,8 +406,8 @@
J_decelerating = 7
arm = Arm(name='AcceleratingArm', J=J_accelerating)
- arm_integral_controller = IntegralArm(
- name='AcceleratingIntegralArm', J=J_accelerating)
+ arm_integral_controller = IntegralArm(name='AcceleratingIntegralArm',
+ J=J_accelerating)
arm_observer = IntegralArm()
# Test moving the shoulder with constant separation.
@@ -418,12 +423,11 @@
arm.X = initial_X[0:4, 0]
arm_observer.X = initial_X
- scenario_plotter.run_test(
- arm=arm,
- end_goal=R,
- iterations=300,
- controller=arm_integral_controller,
- observer=arm_observer)
+ scenario_plotter.run_test(arm=arm,
+ end_goal=R,
+ iterations=300,
+ controller=arm_integral_controller,
+ observer=arm_observer)
if len(argv) != 5:
glog.fatal(
@@ -432,8 +436,9 @@
else:
namespaces = ['y2016', 'control_loops', 'superstructure']
decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
- loop_writer = control_loop.ControlLoopWriter(
- 'Arm', [arm, decelerating_arm], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Arm',
+ [arm, decelerating_arm],
+ namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
decelerating_integral_arm_controller = IntegralArm(
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index 8abdd74..a0205e7 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -11,31 +11,34 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-kDrivetrain = drivetrain.DrivetrainParams(J = 2.0,
- mass = 68,
- robot_radius = 0.601 / 2.0,
- wheel_radius = 0.097155 * 0.9811158901447808 / 118.0 * 115.75,
- G_high = 14.0 / 48.0 * 28.0 / 50.0 * 18.0 / 36.0,
- G_low = 14.0 / 48.0 * 18.0 / 60.0 * 18.0 / 36.0,
- q_pos_low = 0.12,
- q_pos_high = 0.14,
- q_vel_low = 1.0,
- q_vel_high = 0.95,
- has_imu = False,
- dt = 0.005,
- controller_poles = [0.67, 0.67])
+kDrivetrain = drivetrain.DrivetrainParams(
+ J=2.0,
+ mass=68,
+ robot_radius=0.601 / 2.0,
+ wheel_radius=0.097155 * 0.9811158901447808 / 118.0 * 115.75,
+ G_high=14.0 / 48.0 * 28.0 / 50.0 * 18.0 / 36.0,
+ G_low=14.0 / 48.0 * 18.0 / 60.0 * 18.0 / 36.0,
+ q_pos_low=0.12,
+ q_pos_high=0.14,
+ q_vel_low=1.0,
+ q_vel_high=0.95,
+ has_imu=False,
+ dt=0.005,
+ controller_poles=[0.67, 0.67])
+
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print("Expected .h file name and .cc file name")
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2016', kDrivetrain)
+ if FLAGS.plot:
+ drivetrain.PlotDrivetrainMotions(kDrivetrain)
+ elif len(argv) != 5:
+ print("Expected .h file name and .cc file name")
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2016', kDrivetrain)
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/polydrivetrain.py b/y2016/control_loops/python/polydrivetrain.py
index 1b7a0ba..69a7be9 100755
--- a/y2016/control_loops/python/polydrivetrain.py
+++ b/y2016/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], 'y2016',
- 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],
+ 'y2016', 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/y2016/control_loops/python/polydrivetrain_test.py b/y2016/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2016/control_loops/python/polydrivetrain_test.py
+++ b/y2016/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/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index 70af1b0..cde9c8d 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -135,8 +135,11 @@
r_pos = 0.05
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
@@ -256,12 +259,11 @@
initial_X = numpy.matrix([[0.0], [0.0]])
R = numpy.matrix([[0.0], [100.0], [0.0]])
- scenario_plotter.run_test(
- shooter,
- goal=R,
- controller_shooter=shooter_controller,
- observer_shooter=observer_shooter,
- iterations=200)
+ scenario_plotter.run_test(shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter,
+ iterations=200)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -271,8 +273,8 @@
else:
namespaces = ['y2016', 'control_loops', 'shooter']
shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter(
- 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
+ namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
integral_shooter = IntegralShooter('IntegralShooter')
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 7f46b2c..d406089 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -12,155 +12,168 @@
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 Shoulder(control_loop.ControlLoop):
- def __init__(self, name="Shoulder", J=None):
- super(Shoulder, self).__init__(name)
- # TODO(constants): Update all of these & retune poles.
- # Stall Torque in N m
- self.stall_torque = 0.71
- # Stall Current in Amps
- self.stall_current = 134
- # Free Speed in RPM
- self.free_speed = 18730
- # Free Current in Amps
- self.free_current = 0.7
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (42.0 / 12.0)
+ def __init__(self, name="Shoulder", J=None):
+ super(Shoulder, self).__init__(name)
+ # TODO(constants): Update all of these & retune poles.
+ # Stall Torque in N m
+ self.stall_torque = 0.71
+ # Stall Current in Amps
+ self.stall_current = 134
+ # Free Speed in RPM
+ self.free_speed = 18730
+ # Free Current in Amps
+ self.free_current = 0.7
- if J is None:
- self.J = 10.0
- else:
- self.J = J
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (42.0 / 12.0)
- # Control loop time step
- self.dt = 0.005
+ if J is None:
+ self.J = 10.0
+ else:
+ self.J = J
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
- C2 = self.Kt * self.G / (self.J * self.R)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
+ C2 = self.Kt * self.G / (self.J * self.R)
- # 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)
- q_pos = 0.16
- q_vel = 0.95
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel ** 2.0))]])
+ controllability = controls.ctrb(self.A, self.B)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ q_pos = 0.16
+ q_vel = 0.95
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel**2.0))]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+ self.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- glog.debug('Poles are %s for %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]), self._name)
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
- q_pos = 0.05
- q_vel = 2.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ glog.debug('Poles are %s for %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]),
+ self._name)
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.05
+ q_vel = 2.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)
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- self.L = self.A * self.KalmanGain
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
- # 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.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 IntegralShoulder(Shoulder):
- def __init__(self, name="IntegralShoulder"):
- super(IntegralShoulder, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralShoulder"):
+ super(IntegralShoulder, 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.08
- q_vel = 4.00
- q_voltage = 6.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.08
+ q_vel = 4.00
+ q_voltage = 6.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.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.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, shoulder, goal, iterations=200, controller_shoulder=None,
- observer_shoulder=None):
- """Runs the shoulder 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,
+ shoulder,
+ goal,
+ iterations=200,
+ controller_shoulder=None,
+ observer_shoulder=None):
+ """Runs the shoulder plant with an initial condition and goal.
Test for whether the goal has been reached and whether the separation
goes outside of the initial and goal values by more than
@@ -178,100 +191,106 @@
use the actual state.
"""
- if controller_shoulder is None:
- controller_shoulder = shoulder
+ if controller_shoulder is None:
+ controller_shoulder = shoulder
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + shoulder.dt
- else:
- initial_t = 0
+ if self.t:
+ initial_t = self.t[-1] + shoulder.dt
+ else:
+ initial_t = 0
- for i in range(iterations):
- X_hat = shoulder.X
+ for i in range(iterations):
+ X_hat = shoulder.X
- if observer_shoulder is not None:
- X_hat = observer_shoulder.X_hat
- self.x_hat.append(observer_shoulder.X_hat[0, 0])
+ if observer_shoulder is not None:
+ X_hat = observer_shoulder.X_hat
+ self.x_hat.append(observer_shoulder.X_hat[0, 0])
- U = controller_shoulder.K * (goal - X_hat)
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(shoulder.X[0, 0])
+ U = controller_shoulder.K * (goal - X_hat)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shoulder.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(shoulder.X[1, 0])
- self.a.append((self.v[-1] - last_v) / shoulder.dt)
+ self.v.append(shoulder.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / shoulder.dt)
- if observer_shoulder is not None:
- observer_shoulder.Y = shoulder.Y
- observer_shoulder.CorrectObserver(U)
- self.offset.append(observer_shoulder.X_hat[2, 0])
+ if observer_shoulder is not None:
+ observer_shoulder.Y = shoulder.Y
+ observer_shoulder.CorrectObserver(U)
+ self.offset.append(observer_shoulder.X_hat[2, 0])
- shoulder.Update(U + 2.0)
+ shoulder.Update(U + 2.0)
- if observer_shoulder is not None:
- observer_shoulder.PredictObserver(U)
+ if observer_shoulder is not None:
+ observer_shoulder.PredictObserver(U)
- self.t.append(initial_t + i * shoulder.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * shoulder.dt)
+ self.u.append(U[0, 0])
- glog.debug('Time: %f', self.t[-1])
+ glog.debug('Time: %f', self.t[-1])
- 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)
+ argv = FLAGS(argv)
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- shoulder = Shoulder()
- shoulder_controller = IntegralShoulder()
- observer_shoulder = IntegralShoulder()
+ shoulder = Shoulder()
+ shoulder_controller = IntegralShoulder()
+ observer_shoulder = IntegralShoulder()
- # Test moving the shoulder 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(shoulder, goal=R, controller_shoulder=shoulder_controller,
- observer_shoulder=observer_shoulder, iterations=200)
+ # Test moving the shoulder 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(shoulder,
+ goal=R,
+ controller_shoulder=shoulder_controller,
+ observer_shoulder=observer_shoulder,
+ 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 shoulder and integral shoulder.')
- else:
- namespaces = ['y2016', 'control_loops', 'superstructure']
- shoulder = Shoulder("Shoulder")
- loop_writer = control_loop.ControlLoopWriter('Shoulder', [shoulder],
- namespaces=namespaces)
- 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 shoulder and integral shoulder.'
+ )
+ else:
+ namespaces = ['y2016', 'control_loops', 'superstructure']
+ shoulder = Shoulder("Shoulder")
+ loop_writer = control_loop.ControlLoopWriter('Shoulder', [shoulder],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_shoulder = IntegralShoulder("IntegralShoulder")
- integral_loop_writer = control_loop.ControlLoopWriter("IntegralShoulder", [integral_shoulder],
- namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_shoulder = IntegralShoulder("IntegralShoulder")
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ "IntegralShoulder", [integral_shoulder], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index ca6874b..12dcc39 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -11,152 +11,165 @@
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 Wrist(control_loop.ControlLoop):
- def __init__(self, name="Wrist"):
- super(Wrist, self).__init__(name)
- # TODO(constants): Update all of these & retune poles.
- # Stall Torque in N m
- self.stall_torque = 0.71
- # Stall Current in Amps
- self.stall_current = 134
- # Free Speed in RPM
- self.free_speed = 18730
- # Free Current in Amps
- self.free_current = 0.7
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
+ def __init__(self, name="Wrist"):
+ super(Wrist, self).__init__(name)
+ # TODO(constants): Update all of these & retune poles.
+ # Stall Torque in N m
+ self.stall_torque = 0.71
+ # Stall Current in Amps
+ self.stall_current = 134
+ # Free Speed in RPM
+ self.free_speed = 18730
+ # Free Current in Amps
+ self.free_current = 0.7
- self.J = 0.35
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
- # Control loop time step
- self.dt = 0.005
+ self.J = 0.35
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
- C2 = self.Kt * self.G / (self.J * self.R)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
+ C2 = self.Kt * self.G / (self.J * self.R)
- # 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)
- q_pos = 0.20
- q_vel = 8.0
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel ** 2.0))]])
+ controllability = controls.ctrb(self.A, self.B)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ q_pos = 0.20
+ q_vel = 8.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel**2.0))]])
- glog.debug('Poles are %s for %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]), self._name)
+ self.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- q_pos = 0.05
- q_vel = 2.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ glog.debug('Poles are %s for %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]),
+ self._name)
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.05
+ q_vel = 2.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)
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- self.L = self.A * self.KalmanGain
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
- # 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.L = self.A * self.KalmanGain
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+ # 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()
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+
+ self.InitializeState()
+
class IntegralWrist(Wrist):
- def __init__(self, name="IntegralWrist"):
- super(IntegralWrist, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralWrist"):
+ super(IntegralWrist, 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.08
- q_vel = 4.00
- q_voltage = 1.5
- 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.08
+ q_vel = 4.00
+ q_voltage = 1.5
+ 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.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.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, wrist, goal, iterations=200, controller_wrist=None,
- observer_wrist=None):
- """Runs the wrist 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,
+ wrist,
+ goal,
+ iterations=200,
+ controller_wrist=None,
+ observer_wrist=None):
+ """Runs the wrist plant with an initial condition and goal.
Test for whether the goal has been reached and whether the separation
goes outside of the initial and goal values by more than
@@ -174,99 +187,105 @@
use the actual state.
"""
- if controller_wrist is None:
- controller_wrist = wrist
+ if controller_wrist is None:
+ controller_wrist = wrist
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + wrist.dt
- else:
- initial_t = 0
+ if self.t:
+ initial_t = self.t[-1] + wrist.dt
+ else:
+ initial_t = 0
- for i in range(iterations):
- X_hat = wrist.X
+ for i in range(iterations):
+ X_hat = wrist.X
- if observer_wrist is not None:
- X_hat = observer_wrist.X_hat
- self.x_hat.append(observer_wrist.X_hat[0, 0])
+ if observer_wrist is not None:
+ X_hat = observer_wrist.X_hat
+ self.x_hat.append(observer_wrist.X_hat[0, 0])
- U = controller_wrist.K * (goal - X_hat)
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(wrist.X[0, 0])
+ U = controller_wrist.K * (goal - X_hat)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(wrist.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(wrist.X[1, 0])
- self.a.append((self.v[-1] - last_v) / wrist.dt)
+ self.v.append(wrist.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / wrist.dt)
- if observer_wrist is not None:
- observer_wrist.Y = wrist.Y
- observer_wrist.CorrectObserver(U)
- self.offset.append(observer_wrist.X_hat[2, 0])
+ if observer_wrist is not None:
+ observer_wrist.Y = wrist.Y
+ observer_wrist.CorrectObserver(U)
+ self.offset.append(observer_wrist.X_hat[2, 0])
- wrist.Update(U + 2.0)
+ wrist.Update(U + 2.0)
- if observer_wrist is not None:
- observer_wrist.PredictObserver(U)
+ if observer_wrist is not None:
+ observer_wrist.PredictObserver(U)
- self.t.append(initial_t + i * wrist.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * wrist.dt)
+ self.u.append(U[0, 0])
- glog.debug('Time: %f', self.t[-1])
+ glog.debug('Time: %f', self.t[-1])
- 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):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- wrist = Wrist()
- wrist_controller = IntegralWrist()
- observer_wrist = IntegralWrist()
+ wrist = Wrist()
+ wrist_controller = IntegralWrist()
+ observer_wrist = IntegralWrist()
- # Test moving the wrist with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0]])
- scenario_plotter.run_test(wrist, goal=R, controller_wrist=wrist_controller,
- observer_wrist=observer_wrist, iterations=200)
+ # Test moving the wrist with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
+ scenario_plotter.run_test(wrist,
+ goal=R,
+ controller_wrist=wrist_controller,
+ observer_wrist=observer_wrist,
+ 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 wrist and integral wrist.')
- else:
- namespaces = ['y2016', 'control_loops', 'superstructure']
- wrist = Wrist('Wrist')
- loop_writer = control_loop.ControlLoopWriter(
- 'Wrist', [wrist], namespaces=namespaces)
- 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 wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['y2016', 'control_loops', 'superstructure']
+ wrist = Wrist('Wrist')
+ loop_writer = control_loop.ControlLoopWriter('Wrist', [wrist],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_wrist = IntegralWrist('IntegralWrist')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralWrist', [integral_wrist], namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_wrist = IntegralWrist('IntegralWrist')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralWrist', [integral_wrist], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))