Updated constants in control loop python files.
Although the shooter should have the correct constants and all, there are currently stability issues.
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 3d6b9fc..e2e32e6 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -9,17 +9,16 @@
def __init__(self, name="RawClaw"):
super(Claw, self).__init__(name)
# Stall Torque in N m
- self.stall_torque = 1.4
+ self.stall_torque = 2.42
# Stall Current in Amps
- self.stall_current = 86
- # Free Speed in RPM
- self.free_speed = 6200.0
+ self.stall_current = 133
+ # Free Speed in RPM, pulled from drivetrain
+ self.free_speed = 4650.0
# Free Current in Amps
- self.free_current = 1.5
+ self.free_current = 2.7
# Moment of inertia of the claw in kg m^2
- # TODO(aschuh): Measure this in reality. It doesn't seem high enough.
- # James measured 0.51, but that can't be right given what I am seeing.
- self.J = 2.0
+ # approzimately 0.76 (without ball) in CAD
+ self.J = 0.76
# Resistance of the motor
self.R = 12.0 / self.stall_current + 0.024 + .003
# Motor velocity constant
@@ -28,7 +27,7 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
- self.G = 1.0 / ((84.0 / 20.0) * (50.0 / 14.0) * (40.0 / 14.0) * (40.0 / 12.0))
+ self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
# Control loop time step
self.dt = 0.01
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index fcca56a..001fd1e 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, is_clutch=False):
- super(Drivetrain, self).__init__(("Clutch" if is_clutch else "Dog" )+"Drivetrain")
+ def __init__(self, left_low=True, right_low=True):
+ super(Drivetrain, self).__init__("Drivetrain")
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
@@ -70,19 +70,15 @@
# Radius of the wheels, in meters.
self.r = .04445
# Resistance of the motor, divided by the number of motors.
- self.R = (12.0 / self.stall_current / 4 + 0.03) / (0.93 ** 2.0)
+ self.R = 12.0 / self.stall_current / 4
# 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 ratios
- 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
+ self.G_low = 18.0 / 60.0 * 18.0 / 50.0
+ self.G_high = 28.0 / 50.0 * 18.0 / 50.0
if left_low:
self.Gl = self.G_low
else:
@@ -204,23 +200,16 @@
#pylab.show()
# Write the generated constants out to a file.
- dog_drivetrain = Drivetrain(is_clutch=False)
- clutch_drivetrain = Drivetrain(is_clutch=True)
+ drivetrain = Drivetrain()
if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
- dog_loop_writer = control_loop.ControlLoopWriter("DogDrivetrain", [dog_drivetrain])
+ dog_loop_writer = control_loop.ControlLoopWriter("Drivetrain", [drivetrain])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
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 5ffcff4..280db16 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -96,11 +96,10 @@
class VelocityDrivetrainModel(control_loop.ControlLoop):
- def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel", is_clutch=False):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
- right_low=right_low,
- is_clutch=is_clutch)
+ right_low=right_low)
self.dt = 0.01
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
@@ -138,13 +137,12 @@
SHIFTING_UP = 'up'
SHIFTING_DOWN = 'down'
- def __init__(self, is_clutch):
- prefix = 'Clutch' if is_clutch else 'Dog'
+ def __init__(self):
self.drivetrain_low_low = VelocityDrivetrainModel(
- 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)
+ 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')
# X is [lvel, rvel]
self.X = numpy.matrix(
@@ -392,34 +390,22 @@
def main(argv):
- dog_vdrivetrain = VelocityDrivetrain(False)
- clutch_vdrivetrain = VelocityDrivetrain(True)
+ vdrivetrain = VelocityDrivetrain()
if len(argv) != 7:
print "Expected .h file name and .cc file name"
else:
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])
+ "VDogDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
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()])
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 0015e91..0d828d0 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -17,19 +17,19 @@
# Free Current in Amps
self.free_current = 1.2
# Moment of inertia of the shooter in kg m^2
- # Calculate Moment of Irtia
- self.J = 0.3
+ # Needs to be figured out in practice.
+ self.J = 5
# Resistance of the motor, divided by the number of motors.
self.R = 12.0 / self.stall_current / 2.0
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (13.5 - self.R * self.free_current))
+ (12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Spring constant for the springs, N/m
- self.Ks = 3600.0
- # Gear ratio
- self.G = 1.0 / ((84.0 / 20.0) * (50.0 / 14.0) * (40.0 / 14.0) * (40.0 / 12.0))
+ self.Ks = 2800.0
+ # Gear ratio multiplied by radius of final sprocket.
+ self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 0.0182
# Control loop time step
self.dt = 0.01
@@ -38,23 +38,26 @@
# TODO(james): Make this work with origins other than at kx = 0.
self.A_continuous = numpy.matrix(
[[0, 1],
- [-self.Ks * 0.01 / self.J,
+ [-self.Ks / self.J,
-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
self.B_continuous = numpy.matrix(
[[0],
[self.Kt / (self.J * self.G * self.R)]])
+ print "Continuous A, B:", self.A_continuous, self.B_continuous
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
+ print "Discrete A, B: ", self.A, self.B
+ print "Eigenvalues A: ", numpy.linalg.eig(self.A)[0]
- self.PlaceControllerPoles([0.85, 0.45])
+ self.PlaceControllerPoles([0.85, 0.85])
self.rpl = .05
self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ self.PlaceObserverPoles([self.rpl,
+ self.rpl])
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
@@ -110,11 +113,16 @@
# Simulate the response of the system to a step input.
shooter = Shooter()
simulated_x = []
- for _ in xrange(1000):
- shooter.Update(numpy.matrix([[2.0]]))
+ u = []
+ shooter.X[0, 0] = 1
+ for _ in xrange(2000):
+ U = shooter.X[1, 0] / shooter.G / shooter.Kv
+ shooter.Update(numpy.matrix([[U]]))
simulated_x.append(shooter.X[0, 0])
+ u.append(U / 10.0)
- pylab.plot(range(1000), simulated_x)
+ pylab.plot(range(2000), simulated_x)
+ pylab.plot(range(2000), u)
pylab.show()
# Simulate the response of the system to a goal.