Added constants for motors and the such.
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 98d79ea..8ba75ba 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -10,17 +10,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
@@ -29,7 +28,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 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.