Break out the CIM class to avoid duplication
Before this patch we had 7 identical copies of the CIM class. Now it's
in its own module.
More de-duplication to come.
Change-Id: I598224020e50e8ec63584333dc080d34a2407cde
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 897738a..ede85c0 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -3,6 +3,7 @@
py_library(
name = 'controls',
srcs = [
+ 'cim.py',
'control_loop.py',
'controls.py',
'polytope.py',
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
new file mode 100644
index 0000000..fc39eb9
--- /dev/null
+++ b/frc971/control_loops/python/cim.py
@@ -0,0 +1,46 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+import numpy
+
+class CIM(control_loop.ControlLoop):
+ def __init__(self):
+ super(CIM, self).__init__("CIM")
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the CIM in kg m^2
+ self.J = 0.0001
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Control loop time step
+ self.dt = 0.005
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / self.Kv / (self.J * self.resistance)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.resistance)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([0.01])
+ self.PlaceObserverPoles([0.01])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
diff --git a/y2012/control_loops/python/drivetrain.py b/y2012/control_loops/python/drivetrain.py
index 291fe14..a57f211 100755
--- a/y2012/control_loops/python/drivetrain.py
+++ b/y2012/control_loops/python/drivetrain.py
@@ -14,48 +14,6 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
-
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
diff --git a/y2012/control_loops/python/polydrivetrain.py b/y2012/control_loops/python/polydrivetrain.py
index 9948ff2..c19c888 100755
--- a/y2012/control_loops/python/polydrivetrain.py
+++ b/y2012/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
from y2012.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
@@ -192,8 +193,8 @@
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
- self.left_cim = drivetrain.CIM()
- self.right_cim = drivetrain.CIM()
+ self.left_cim = CIM()
+ self.right_cim = CIM()
def IsInGear(self, gear):
return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -423,8 +424,7 @@
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(argv[3], argv[4])
return
@@ -478,7 +478,7 @@
cim_velocity_plot = []
cim_voltage_plot = []
cim_time = []
- cim = drivetrain.CIM()
+ cim = CIM()
R = numpy.matrix([[300]])
for t in numpy.arange(0, 0.5, cim.dt):
U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index 2a93285..7413158 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -14,48 +14,6 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
-
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index 93f3884..387fd78 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
from y2014.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
@@ -192,8 +193,8 @@
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
- self.left_cim = drivetrain.CIM()
- self.right_cim = drivetrain.CIM()
+ self.left_cim = CIM()
+ self.right_cim = CIM()
def IsInGear(self, gear):
return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -423,8 +424,7 @@
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(argv[3], argv[4])
return
@@ -478,7 +478,7 @@
cim_velocity_plot = []
cim_voltage_plot = []
cim_time = []
- cim = drivetrain.CIM()
+ cim = CIM()
R = numpy.matrix([[300]])
for t in numpy.arange(0, 0.5, cim.dt):
U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index 4c3577b..0dc2466 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -14,48 +14,6 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
-
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index e047c11..1d426d9 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
from y2014_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
@@ -196,8 +197,8 @@
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
- self.left_cim = drivetrain.CIM()
- self.right_cim = drivetrain.CIM()
+ self.left_cim = CIM()
+ self.right_cim = CIM()
def IsInGear(self, gear):
return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -428,8 +429,7 @@
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(argv[3], argv[4])
return
diff --git a/y2015/control_loops/python/drivetrain.py b/y2015/control_loops/python/drivetrain.py
index 75425ba..3f03226 100755
--- a/y2015/control_loops/python/drivetrain.py
+++ b/y2015/control_loops/python/drivetrain.py
@@ -13,48 +13,6 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
-
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 5320145..9a5b48b 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
from y2015.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
@@ -196,8 +197,8 @@
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
- self.left_cim = drivetrain.CIM()
- self.right_cim = drivetrain.CIM()
+ self.left_cim = CIM()
+ self.right_cim = CIM()
def IsInGear(self, gear):
return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -429,8 +430,7 @@
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(argv[3], argv[4])
return
@@ -484,7 +484,7 @@
cim_velocity_plot = []
cim_voltage_plot = []
cim_time = []
- cim = drivetrain.CIM()
+ cim = CIM()
R = numpy.matrix([[300]])
for t in numpy.arange(0, 0.5, cim.dt):
U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
diff --git a/y2015_bot3/control_loops/python/drivetrain.py b/y2015_bot3/control_loops/python/drivetrain.py
index a1df0ec..0adeede 100755
--- a/y2015_bot3/control_loops/python/drivetrain.py
+++ b/y2015_bot3/control_loops/python/drivetrain.py
@@ -8,48 +8,6 @@
import glog
-class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current
- # 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
- # Control loop time step
- self.dt = 0.005
-
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.R)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.R)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index 9aa4821..88caa73 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
import drivetrain
import control_loop
import controls
+from frc971.control_loops.python.cim import CIM
from matplotlib import pylab
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
@@ -183,8 +184,8 @@
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
- self.left_cim = drivetrain.CIM()
- self.right_cim = drivetrain.CIM()
+ self.left_cim = CIM()
+ self.right_cim = CIM()
def IsInGear(self, gear):
return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -413,8 +414,7 @@
else:
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()],
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()],
namespaces=['y2015_bot3', 'control_loops'])
if argv[5][-3:] == '.cc':
@@ -472,7 +472,7 @@
cim_velocity_plot = []
cim_voltage_plot = []
cim_time = []
- cim = drivetrain.CIM()
+ cim = CIM()
R = numpy.matrix([[300]])
for t in numpy.arange(0, 0.5, cim.dt):
U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index db8388b..46f0600 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -13,48 +13,6 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
-
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
diff --git a/y2016/control_loops/python/polydrivetrain.py b/y2016/control_loops/python/polydrivetrain.py
index 6b07224..62a4895 100755
--- a/y2016/control_loops/python/polydrivetrain.py
+++ b/y2016/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
from y2016.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
@@ -196,8 +197,8 @@
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
- self.left_cim = drivetrain.CIM()
- self.right_cim = drivetrain.CIM()
+ self.left_cim = CIM()
+ self.right_cim = CIM()
def IsInGear(self, gear):
return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -428,8 +429,7 @@
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(argv[3], argv[4])
return
diff --git a/y2016_bot3/control_loops/python/drivetrain.py b/y2016_bot3/control_loops/python/drivetrain.py
index 5de7dbc..738031c 100755
--- a/y2016_bot3/control_loops/python/drivetrain.py
+++ b/y2016_bot3/control_loops/python/drivetrain.py
@@ -14,48 +14,6 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
-
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
-
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
class Drivetrain(control_loop.ControlLoop):
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
diff --git a/y2016_bot3/control_loops/python/polydrivetrain.py b/y2016_bot3/control_loops/python/polydrivetrain.py
index 07f9ff9..aed8ebe 100755
--- a/y2016_bot3/control_loops/python/polydrivetrain.py
+++ b/y2016_bot3/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
from y2016_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
@@ -196,8 +197,8 @@
self.right_gear = VelocityDrivetrain.LOW
self.left_shifter_position = 0.0
self.right_shifter_position = 0.0
- self.left_cim = drivetrain.CIM()
- self.right_cim = drivetrain.CIM()
+ self.left_cim = CIM()
+ self.right_cim = CIM()
def IsInGear(self, gear):
return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -428,8 +429,7 @@
dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(argv[3], argv[4])
return