Upgraded //y2012 to use the common drive code.
This is a fixup from changing the call signature of DoCoerceGoal
Change-Id: I6a99f666bebd9a728f42083a9c0280e8f2dab295
diff --git a/y2012/control_loops/python/drivetrain.py b/y2012/control_loops/python/drivetrain.py
index a57f211..f9004c0 100755
--- a/y2012/control_loops/python/drivetrain.py
+++ b/y2012/control_loops/python/drivetrain.py
@@ -4,7 +4,6 @@
from frc971.control_loops.python import controls
import numpy
import sys
-import argparse
from matplotlib import pylab
import gflags
@@ -89,8 +88,14 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- q_pos = 0.12
- q_vel = 1.0
+ if left_low or right_low:
+ q_pos = 0.12
+ q_vel = 1.0
+ else:
+ q_pos = 0.14
+ q_vel = 0.95
+
+ # Tune the LQR controller
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
[0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
@@ -100,9 +105,9 @@
[0.0, (1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- glog.debug('DT K %s', name)
- glog.debug(str(self.K))
+ glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', repr(self.K))
self.hlp = 0.3
self.llp = 0.4
@@ -110,6 +115,7 @@
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
self.InitializeState()
@@ -120,12 +126,19 @@
self.unaugmented_A_continuous = self.A_continuous
self.unaugmented_B_continuous = self.B_continuous
- # The states are
# The practical voltage applied to the wheels is
# V_left = U_left + left_voltage_error
#
+ # The states are
# [left position, left velocity, right position, right velocity,
# left voltage error, right voltage error, angular_error]
+ #
+ # The left and right positions are filtered encoder positions and are not
+ # adjusted for heading error.
+ # The turn velocity as computed by the left and right velocities is
+ # adjusted by the gyro velocity.
+ # The angular_error is the angular velocity error between the wheel speed
+ # and the gyro speed.
self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
@@ -170,15 +183,34 @@
self.L = self.A * self.KalmanGain
- # We need a nothing controller for the autogen code to be happy.
+ unaug_K = self.K
+
+ # Implement a nice closed loop controller for use by the closed loop
+ # controller.
self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+ self.K[0:2, 0:4] = unaug_K
+ self.K[0, 4] = 1.0
+ self.K[1, 5] = 1.0
+
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos = 0.005
+ qff_vel = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
+ self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
+ self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
+ self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
+ self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+ self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+
+ self.InitializeState()
def main(argv):
argv = FLAGS(argv)
+ glog.init()
# Simulate the response of the system to a step input.
- drivetrain = Drivetrain()
+ drivetrain = Drivetrain(left_low=False, right_low=False)
simulated_left = []
simulated_right = []
for _ in xrange(100):
@@ -189,24 +221,33 @@
if FLAGS.plot:
pylab.plot(range(100), simulated_left)
pylab.plot(range(100), simulated_right)
+ pylab.suptitle('Acceleration Test')
pylab.show()
# Simulate forwards motion.
- drivetrain = Drivetrain()
+ drivetrain = Drivetrain(left_low=False, right_low=False)
close_loop_left = []
close_loop_right = []
+ left_power = []
+ right_power = []
R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
- for _ in xrange(100):
+ for _ in xrange(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
drivetrain.U_min, drivetrain.U_max)
drivetrain.UpdateObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
if FLAGS.plot:
- pylab.plot(range(100), close_loop_left)
- pylab.plot(range(100), close_loop_right)
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, label='right position')
+ pylab.plot(range(300), left_power, label='left power')
+ pylab.plot(range(300), right_power, label='right power')
+ pylab.suptitle('Linear Move')
+ pylab.legend()
pylab.show()
# Try turning in place
@@ -225,6 +266,7 @@
if FLAGS.plot:
pylab.plot(range(100), close_loop_left)
pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Angular Move')
pylab.show()
# Try turning just one side.
@@ -243,6 +285,7 @@
if FLAGS.plot:
pylab.plot(range(100), close_loop_left)
pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Pivot')
pylab.show()
# Write the generated constants out to a file.
@@ -296,6 +339,10 @@
drivetrain_low_low.Kv))
dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
drivetrain_low_low.Kt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
+ drivetrain_low_low.G_low))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
+ drivetrain_high_high.G_high))
dog_loop_writer.Write(argv[1], argv[2])