Merge 'brian/devel' into claw
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 9e2bb74..315b402 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -10,48 +10,57 @@
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
+ self.stall_current = 133
# Free Speed in RPM
- self.free_speed = 6200.0
+ self.free_speed = 5500.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
+ # measured from CAD
+ self.J_top = 0.3
+ self.J_bottom = 0.9
# Resistance of the motor
- self.R = 12.0 / self.stall_current + 0.024 + .003
+ self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(13.5 - self.R * self.free_current))
# 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
# State is [bottom position, top - bottom position,
# bottom velocity, top - bottom velocity]
- # Input is [bottom power, top power]
- # Motor time constant.
- self.motor_timeconstant = self.Kt / self.Kv / (self.J * self.G * self.G * self.R)
+ # Input is [bottom power, top - bottom power]
+ # Motor time constants. difference_bottom refers to the constant for how the
+ # bottom velocity affects the difference of the top and bottom velocities.
+ self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
+ self.bottom_bottom = self.common_motor_constant / self.J_bottom
+ self.difference_bottom = self.common_motor_constant * (1 / self.J_bottom
+ - 1 / self.J_top)
+ self.difference_difference = self.common_motor_constant / self.J_top
# State feedback matrices
self.A_continuous = numpy.matrix(
[[0, 0, 1, 0],
[0, 0, 0, 1],
- [0, 0, -self.motor_timeconstant, 0],
- [0, 0, 0, -self.motor_timeconstant]])
+ [0, 0, self.bottom_bottom, 0],
+ [0, 0, self.difference_bottom, self.difference_difference]])
- self.motor_feedforward = self.Kt / (self.J * self.G * self.R)
-
+ self.motor_feedforward = self.Kt / (self.G * self.R)
+ self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
+ self.motor_feedforward_difference = self.motor_feedforward / self.J_top
+ self.motor_feedforward_difference_bottom = (
+ self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
self.B_continuous = numpy.matrix(
[[0, 0],
[0, 0],
- [self.motor_feedforward, 0],
- [0.0, self.motor_feedforward]])
+ [self.motor_feedforward_bottom, 0],
+ [self.motor_feedforward_difference_bottom,
+ self.motor_feedforward_difference]])
self.C = numpy.matrix([[1, 0, 0, 0],
[1, 1, 0, 0]])
self.D = numpy.matrix([[0, 0],
@@ -63,13 +72,13 @@
#controlability = controls.ctrb(self.A, self.B);
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
- self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.03 ** 2.0)), 0.0, 0.0],
+ self.Q = numpy.matrix([[(1.0 / (0.40 ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.007 ** 2.0)), 0.0, 0.0],
[0.0, 0.0, 0.2, 0.0],
[0.0, 0.0, 0.0, 2.00]])
- self.R = numpy.matrix([[(1.0 / (20.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (20.0 ** 2.0))]])
+ self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (5.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
print "K unaugmented"
@@ -82,6 +91,8 @@
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
+ # 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], [24.0]])
self.U_min = numpy.matrix([[-12.0], [-24.0]])
@@ -197,16 +208,18 @@
def AverageUFix(claw, U):
bottom_u = U[0, 0]
- top_u = U[1, 0] + bottom_u
+ top_u = bottom_u + U[1, 0]
+#top_u = claw.J_top * (bottom_u / claw.J_bottom - U[1, 0])
#print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
- if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[1, 0] or
- top_u < claw.U_min[1, 0] or bottom_u < claw.U_min[0, 0]):
+ if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[0, 0] or
+ top_u < claw.U_min[0, 0] or bottom_u < claw.U_min[0, 0]):
scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
top_u *= scalar
bottom_u *= scalar
return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+ #return numpy.matrix([[bottom_u], [bottom_u / claw.J_bottom - top_u / claw.J_top]])
def ClipDeltaU(claw, U):
delta_u = U[0, 0]
@@ -247,15 +260,17 @@
#pylab.plot(range(100), simulated_x)
#pylab.show()
- # Simulate the closed loop response of the system to a step input.
+ # Simulate the closed loop response of the system.
claw = Claw("TopClaw")
t = []
close_loop_x_bottom = []
close_loop_x_sep = []
+ close_loop_x_top = []
close_loop_u_bottom = []
close_loop_u_top = []
- R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.1], [0.05], [0.0], [0.0]])
claw.X[0, 0] = 0
+ claw.X[1, 0] = .05
for i in xrange(100):
#print "Error is", (R - claw.X_hat)
U = claw.K * (R - claw.X_hat)
@@ -268,12 +283,14 @@
claw.Update(U)
close_loop_x_bottom.append(claw.X[0, 0] * 10)
close_loop_u_bottom.append(U[0, 0])
- close_loop_x_sep.append(claw.X[1, 0] * 10)
+ close_loop_x_sep.append(claw.X[1, 0] * 100)
+ close_loop_x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10)
close_loop_u_top.append(U[1, 0] + U[0, 0])
t.append(0.01 * i)
pylab.plot(t, close_loop_x_bottom, label='x bottom')
pylab.plot(t, close_loop_x_sep, label='separation')
+ pylab.plot(t, close_loop_x_top, label='x top')
pylab.plot(t, close_loop_u_bottom, label='u bottom')
pylab.plot(t, close_loop_u_top, label='u top')
pylab.legend()
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 bb88523..ea3aab8 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -9,33 +9,39 @@
def __init__(self, name="RawShooter"):
super(Shooter, self).__init__(name)
# Stall Torque in N m
- self.stall_torque = .4862
+ self.stall_torque = .4982
# Stall Current in Amps
self.stall_current = 85
# Free Speed in RPM
self.free_speed = 19300.0
# Free Current in Amps
- self.free_current = 1.4
- # Moment of inertia of the shooter 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
- # Resistance of the motor
- self.R = 12.0 / self.stall_current + 0.024 + .003 #TODO comment on these constants
+ self.free_current = 1.2
+ # Effective mass of the shooter in kg.
+ # This rough estimate should about include the effect of the masses
+ # of the gears. If this number is too low, the eigen values of self.A
+ # will start to become extremely small.
+ self.J = 12
+ # 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
- # Gear ratio
- self.G = 1.0 / ((84.0 / 20.0) * (50.0 / 14.0) * (40.0 / 14.0) * (40.0 / 12.0))
+ # Spring constant for the springs, N/m
+ 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
+
# State feedback matrices
+ # TODO(james): Make this work with origins other than at kx = 0.
self.A_continuous = numpy.matrix(
[[0, 1],
- [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ [-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)]])
@@ -45,12 +51,12 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([0.85, 0.45])
+ self.PlaceControllerPoles([0.45, 0.45])
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]])
@@ -77,7 +83,7 @@
self.C = numpy.matrix([[1.0, 0.0, 0.0]])
self.D = numpy.matrix([[0.0]])
- self.PlaceControllerPoles([0.55, 0.35, 0.80])
+ self.PlaceControllerPoles([0.55, 0.45, 0.80])
print "K"
print self.K
@@ -104,31 +110,35 @@
def main(argv):
# Simulate the response of the system to a step input.
- shooter = ShooterDeltaU()
+ shooter = Shooter()
simulated_x = []
- for _ in xrange(100):
- shooter.Update(numpy.matrix([[12.0]]))
+ for _ in xrange(2000):
+ U = 2.0
+ shooter.Update(numpy.matrix([[U]]))
simulated_x.append(shooter.X[0, 0])
- pylab.plot(range(100), simulated_x)
+ pylab.plot(range(2000), simulated_x)
pylab.show()
- # Simulate the closed loop response of the system to a step input.
- shooter = ShooterDeltaU()
+ # Simulate the response of the system to a goal.
+ shooter = Shooter()
close_loop_x = []
close_loop_u = []
- R = numpy.matrix([[1.0], [0.0], [0.0]])
- shooter.X[2, 0] = -5
- for _ in xrange(100):
- U = numpy.clip(shooter.K * (R - shooter.X_hat), shooter.U_min, shooter.U_max)
- U = ClipDeltaU(shooter, U)
+ R = numpy.matrix([[0.3], [0.0]])
+ for _ in xrange(500):
+ augment = (-numpy.linalg.lstsq(shooter.B_continuous, numpy.identity(
+ shooter.B_continuous.shape[0]))[0] *
+ shooter.A_continuous * R)
+ U = numpy.clip(shooter.K * (R - shooter.X_hat) + augment,
+ shooter.U_min, shooter.U_max)
+#U = ClipDeltaU(shooter, U)
shooter.UpdateObserver(U)
shooter.Update(U)
close_loop_x.append(shooter.X[0, 0] * 10)
- close_loop_u.append(shooter.X[2, 0])
+ close_loop_u.append(U[0, 0])
- pylab.plot(range(100), close_loop_x)
- pylab.plot(range(100), close_loop_u)
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
pylab.show()
# Write the generated constants out to a file.