added support for both drivetrains with the same code
Previously, we were just switching back and forth manually, which
doesn't work very well. The python code for (both) drivetrain loops now
generates constants for both robots and frc971/constants knows which one
to give the actual drivetrain control loop code.
Various other cleanups to things that were so hard to read it made it
difficult to figure out how to do this too.
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index c6a3f05..c39886a 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -50,8 +50,8 @@
class Drivetrain(control_loop.ControlLoop):
- def __init__(self, left_low=True, right_low=True):
- super(Drivetrain, self).__init__("Drivetrain")
+ def __init__(self, left_low=True, right_low=True, is_clutch=False):
+ super(Drivetrain, self).__init__(("Clutch" if is_clutch else "Dog" )+"Drivetrain")
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
@@ -62,7 +62,10 @@
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 4.5
+ if is_clutch:
+ self.J = 4.5
+ else:
+ self.J = 4.9
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).
@@ -77,8 +80,12 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_low = 16.0 / 60.0 * 19.0 / 50.0
- self.G_high = 28.0 / 48.0 * 19.0 / 50.0
+ if is_clutch:
+ self.G_low = 14.0 / 60.0 * 15.0 / 50.0
+ self.G_high = 30.0 / 44.0 * 15.0 / 50.0
+ else:
+ self.G_low = 16.0 / 60.0 * 17.0 / 50.0
+ self.G_high = 28.0 / 48.0 * 17.0 / 50.0
if left_low:
self.Gl = self.G_low
else:
@@ -200,14 +207,23 @@
#pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 3:
+ dog_drivetrain = Drivetrain(is_clutch=False)
+ clutch_drivetrain = Drivetrain(is_clutch=True)
+
+ if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
- loop_writer = control_loop.ControlLoopWriter("Drivetrain", [drivetrain])
+ dog_loop_writer = control_loop.ControlLoopWriter("DogDrivetrain", [dog_drivetrain])
if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
+ dog_loop_writer.Write(argv[2], argv[1])
else:
- loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ clutch_loop_writer = control_loop.ControlLoopWriter("ClutchDrivetrain", [clutch_drivetrain])
+ if argv[3][-3:] == '.cc':
+ clutch_loop_writer.Write(argv[4], argv[3])
+ else:
+ clutch_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index f28cf42..5ffcff4 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -96,10 +96,11 @@
class VelocityDrivetrainModel(control_loop.ControlLoop):
- def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel", is_clutch=False):
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
- right_low=right_low)
+ right_low=right_low,
+ is_clutch=is_clutch)
self.dt = 0.01
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
@@ -137,12 +138,13 @@
SHIFTING_UP = 'up'
SHIFTING_DOWN = 'down'
- def __init__(self):
+ def __init__(self, is_clutch):
+ prefix = 'Clutch' if is_clutch else 'Dog'
self.drivetrain_low_low = VelocityDrivetrainModel(
- left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
- self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
- self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
- self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+ left_low=True, right_low=True, name=prefix+'VelocityDrivetrainLowLow', is_clutch=is_clutch)
+ self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name=prefix+'VelocityDrivetrainLowHigh', is_clutch=is_clutch)
+ self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = prefix+'VelocityDrivetrainHighLow', is_clutch=is_clutch)
+ self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = prefix+'VelocityDrivetrainHighHigh', is_clutch=is_clutch)
# X is [lvel, rvel]
self.X = numpy.matrix(
@@ -390,29 +392,41 @@
def main(argv):
- vdrivetrain = VelocityDrivetrain()
+ dog_vdrivetrain = VelocityDrivetrain(False)
+ clutch_vdrivetrain = VelocityDrivetrain(True)
- if len(argv) != 5:
+ if len(argv) != 7:
print "Expected .h file name and .cc file name"
else:
- loop_writer = control_loop.ControlLoopWriter(
- "VDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VDogDrivetrain", [dog_vdrivetrain.drivetrain_low_low,
+ dog_vdrivetrain.drivetrain_low_high,
+ dog_vdrivetrain.drivetrain_high_low,
+ dog_vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
+ dog_loop_writer.Write(argv[2], argv[1])
else:
- loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ clutch_loop_writer = control_loop.ControlLoopWriter(
+ "VClutchDrivetrain", [clutch_vdrivetrain.drivetrain_low_low,
+ clutch_vdrivetrain.drivetrain_low_high,
+ clutch_vdrivetrain.drivetrain_high_low,
+ clutch_vdrivetrain.drivetrain_high_high])
+
+ if argv[3][-3:] == '.cc':
+ clutch_loop_writer.Write(argv[4], argv[3])
+ else:
+ clutch_loop_writer.Write(argv[3], argv[4])
cim_writer = control_loop.ControlLoopWriter(
"CIM", [drivetrain.CIM()])
- if argv[3][-3:] == '.cc':
- cim_writer.Write(argv[4], argv[3])
+ if argv[5][-3:] == '.cc':
+ cim_writer.Write(argv[6], argv[5])
else:
- cim_writer.Write(argv[3], argv[4])
+ cim_writer.Write(argv[5], argv[6])
return
vl_plot = []