Upgraded //y2015_bot3 to use the common drive code.
This is a fixup from changing the call signature of DoCoerceGoal
Change-Id: I3b96da7107a524ca0e6cd66c9de3ae3443124253
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index 88caa73..e85d131 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -2,15 +2,24 @@
import numpy
import sys
-import polytope
-import drivetrain
-import control_loop
-import controls
+from frc971.control_loops.python import polytope
+from y2015_bot3.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
from frc971.control_loops.python.cim import CIM
from matplotlib import pylab
+import gflags
+import glog
+
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
def CoerceGoal(region, K, w, R):
"""Intersects a line with a region, and finds the closest point to R.
@@ -38,7 +47,7 @@
perpendicular_vector = K.T / numpy.linalg.norm(K)
parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
[-perpendicular_vector[0, 0]]])
-
+
# We want to impose the constraint K * X = w on the polytope H * X <= k.
# We do this by breaking X up into parallel and perpendicular components to
# the half plane. This gives us the following equation.
@@ -125,7 +134,7 @@
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
- self.R = self._drivetrain.R
+ self.resistance = self._drivetrain.resistance
self.r = self._drivetrain.r
self.Kv = self._drivetrain.Kv
self.Kt = self._drivetrain.Kt
@@ -175,6 +184,10 @@
[[0.0],
[0.0]])
+ self.U_ideal = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
# ttrust is the comprimise between having full throttle negative inertia,
# and having no throttle negative inertia. A value of 0 is full throttle
# inertia. A value of 1 is no throttle negative inertia.
@@ -230,9 +243,9 @@
self.CurrentDrivetrain().r)
#print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
high_power = high_torque * high_omega
low_power = low_torque * low_omega
#if should_print:
@@ -251,16 +264,16 @@
#goal_gear_is_high = True
if not self.IsInGear(current_gear):
- print gear_name, 'Not in gear.'
+ glog.debug('%s Not in gear.', gear_name)
return current_gear
else:
is_high = current_gear is VelocityDrivetrain.HIGH
if is_high != goal_gear_is_high:
if goal_gear_is_high:
- print gear_name, 'Shifting up.'
+ glog.debug('%s Shifting up.', gear_name)
return VelocityDrivetrain.SHIFTING_UP
else:
- print gear_name, 'Shifting down.'
+ glog.debug('%s Shifting down.', gear_name)
return VelocityDrivetrain.SHIFTING_DOWN
else:
return current_gear
@@ -363,7 +376,7 @@
FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
else:
- print 'Not all in gear'
+ glog.debug('Not all in gear')
if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
# TODO(austin): Use battery volts here.
R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
@@ -392,36 +405,36 @@
self.right_gear, self.right_shifter_position = self.SimShifter(
self.right_gear, self.right_shifter_position)
- print "U is", self.U[0, 0], self.U[1, 0]
- print "Left shifter", self.left_gear, self.left_shifter_position, "Right shifter", self.right_gear, self.right_shifter_position
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d',
+ self.left_gear, self.left_shifter_position,
+ self.right_gear, self.right_shifter_position)
def main(argv):
+ argv = FLAGS(argv)
+
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 7:
- print "Expected .h file name and .cc file name"
- else:
- dog_loop_writer = control_loop.ControlLoopWriter(
- "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high],
- namespaces=['y2015_bot3', 'control_loops'])
-
- if argv[1][-3:] == '.cc':
- dog_loop_writer.Write(argv[2], argv[1])
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
else:
+ namespaces = ['y2015_bot3', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high],
+ namespaces=namespaces)
+
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()],
- namespaces=['y2015_bot3', 'control_loops'])
+ cim_writer = control_loop.ControlLoopWriter(
+ "CIM", [CIM()], namespaces=namespaces)
- if argv[5][-3:] == '.cc':
- cim_writer.Write(argv[6], argv[5])
- else:
- cim_writer.Write(argv[5], argv[6])
- return
+ cim_writer.Write(argv[3], argv[4])
+ return
vl_plot = []
vr_plot = []
@@ -436,16 +449,16 @@
vdrivetrain.left_gear = VelocityDrivetrain.LOW
vdrivetrain.right_gear = VelocityDrivetrain.LOW
- print "K is", vdrivetrain.CurrentDrivetrain().K
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
- print "Left is high"
+ glog.debug('Left is high')
else:
- print "Left is low"
+ glog.debug('Left is low')
if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
- print "Right is high"
+ glog.debug('Right is high')
else:
- print "Right is low"
+ glog.debug('Right is low')
for t in numpy.arange(0, 1.7, vdrivetrain.dt):
if t < 0.5: