Reformat python and c++ files
Change-Id: I7d7d99a2094c2a9181ed882735b55159c14db3b0
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index a31390a..2b5a37e 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -9,6 +9,7 @@
class AngularSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -35,6 +36,7 @@
class AngularSystem(control_loop.ControlLoop):
+
def __init__(self, params, name="AngularSystem"):
super(AngularSystem, self).__init__(name)
self.params = params
@@ -45,15 +47,15 @@
self.G = params.G
# Moment of inertia in kg m^2
- self.J = params.J + self.motor.motor_inertia / (self.G ** 2.0)
+ self.J = params.J + self.motor.motor_inertia / (self.G**2.0)
# Control loop time step
self.dt = params.dt
# State is [position, velocity]
# Input is [Voltage]
- C1 = self.motor.Kt / (self.G * self.G * self.motor.resistance *
- self.J * self.motor.Kv)
+ C1 = self.motor.Kt / (
+ self.G * self.G * self.motor.resistance * self.J * self.motor.Kv)
C2 = self.motor.Kt / (self.G * self.J * self.motor.resistance)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -116,6 +118,7 @@
class IntegralAngularSystem(AngularSystem):
+
def __init__(self, params, name="IntegralAngularSystem"):
super(IntegralAngularSystem, self).__init__(params, name=name)
@@ -136,10 +139,9 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
- self.Q = numpy.matrix(
- [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
- [0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+ self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+ [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+ [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -197,8 +199,8 @@
vbat = 12.0
- goal = numpy.concatenate(
- (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
profile = TrapezoidProfile(plant.dt)
profile.set_maximum_acceleration(max_acceleration)
@@ -257,8 +259,7 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -374,7 +375,8 @@
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', angular_system.G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', angular_system.motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ angular_system.motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
integral_angular_system = IntegralAngularSystem(params,
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index fc39eb9..8c13eb0 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -3,44 +3,46 @@
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]])
+ 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
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
+ # 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.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceControllerPoles([0.01])
+ self.PlaceObserverPoles([0.01])
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e3e00b7..ac22821 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -2,548 +2,591 @@
import numpy
import os
-class Constant(object):
- def __init__ (self, name, formatt, value):
- self.name = name
- self.formatt = formatt
- self.value = value
- self.formatToType = {}
- self.formatToType['%f'] = "double"
- self.formatToType['%d'] = "int"
- def Render(self, loop_type):
- typestring = self.formatToType[self.formatt]
- if loop_type == 'float' and typestring == 'double':
- typestring = loop_type
- return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (typestring, self.name, self.value)
+class Constant(object):
+
+ def __init__(self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double"
+ self.formatToType['%d'] = "int"
+
+ def Render(self, loop_type):
+ typestring = self.formatToType[self.formatt]
+ if loop_type == 'float' and typestring == 'double':
+ typestring = loop_type
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (typestring, self.name, self.value)
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None,
- write_constants=False, plant_type='StateFeedbackPlant',
- observer_type='StateFeedbackObserver',
- scalar_type='double'):
- """Constructs a control loop writer.
- Args:
- gain_schedule_name: string, Name of the overall controller.
- loops: array[ControlLoop], a list of control loops to gain schedule
- in order.
- namespaces: array[string], a list of names of namespaces to nest in
- order. If None, the default will be used.
- plant_type: string, The C++ type of the plant.
- observer_type: string, The C++ type of the observer.
- scalar_type: string, The C++ type of the base scalar.
- """
- self._gain_schedule_name = gain_schedule_name
- self._loops = loops
- if namespaces:
- self._namespaces = namespaces
- else:
- self._namespaces = ['frc971', 'control_loops']
+ def __init__(self,
+ gain_schedule_name,
+ loops,
+ namespaces=None,
+ write_constants=False,
+ plant_type='StateFeedbackPlant',
+ observer_type='StateFeedbackObserver',
+ scalar_type='double'):
+ """Constructs a control loop writer.
- self._namespace_start = '\n'.join(
- ['namespace %s {' % name for name in self._namespaces])
+ Args:
+ gain_schedule_name: string, Name of the overall controller.
+ loops: array[ControlLoop], a list of control loops to gain schedule
+ in order.
+ namespaces: array[string], a list of names of namespaces to nest in
+ order. If None, the default will be used.
+ plant_type: string, The C++ type of the plant.
+ observer_type: string, The C++ type of the observer.
+ scalar_type: string, The C++ type of the base scalar.
+ """
+ self._gain_schedule_name = gain_schedule_name
+ self._loops = loops
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'control_loops']
- self._namespace_end = '\n'.join(
- ['} // namespace %s' % name for name in reversed(self._namespaces)])
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
- self._constant_list = []
- self._plant_type = plant_type
- self._observer_type = observer_type
- self._scalar_type = scalar_type
+ self._namespace_end = '\n'.join([
+ '} // namespace %s' % name for name in reversed(self._namespaces)
+ ])
- def AddConstant(self, constant):
- """Adds a constant to write.
+ self._constant_list = []
+ self._plant_type = plant_type
+ self._observer_type = observer_type
+ self._scalar_type = scalar_type
- Args:
- constant: Constant, the constant to add to the header.
- """
- self._constant_list.append(constant)
+ def AddConstant(self, constant):
+ """Adds a constant to write.
- def _TopDirectory(self):
- return self._namespaces[0]
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
- def _HeaderGuard(self, header_file):
- return ('_'.join([namespace.upper() for namespace in self._namespaces]) + '_' +
- os.path.basename(header_file).upper()
- .replace('.', '_').replace('/', '_') + '_')
+ def _TopDirectory(self):
+ return self._namespaces[0]
- def Write(self, header_file, cc_file):
- """Writes the loops to the specified files."""
- self.WriteHeader(header_file)
- self.WriteCC(os.path.basename(header_file), cc_file)
+ def _HeaderGuard(self, header_file):
+ return ('_'.join([namespace.upper() for namespace in self._namespaces])
+ + '_' + os.path.basename(header_file).upper().replace(
+ '.', '_').replace('/', '_') + '_')
- def _GenericType(self, typename, extra_args=None):
- """Returns a loop template using typename for the type."""
- num_states = self._loops[0].A.shape[0]
- num_inputs = self._loops[0].B.shape[1]
- num_outputs = self._loops[0].C.shape[0]
- if extra_args is not None:
- extra_args = ', ' + extra_args
- else:
- extra_args = ''
- if self._scalar_type != 'double':
- extra_args += ', ' + self._scalar_type
- return '%s<%d, %d, %d%s>' % (
- typename, num_states, num_inputs, num_outputs, extra_args)
+ def Write(self, header_file, cc_file):
+ """Writes the loops to the specified files."""
+ self.WriteHeader(header_file)
+ self.WriteCC(os.path.basename(header_file), cc_file)
- def _ControllerType(self):
- """Returns a template name for StateFeedbackController."""
- return self._GenericType('StateFeedbackController')
+ def _GenericType(self, typename, extra_args=None):
+ """Returns a loop template using typename for the type."""
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+ if extra_args is not None:
+ extra_args = ', ' + extra_args
+ else:
+ extra_args = ''
+ if self._scalar_type != 'double':
+ extra_args += ', ' + self._scalar_type
+ return '%s<%d, %d, %d%s>' % (typename, num_states, num_inputs,
+ num_outputs, extra_args)
- def _ObserverType(self):
- """Returns a template name for StateFeedbackObserver."""
- return self._GenericType(self._observer_type)
+ def _ControllerType(self):
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
- def _LoopType(self):
- """Returns a template name for StateFeedbackLoop."""
- num_states = self._loops[0].A.shape[0]
- num_inputs = self._loops[0].B.shape[1]
- num_outputs = self._loops[0].C.shape[0]
+ def _ObserverType(self):
+ """Returns a template name for StateFeedbackObserver."""
+ return self._GenericType(self._observer_type)
- return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
- num_states,
- num_inputs,
- num_outputs, self._scalar_type,
- self._PlantType(), self._ObserverType())
+ def _LoopType(self):
+ """Returns a template name for StateFeedbackLoop."""
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+ return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
+ num_states, num_inputs, num_outputs, self._scalar_type,
+ self._PlantType(), self._ObserverType())
- def _PlantType(self):
- """Returns a template name for StateFeedbackPlant."""
- return self._GenericType(self._plant_type)
+ def _PlantType(self):
+ """Returns a template name for StateFeedbackPlant."""
+ return self._GenericType(self._plant_type)
- def _PlantCoeffType(self):
- """Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType(self._plant_type + 'Coefficients')
+ def _PlantCoeffType(self):
+ """Returns a template name for StateFeedbackPlantCoefficients."""
+ return self._GenericType(self._plant_type + 'Coefficients')
- def _ControllerCoeffType(self):
- """Returns a template name for StateFeedbackControllerCoefficients."""
- return self._GenericType('StateFeedbackControllerCoefficients')
+ def _ControllerCoeffType(self):
+ """Returns a template name for StateFeedbackControllerCoefficients."""
+ return self._GenericType('StateFeedbackControllerCoefficients')
- def _ObserverCoeffType(self):
- """Returns a template name for StateFeedbackObserverCoefficients."""
- return self._GenericType(self._observer_type + 'Coefficients')
+ def _ObserverCoeffType(self):
+ """Returns a template name for StateFeedbackObserverCoefficients."""
+ return self._GenericType(self._observer_type + 'Coefficients')
- def WriteHeader(self, header_file):
- """Writes the header file to the file named header_file."""
- with open(header_file, 'w') as fd:
- header_guard = self._HeaderGuard(header_file)
- fd.write('#ifndef %s\n'
- '#define %s\n\n' % (header_guard, header_guard))
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- if (self._plant_type == 'StateFeedbackHybridPlant' or
- self._observer_type == 'HybridKalman'):
- fd.write('#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n')
+ def WriteHeader(self, header_file):
+ """Writes the header file to the file named header_file."""
+ with open(header_file, 'w') as fd:
+ header_guard = self._HeaderGuard(header_file)
+ fd.write('#ifndef %s\n'
+ '#define %s\n\n' % (header_guard, header_guard))
+ fd.write(
+ '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ if (self._plant_type == 'StateFeedbackHybridPlant' or
+ self._observer_type == 'HybridKalman'):
+ fd.write(
+ '#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n'
+ )
- fd.write('\n')
+ fd.write('\n')
- fd.write(self._namespace_start)
+ fd.write(self._namespace_start)
- for const in self._constant_list:
- fd.write(const.Render(self._scalar_type))
+ for const in self._constant_list:
+ fd.write(const.Render(self._scalar_type))
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
- fd.write('\n')
- fd.write(loop.DumpControllerHeader(self._scalar_type))
- fd.write('\n')
- fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
- fd.write('\n')
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
+ fd.write('\n')
+ fd.write(loop.DumpControllerHeader(self._scalar_type))
+ fd.write('\n')
+ fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
+ fd.write('\n')
- fd.write('%s Make%sPlant();\n\n' %
- (self._PlantType(), self._gain_schedule_name))
+ fd.write('%s Make%sPlant();\n\n' % (self._PlantType(),
+ self._gain_schedule_name))
- fd.write('%s Make%sController();\n\n' %
- (self._ControllerType(), self._gain_schedule_name))
+ fd.write('%s Make%sController();\n\n' % (self._ControllerType(),
+ self._gain_schedule_name))
- fd.write('%s Make%sObserver();\n\n' %
- (self._ObserverType(), self._gain_schedule_name))
+ fd.write('%s Make%sObserver();\n\n' % (self._ObserverType(),
+ self._gain_schedule_name))
- fd.write('%s Make%sLoop();\n\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write('%s Make%sLoop();\n\n' % (self._LoopType(),
+ self._gain_schedule_name))
- fd.write(self._namespace_end)
- fd.write('\n\n')
- fd.write("#endif // %s\n" % header_guard)
+ fd.write(self._namespace_end)
+ fd.write('\n\n')
+ fd.write("#endif // %s\n" % header_guard)
- def WriteCC(self, header_file_name, cc_file):
- """Writes the cc file to the file named cc_file."""
- with open(cc_file, 'w') as fd:
- fd.write('#include \"%s/%s\"\n' %
- (os.path.join(*self._namespaces), header_file_name))
- fd.write('\n')
- fd.write('#include <vector>\n')
- fd.write('\n')
- fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
- fd.write('\n')
- fd.write(self._namespace_start)
- fd.write('\n\n')
- for loop in self._loops:
- fd.write(loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
- fd.write('\n')
+ def WriteCC(self, header_file_name, cc_file):
+ """Writes the cc file to the file named cc_file."""
+ with open(cc_file, 'w') as fd:
+ fd.write('#include \"%s/%s\"\n' % (os.path.join(*self._namespaces),
+ header_file_name))
+ fd.write('\n')
+ fd.write('#include <vector>\n')
+ fd.write('\n')
+ fd.write(
+ '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+ fd.write(self._namespace_start)
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(
+ loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
+ fd.write('\n')
- for loop in self._loops:
- fd.write(loop.DumpController(self._scalar_type))
- fd.write('\n')
+ for loop in self._loops:
+ fd.write(loop.DumpController(self._scalar_type))
+ fd.write('\n')
- for loop in self._loops:
- fd.write(loop.DumpObserver(self._ObserverCoeffType(), self._scalar_type))
- fd.write('\n')
+ for loop in self._loops:
+ fd.write(
+ loop.DumpObserver(self._ObserverCoeffType(),
+ self._scalar_type))
+ fd.write('\n')
- fd.write('%s Make%sPlant() {\n' %
- (self._PlantType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._PlantCoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._PlantCoeffType(), self._PlantCoeffType(),
- loop.PlantFunction()))
- fd.write(' return %s(&plants);\n' % self._PlantType())
- fd.write('}\n\n')
+ fd.write('%s Make%sPlant() {\n' % (self._PlantType(),
+ self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' %
+ (self._PlantCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
+ loop.PlantFunction()))
+ fd.write(' return %s(&plants);\n' % self._PlantType())
+ fd.write('}\n\n')
- fd.write('%s Make%sController() {\n' %
- (self._ControllerType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerCoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
- loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._ControllerType())
- fd.write('}\n\n')
+ fd.write('%s Make%sController() {\n' % (self._ControllerType(),
+ self._gain_schedule_name))
+ fd.write(
+ ' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' %
+ (self._ControllerCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(
+ ' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ControllerCoeffType(),
+ self._ControllerCoeffType(), loop.ControllerFunction()))
+ fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write('}\n\n')
- fd.write('%s Make%sObserver() {\n' %
- (self._ObserverType(), self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
- self._ObserverCoeffType(), len(self._loops)))
- for index, loop in enumerate(self._loops):
- fd.write(' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ObserverCoeffType(), self._ObserverCoeffType(),
- loop.ObserverFunction()))
- fd.write(' return %s(&observers);\n' % self._ObserverType())
- fd.write('}\n\n')
+ fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
+ self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n'
+ % (self._ObserverCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(
+ ' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
+ % (index, self._ObserverCoeffType(),
+ self._ObserverCoeffType(), loop.ObserverFunction()))
+ fd.write(' return %s(&observers);\n' % self._ObserverType())
+ fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
- fd.write(' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
- (self._LoopType(), self._gain_schedule_name,
- self._gain_schedule_name, self._gain_schedule_name))
- fd.write('}\n\n')
+ fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
+ self._gain_schedule_name))
+ fd.write(
+ ' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n'
+ % (self._LoopType(), self._gain_schedule_name,
+ self._gain_schedule_name, self._gain_schedule_name))
+ fd.write('}\n\n')
- fd.write(self._namespace_end)
- fd.write('\n')
+ fd.write(self._namespace_end)
+ fd.write('\n')
class ControlLoop(object):
- def __init__(self, name):
- """Constructs a control loop object.
- Args:
- name: string, The name of the loop to use when writing the C++ files.
- """
- self._name = name
+ def __init__(self, name):
+ """Constructs a control loop object.
- @property
- def name(self):
- """Returns the name"""
- return self._name
+ Args:
+ name: string, The name of the loop to use when writing the C++ files.
+ """
+ self._name = name
- def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
- """Calculates the discrete time values for A and B.
+ @property
+ def name(self):
+ """Returns the name"""
+ return self._name
- Args:
- A_continuous: numpy.matrix, The continuous time A matrix
- B_continuous: numpy.matrix, The continuous time B matrix
- dt: float, The time step of the control loop
+ def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+ """Calculates the discrete time values for A and B.
- Returns:
- (A, B), numpy.matrix, the control matricies.
- """
- return controls.c2d(A_continuous, B_continuous, dt)
+ Args:
+ A_continuous: numpy.matrix, The continuous time A matrix
+ B_continuous: numpy.matrix, The continuous time B matrix
+ dt: float, The time step of the control loop
- def InitializeState(self):
- """Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
- self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
+ Returns:
+ (A, B), numpy.matrix, the control matricies.
+ """
+ return controls.c2d(A_continuous, B_continuous, dt)
- def PlaceControllerPoles(self, poles):
- """Places the controller poles.
+ def InitializeState(self):
+ """Sets X, Y, and X_hat to zero defaults."""
+ self.X = numpy.zeros((self.A.shape[0], 1))
+ self.Y = self.C * self.X
+ self.X_hat = numpy.zeros((self.A.shape[0], 1))
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.K = controls.dplace(self.A, self.B, poles)
+ def PlaceControllerPoles(self, poles):
+ """Places the controller poles.
- def PlaceObserverPoles(self, poles):
- """Places the observer poles.
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.K = controls.dplace(self.A, self.B, poles)
- Args:
- poles: array, An array of poles. Must be complex conjegates if they have
- any imaginary portions.
- """
- self.L = controls.dplace(self.A.T, self.C.T, poles).T
+ def PlaceObserverPoles(self, poles):
+ """Places the observer poles.
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.L = controls.dplace(self.A.T, self.C.T, poles).T
- def Update(self, U):
- """Simulates one time step with the provided U."""
- #U = numpy.clip(U, self.U_min, self.U_max)
- self.X = self.A * self.X + self.B * U
- self.Y = self.C * self.X + self.D * U
+ def Update(self, U):
+ """Simulates one time step with the provided U."""
+ #U = numpy.clip(U, self.U_min, self.U_max)
+ self.X = self.A * self.X + self.B * U
+ self.Y = self.C * self.X + self.D * U
- def PredictObserver(self, U):
- """Runs the predict step of the observer update."""
- self.X_hat = (self.A * self.X_hat + self.B * U)
+ def PredictObserver(self, U):
+ """Runs the predict step of the observer update."""
+ self.X_hat = (self.A * self.X_hat + self.B * U)
- def CorrectObserver(self, U):
- """Runs the correct step of the observer update."""
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
- else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
+ def CorrectObserver(self, U):
+ """Runs the correct step of the observer update."""
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
- def UpdateObserver(self, U):
- """Updates the observer given the provided U."""
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
- else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat = (self.A * self.X_hat + self.B * U +
- self.A * KalmanGain * (self.Y - self.C * self.X_hat - self.D * U))
+ def UpdateObserver(self, U):
+ """Updates the observer given the provided U."""
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ self.X_hat = (self.A * self.X_hat + self.B * U + self.A * KalmanGain *
+ (self.Y - self.C * self.X_hat - self.D * U))
- def _DumpMatrix(self, matrix_name, matrix, scalar_type):
- """Dumps the provided matrix into a variable called matrix_name.
+ def _DumpMatrix(self, matrix_name, matrix, scalar_type):
+ """Dumps the provided matrix into a variable called matrix_name.
- Args:
- matrix_name: string, The variable name to save the matrix to.
- matrix: The matrix to dump.
- scalar_type: The C++ type to use for the scalar in the matrix.
+ Args:
+ matrix_name: string, The variable name to save the matrix to.
+ matrix: The matrix to dump.
+ scalar_type: The C++ type to use for the scalar in the matrix.
- Returns:
- string, The C++ commands required to populate a variable named matrix_name
- with the contents of matrix.
- """
- ans = [' Eigen::Matrix<%s, %d, %d> %s;\n' % (
- scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)]
- for x in xrange(matrix.shape[0]):
- for y in xrange(matrix.shape[1]):
- write_type = repr(matrix[x, y])
- if scalar_type == 'float':
- if '.' not in write_type:
- write_type += '.0'
- write_type += 'f'
- ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
+ Returns:
+ string, The C++ commands required to populate a variable named matrix_name
+ with the contents of matrix.
+ """
+ ans = [
+ ' Eigen::Matrix<%s, %d, %d> %s;\n' % (scalar_type, matrix.shape[0],
+ matrix.shape[1], matrix_name)
+ ]
+ for x in xrange(matrix.shape[0]):
+ for y in xrange(matrix.shape[1]):
+ write_type = repr(matrix[x, y])
+ if scalar_type == 'float':
+ if '.' not in write_type:
+ write_type += '.0'
+ write_type += 'f'
+ ans.append(
+ ' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
- return ''.join(ans)
+ return ''.join(ans)
- def DumpPlantHeader(self, plant_coefficient_type):
- """Writes out a c++ header declaration which will create a Plant object.
+ def DumpPlantHeader(self, plant_coefficient_type):
+ """Writes out a c++ header declaration which will create a Plant object.
- Returns:
- string, The header declaration for the function.
- """
- return '%s Make%sPlantCoefficients();\n' % (
- plant_coefficient_type, self._name)
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s Make%sPlantCoefficients();\n' % (plant_coefficient_type,
+ self._name)
- def DumpPlant(self, plant_coefficient_type, scalar_type):
- """Writes out a c++ function which will create a PlantCoefficients object.
+ def DumpPlant(self, plant_coefficient_type, scalar_type):
+ """Writes out a c++ function which will create a PlantCoefficients object.
- Returns:
- string, The function which will create the object.
- """
- ans = ['%s Make%sPlantCoefficients() {\n' % (
- plant_coefficient_type, self._name)]
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = [
+ '%s Make%sPlantCoefficients() {\n' % (plant_coefficient_type,
+ self._name)
+ ]
- ans.append(self._DumpMatrix('C', self.C, scalar_type))
- ans.append(self._DumpMatrix('D', self.D, scalar_type))
- ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
- ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
+ ans.append(self._DumpMatrix('C', self.C, scalar_type))
+ ans.append(self._DumpMatrix('D', self.D, scalar_type))
+ ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
+ ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
- if plant_coefficient_type.startswith('StateFeedbackPlant'):
- ans.append(self._DumpMatrix('A', self.A, scalar_type))
- ans.append(self._DumpMatrix('B', self.B, scalar_type))
- ans.append(' return %s'
- '(A, B, C, D, U_max, U_min);\n' % (
- plant_coefficient_type))
- elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous, scalar_type))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous, scalar_type))
- ans.append(' return %s'
- '(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
- plant_coefficient_type))
- else:
- glog.fatal('Unsupported plant type %s', plant_coefficient_type)
+ if plant_coefficient_type.startswith('StateFeedbackPlant'):
+ ans.append(self._DumpMatrix('A', self.A, scalar_type))
+ ans.append(self._DumpMatrix('B', self.B, scalar_type))
+ ans.append(
+ ' return %s'
+ '(A, B, C, D, U_max, U_min);\n' % (plant_coefficient_type))
+ elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
+ ans.append(
+ self._DumpMatrix('A_continuous', self.A_continuous,
+ scalar_type))
+ ans.append(
+ self._DumpMatrix('B_continuous', self.B_continuous,
+ scalar_type))
+ ans.append(' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min);\n' %
+ (plant_coefficient_type))
+ else:
+ glog.fatal('Unsupported plant type %s', plant_coefficient_type)
- ans.append('}\n')
- return ''.join(ans)
+ ans.append('}\n')
+ return ''.join(ans)
- def PlantFunction(self):
- """Returns the name of the plant coefficient function."""
- return 'Make%sPlantCoefficients()' % self._name
+ def PlantFunction(self):
+ """Returns the name of the plant coefficient function."""
+ return 'Make%sPlantCoefficients()' % self._name
- def ControllerFunction(self):
- """Returns the name of the controller function."""
- return 'Make%sControllerCoefficients()' % self._name
+ def ControllerFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sControllerCoefficients()' % self._name
- def ObserverFunction(self):
- """Returns the name of the controller function."""
- return 'Make%sObserverCoefficients()' % self._name
+ def ObserverFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sObserverCoefficients()' % self._name
- def DumpControllerHeader(self, scalar_type):
- """Writes out a c++ header declaration which will create a Controller object.
+ def DumpControllerHeader(self, scalar_type):
+ """Writes out a c++ header declaration which will create a Controller object.
- Returns:
- string, The header declaration for the function.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
- num_states, num_inputs, num_outputs, scalar_type,
- self.ControllerFunction())
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
+ num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())
- def DumpController(self, scalar_type):
- """Returns a c++ function which will create a Controller object.
+ def DumpController(self, scalar_type):
+ """Returns a c++ function which will create a Controller object.
- Returns:
- string, The function which will create the object.
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' % (
- num_states, num_inputs, num_outputs, scalar_type,
- self.ControllerFunction())]
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = [
+ 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' %
+ (num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())
+ ]
- ans.append(self._DumpMatrix('K', self.K, scalar_type))
- if not hasattr(self, 'Kff'):
- self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
+ ans.append(self._DumpMatrix('K', self.K, scalar_type))
+ if not hasattr(self, 'Kff'):
+ self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
- ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
+ ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
- ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
- '(K, Kff);\n' % (
- num_states, num_inputs, num_outputs, scalar_type))
- ans.append('}\n')
- return ''.join(ans)
+ ans.append(
+ ' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
+ '(K, Kff);\n' % (num_states, num_inputs, num_outputs, scalar_type))
+ ans.append('}\n')
+ return ''.join(ans)
- def DumpObserverHeader(self, observer_coefficient_type):
- """Writes out a c++ header declaration which will create a Observer object.
+ def DumpObserverHeader(self, observer_coefficient_type):
+ """Writes out a c++ header declaration which will create a Observer object.
- Returns:
- string, The header declaration for the function.
- """
- return '%s %s;\n' % (
- observer_coefficient_type, self.ObserverFunction())
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s %s;\n' % (observer_coefficient_type, self.ObserverFunction())
- def DumpObserver(self, observer_coefficient_type, scalar_type):
- """Returns a c++ function which will create a Observer object.
+ def DumpObserver(self, observer_coefficient_type, scalar_type):
+ """Returns a c++ function which will create a Observer object.
- Returns:
- string, The function which will create the object.
- """
- ans = ['%s %s {\n' % (
- observer_coefficient_type, self.ObserverFunction())]
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = [
+ '%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
+ ]
- if observer_coefficient_type.startswith('StateFeedbackObserver'):
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
- Q = self.Q
- R = self.R
- else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- Q = numpy.zeros(self.A.shape)
- R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
- ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
- ans.append(self._DumpMatrix('Q', Q, scalar_type))
- ans.append(self._DumpMatrix('R', R, scalar_type))
- ans.append(' return %s(KalmanGain, Q, R);\n'
- % (observer_coefficient_type,))
+ if observer_coefficient_type.startswith('StateFeedbackObserver'):
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ Q = self.Q
+ R = self.R
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ Q = numpy.zeros(self.A.shape)
+ R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
+ ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
+ ans.append(self._DumpMatrix('Q', Q, scalar_type))
+ ans.append(self._DumpMatrix('R', R, scalar_type))
+ ans.append(' return %s(KalmanGain, Q, R);\n' %
+ (observer_coefficient_type,))
- elif observer_coefficient_type.startswith('HybridKalman'):
- ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
- ans.append(self._DumpMatrix('R_continuous', self.R_continuous, scalar_type))
- ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state, scalar_type))
- ans.append(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
- observer_coefficient_type,))
- else:
- glog.fatal('Unsupported observer type %s', observer_coefficient_type)
+ elif observer_coefficient_type.startswith('HybridKalman'):
+ ans.append(
+ self._DumpMatrix('Q_continuous', self.Q_continuous,
+ scalar_type))
+ ans.append(
+ self._DumpMatrix('R_continuous', self.R_continuous,
+ scalar_type))
+ ans.append(
+ self._DumpMatrix('P_steady_state', self.P_steady_state,
+ scalar_type))
+ ans.append(
+ ' return %s(Q_continuous, R_continuous, P_steady_state);\n' %
+ (observer_coefficient_type,))
+ else:
+ glog.fatal('Unsupported observer type %s',
+ observer_coefficient_type)
- ans.append('}\n')
- return ''.join(ans)
+ ans.append('}\n')
+ return ''.join(ans)
+
class HybridControlLoop(ControlLoop):
- def __init__(self, name):
- super(HybridControlLoop, self).__init__(name=name)
- def Discretize(self, dt):
- [self.A, self.B, self.Q, self.R] = \
- controls.kalmd(self.A_continuous, self.B_continuous,
- self.Q_continuous, self.R_continuous, dt)
+ def __init__(self, name):
+ super(HybridControlLoop, self).__init__(name=name)
- def PredictHybridObserver(self, U, dt):
- self.Discretize(dt)
- self.X_hat = self.A * self.X_hat + self.B * U
- self.P = (self.A * self.P * self.A.T + self.Q)
+ def Discretize(self, dt):
+ [self.A, self.B, self.Q, self.R] = \
+ controls.kalmd(self.A_continuous, self.B_continuous,
+ self.Q_continuous, self.R_continuous, dt)
- def CorrectHybridObserver(self, U):
- Y_bar = self.Y - self.C * self.X_hat
- C_t = self.C.T
- S = self.C * self.P * C_t + self.R
- self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
- self.X_hat = self.X_hat + self.KalmanGain * Y_bar
- self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+ def PredictHybridObserver(self, U, dt):
+ self.Discretize(dt)
+ self.X_hat = self.A * self.X_hat + self.B * U
+ self.P = (self.A * self.P * self.A.T + self.Q)
- def InitializeState(self):
- super(HybridControlLoop, self).InitializeState()
- if hasattr(self, 'Q_steady_state'):
- self.P = self.Q_steady_state
- else:
- self.P = numpy.matrix(numpy.zeros((self.A.shape[0], self.A.shape[0])))
+ def CorrectHybridObserver(self, U):
+ Y_bar = self.Y - self.C * self.X_hat
+ C_t = self.C.T
+ S = self.C * self.P * C_t + self.R
+ self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
+ self.X_hat = self.X_hat + self.KalmanGain * Y_bar
+ self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+
+ def InitializeState(self):
+ super(HybridControlLoop, self).InitializeState()
+ if hasattr(self, 'Q_steady_state'):
+ self.P = self.Q_steady_state
+ else:
+ self.P = numpy.matrix(
+ numpy.zeros((self.A.shape[0], self.A.shape[0])))
class CIM(object):
- def __init__(self):
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133.0
- # Free Speed in rad/s
- self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi
- # Free Current in Amps
- self.free_current = 4.7
- # Resistance of the motor
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
+
+ def __init__(self):
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133.0
+ # Free Speed in rad/s
+ self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in Amps
+ self.free_current = 4.7
+ # Resistance of the motor
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
class MiniCIM(object):
- def __init__(self):
- # Stall Torque in N m
- self.stall_torque = 1.41
- # Stall Current in Amps
- self.stall_current = 89.0
- # Free Speed in rad/s
- self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi
- # Free Current in Amps
- self.free_current = 3.0
- # Resistance of the motor
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
+
+ def __init__(self):
+ # Stall Torque in N m
+ self.stall_torque = 1.41
+ # Stall Current in Amps
+ self.stall_current = 89.0
+ # Free Speed in rad/s
+ self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in Amps
+ self.free_current = 3.0
+ # Resistance of the motor
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
class NMotor(object):
+
def __init__(self, motor, n):
"""Gangs together n motors."""
self.motor = motor
@@ -558,6 +601,7 @@
class Vex775Pro(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 0.71
@@ -570,7 +614,8 @@
# Resistance of the motor
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Motor inertia in kg m^2
@@ -578,37 +623,40 @@
class BAG(object):
- # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor
- def __init__(self):
- # Stall Torque in (N m)
- self.stall_torque = 0.43
- # Stall Current in (Amps)
- self.stall_current = 53.0
- # Free Speed in (rad/s)
- self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi
- # Free Current in (Amps)
- self.free_current = 1.8
- # Resistance of the motor (Ohms)
- self.resistance = 12.0 / self.stall_current
- # Motor velocity constant (radians / (sec * volt))
- self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
- # Torque constant (N * m / A)
- self.Kt = self.stall_torque / self.stall_current
- # Motor inertia in kg m^2
- self.motor_inertia = 0.000006
+ # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor
+ def __init__(self):
+ # Stall Torque in (N m)
+ self.stall_torque = 0.43
+ # Stall Current in (Amps)
+ self.stall_current = 53.0
+ # Free Speed in (rad/s)
+ self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in (Amps)
+ self.free_current = 1.8
+ # Resistance of the motor (Ohms)
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant (radians / (sec * volt))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
+ # Torque constant (N * m / A)
+ self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.000006
+
class MN3510(object):
- def __init__(self):
- # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
- # Free Current in Amps
- self.free_current = 0.0
- # Resistance of the motor
- self.resistance = 0.188
- # Stall Current in Amps
- self.stall_current = 14.0 / self.resistance
- # Motor velocity constant
- self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
- # Torque constant Nm / A
- self.Kt = 1.0 / self.Kv
- # Stall Torque in N m
- self.stall_torque = self.Kt * self.stall_current
+
+ def __init__(self):
+ # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
+ # Free Current in Amps
+ self.free_current = 0.0
+ # Resistance of the motor
+ self.resistance = 0.188
+ # Stall Current in Amps
+ self.stall_current = 14.0 / self.resistance
+ # Motor velocity constant
+ self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
+ # Torque constant Nm / A
+ self.Kt = 1.0 / self.Kv
+ # Stall Torque in N m
+ self.stall_torque = self.Kt * self.stall_current
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 6db8f34..8efcff6 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -16,106 +16,108 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class DownEstimator(control_loop.ControlLoop):
- def __init__(self, name='DownEstimator'):
- super(DownEstimator, self).__init__(name)
- self.dt = 0.005
- # State is [gyro_angle, bias].
- # U is [gyro_x_velocity].
+ def __init__(self, name='DownEstimator'):
+ super(DownEstimator, self).__init__(name)
+ self.dt = 0.005
- self.A_continuous = numpy.matrix([[0, 1],
- [0, 0]])
+ # State is [gyro_angle, bias].
+ # U is [gyro_x_velocity].
- self.B_continuous = numpy.matrix([[1],
- [0]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, 0]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.B_continuous = numpy.matrix([[1], [0]])
- q_gyro_noise = 1e-6
- q_gyro_bias_noise = 1e-3
- self.Q = numpy.matrix([[1.0 / (q_gyro_noise ** 2.0), 0.0],
- [0.0, 1.0 / (q_gyro_bias_noise ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_accelerometer_angle_noise = 5e+7
- self.R = numpy.matrix([[(r_accelerometer_angle_noise ** 2.0)]])
+ q_gyro_noise = 1e-6
+ q_gyro_bias_noise = 1e-3
+ self.Q = numpy.matrix([[1.0 / (q_gyro_noise**2.0), 0.0],
+ [0.0, 1.0 / (q_gyro_bias_noise**2.0)]])
- self.C = numpy.matrix([[1.0, 0.0]])
- self.D = numpy.matrix([[0]])
+ r_accelerometer_angle_noise = 5e+7
+ self.R = numpy.matrix([[(r_accelerometer_angle_noise**2.0)]])
- self.U_max = numpy.matrix([[numpy.pi]])
- self.U_min = numpy.matrix([[-numpy.pi]])
- self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.U_max = numpy.matrix([[numpy.pi]])
+ self.U_min = numpy.matrix([[-numpy.pi]])
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.L = self.A * self.KalmanGain
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.InitializeState()
+ self.L = self.A * self.KalmanGain
- def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
- acceleration = math.sqrt(
- accelerometer_x**2 + accelerometer_y**2 + accelerometer_z**2)
- if acceleration < 0.9 or acceleration > 1.1:
- glog.error('bad total acceleration %f' % acceleration)
- # TODO(Brian): Tune this?
- return
- accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
- Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+ self.InitializeState()
- Y = Z - self.H * self.X_hat
- S = self.H * self.P * self.H.transpose() + self.R
- K = self.P * self.H.transpose() * numpy.linalg.inv(S)
- self.X_hat += K * Y
- self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+ def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
+ acceleration = math.sqrt(accelerometer_x**2 + accelerometer_y**2 +
+ accelerometer_z**2)
+ if acceleration < 0.9 or acceleration > 1.1:
+ glog.error('bad total acceleration %f' % acceleration)
+ # TODO(Brian): Tune this?
+ return
+ accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
+ Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+
+ Y = Z - self.H * self.X_hat
+ S = self.H * self.P * self.H.transpose() + self.R
+ K = self.P * self.H.transpose() * numpy.linalg.inv(S)
+ self.X_hat += K * Y
+ self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- estimator = DownEstimator()
+ estimator = DownEstimator()
- if FLAGS.plot:
- real_angles = [0]
- real_velocities = [0]
- estimated_angles = [0]
- estimated_velocities = [0]
- for _ in xrange(100):
- estimator.Predict(0)
- estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
- real_angles.append(math.pi / 2)
- real_velocities.append(0)
- estimated_angles.append(estimator.X_hat[0, 0])
- estimated_velocities.append(estimator.X_hat[1, 0])
- angle = math.pi / 2
- velocity = 1
- for i in xrange(100):
- measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
- estimator.Predict(measured_velocity)
- estimator.Update(math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
- math.cos(angle) + (random.random() - 0.5) * 0.02,
- measured_velocity)
- real_angles.append(angle)
- real_velocities.append(velocity)
- estimated_angles.append(estimator.X_hat[0, 0])
- estimated_velocities.append(estimator.X_hat[1, 0])
- angle += velocity * estimator.dt
- pylab.plot(range(len(real_angles)), real_angles)
- pylab.plot(range(len(real_velocities)), real_velocities)
- pylab.plot(range(len(estimated_angles)), estimated_angles)
- pylab.plot(range(len(estimated_velocities)), estimated_velocities)
- pylab.show()
+ if FLAGS.plot:
+ real_angles = [0]
+ real_velocities = [0]
+ estimated_angles = [0]
+ estimated_velocities = [0]
+ for _ in xrange(100):
+ estimator.Predict(0)
+ estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
+ real_angles.append(math.pi / 2)
+ real_velocities.append(0)
+ estimated_angles.append(estimator.X_hat[0, 0])
+ estimated_velocities.append(estimator.X_hat[1, 0])
+ angle = math.pi / 2
+ velocity = 1
+ for i in xrange(100):
+ measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
+ estimator.Predict(measured_velocity)
+ estimator.Update(
+ math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
+ math.cos(angle) + (random.random() - 0.5) * 0.02,
+ measured_velocity)
+ real_angles.append(angle)
+ real_velocities.append(velocity)
+ estimated_angles.append(estimator.X_hat[0, 0])
+ estimated_velocities.append(estimator.X_hat[1, 0])
+ angle += velocity * estimator.dt
+ pylab.plot(range(len(real_angles)), real_angles)
+ pylab.plot(range(len(real_velocities)), real_velocities)
+ pylab.plot(range(len(estimated_angles)), estimated_angles)
+ pylab.plot(range(len(estimated_velocities)), estimated_velocities)
+ pylab.show()
- if len(argv) != 3:
- print "Expected .h file name and .cc file name"
- else:
- namespaces = ['frc971', 'control_loops', 'drivetrain']
- kf_loop_writer = control_loop.ControlLoopWriter(
- "DownEstimator", [estimator],
- namespaces = namespaces)
- kf_loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "DownEstimator", [estimator], namespaces=namespaces)
+ kf_loop_writer.Write(argv[1], argv[2])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index c59cbab..1da85bc 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -7,7 +7,9 @@
from matplotlib import pylab
import glog
+
class DrivetrainParams(object):
+
def __init__(self,
J,
mass,
@@ -107,6 +109,7 @@
class Drivetrain(control_loop.ControlLoop):
+
def __init__(self,
drivetrain_params,
name="Drivetrain",
@@ -190,8 +193,8 @@
# Resistance of the motor, divided by the number of motors.
# Motor velocity constant
- self.Kv = (self.free_speed /
- (12.0 - self.resistance * self.free_current))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -215,14 +218,11 @@
# X will be of the format
# [[positionl], [velocityl], [positionr], velocityr]]
self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
- [0, 0, 0, 1],
- [0, -self.msnl * self.tcl, 0, -self.mspr * self.tcr]])
+ [[0, 1, 0, 0], [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
+ [0, 0, 0, 1], [0, -self.msnl * self.tcl, 0,
+ -self.mspr * self.tcr]])
self.B_continuous = numpy.matrix(
- [[0, 0],
- [self.mspl * self.mpl, self.msnr * self.mpr],
- [0, 0],
+ [[0, 0], [self.mspl * self.mpl, self.msnr * self.mpr], [0, 0],
[self.msnl * self.mpl, self.mspr * self.mpr]])
self.C = numpy.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
self.D = numpy.matrix([[0, 0], [0, 0]])
@@ -232,10 +232,10 @@
def BuildDrivetrainController(self, q_pos, q_vel):
# Tune the LQR controller
- self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0, 0.0,
- 0.0], [0.0, (1.0 / (q_vel**2.0)), 0.0, 0.0],
- [0.0, 0.0, (1.0 / (q_pos**2.0)),
- 0.0], [0.0, 0.0, 0.0, (1.0 / (q_vel**2.0))]])
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel**2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos**2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel**2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0**2.0)), 0.0],
[0.0, (1.0 / (12.0**2.0))]])
@@ -254,6 +254,7 @@
class KFDrivetrain(Drivetrain):
+
def __init__(self,
drivetrain_params,
name="KFDrivetrain",
@@ -291,9 +292,10 @@
self.A_continuous[0:4, 0:4] = self.unaugmented_A_continuous
if self.force:
- self.A_continuous[0:4, 4:6] = numpy.matrix(
- [[0.0, 0.0], [self.mspl, self.msnl], [0.0, 0.0],
- [self.msnr, self.mspr]])
+ self.A_continuous[0:4, 4:6] = numpy.matrix([[0.0, 0.0],
+ [self.mspl, self.msnl],
+ [0.0, 0.0],
+ [self.msnr, self.mspr]])
q_voltage = drivetrain_params.kf_q_voltage * self.mpl
else:
self.A_continuous[0:4, 4:6] = self.unaugmented_B_continuous
@@ -307,28 +309,31 @@
self.B_continuous, self.dt)
if self.has_imu:
- self.C = numpy.matrix(
- [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
- 0, -0.5 / drivetrain_params.robot_radius, 0,
- 0.5 / drivetrain_params.robot_radius, 0, 0, 0
- ], [0, 0, 0, 0, 0, 0, 0]])
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
+ [
+ 0, -0.5 / drivetrain_params.robot_radius,
+ 0, 0.5 / drivetrain_params.robot_radius,
+ 0, 0, 0
+ ], [0, 0, 0, 0, 0, 0, 0]])
gravity = 9.8
self.C[3, 0:6] = 0.5 * (
- self.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]
- ) / gravity
+ self.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]) / gravity
- self.D = numpy.matrix([[0, 0], [0, 0], [0, 0], [
- 0.5 *
- (self.B_continuous[1, 0] + self.B_continuous[3, 0]) / gravity,
- 0.5 *
- (self.B_continuous[1, 1] + self.B_continuous[3, 1]) / gravity
- ]])
+ self.D = numpy.matrix(
+ [[0, 0], [0, 0], [0, 0],
+ [
+ 0.5 * (self.B_continuous[1, 0] + self.B_continuous[3, 0]) /
+ gravity,
+ 0.5 * (self.B_continuous[1, 1] + self.B_continuous[3, 1]) /
+ gravity
+ ]])
else:
- self.C = numpy.matrix(
- [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
- 0, -0.5 / drivetrain_params.robot_radius, 0,
- 0.5 / drivetrain_params.robot_radius, 0, 0, 0
- ]])
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
+ [
+ 0, -0.5 / drivetrain_params.robot_radius,
+ 0, 0.5 / drivetrain_params.robot_radius,
+ 0, 0, 0
+ ]])
self.D = numpy.matrix([[0, 0], [0, 0], [0, 0]])
@@ -337,12 +342,12 @@
q_encoder_uncertainty = 2.00
self.Q = numpy.matrix(
- [[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0], [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0,
- 0.0], [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0,
- 0.0], [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0,
- 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
+ [[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty**2.0)]])
r_pos = 0.0001
@@ -365,7 +370,8 @@
# If we don't have an IMU, pad various matrices with zeros so that
# we can still have 4 measurement outputs.
if not self.has_imu:
- self.KalmanGain = numpy.hstack((self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
+ self.KalmanGain = numpy.hstack((self.KalmanGain,
+ numpy.matrix(numpy.zeros((7, 1)))))
self.C = numpy.vstack((self.C, numpy.matrix(numpy.zeros((1, 7)))))
self.D = numpy.vstack((self.D, numpy.matrix(numpy.zeros((1, 2)))))
Rtmp = numpy.zeros((4, 4))
@@ -401,8 +407,11 @@
self.InitializeState()
-def WriteDrivetrain(drivetrain_files, kf_drivetrain_files, year_namespace,
- drivetrain_params, scalar_type='double'):
+def WriteDrivetrain(drivetrain_files,
+ kf_drivetrain_files,
+ year_namespace,
+ drivetrain_params,
+ scalar_type='double'):
# Write the generated constants out to a file.
drivetrain_low_low = Drivetrain(
@@ -569,8 +578,7 @@
pylab.plot(range(200), close_loop_left, label='left position')
pylab.plot(range(200), close_loop_right, label='right position')
pylab.suptitle(
- 'Angular Move\nLeft position going to -1 and right position going to 1'
- )
+ 'Angular Move\nLeft position going to -1 and right position going to 1')
pylab.legend(loc='center right')
pylab.show()
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 6c88e15..088b204 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -15,392 +15,443 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
gflags.DEFINE_string('data', None, 'If defined, plot the provided CAN data')
-gflags.DEFINE_bool('rerun_kf', False, 'If true, rerun the KF. The torque in the data file will be interpreted as the commanded current.')
+gflags.DEFINE_bool(
+ 'rerun_kf', False,
+ 'If true, rerun the KF. The torque in the data file will be interpreted as the commanded current.'
+)
+
class SystemParams(object):
- def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
- q_torque, current_limit):
- self.J = J
- self.G = G
- self.q_pos = q_pos
- self.q_vel = q_vel
- self.q_torque = q_torque
- self.kP = kP
- self.kD = kD
- self.kCompensationTimeconstant = kCompensationTimeconstant
- self.r_pos = 0.001
- self.current_limit = current_limit
- #[15.0, 0.25],
- #[10.0, 0.2],
- #[5.0, 0.13],
- #[3.0, 0.10],
- #[2.0, 0.08],
- #[1.0, 0.06],
- #[0.5, 0.05],
- #[0.25, 0.025],
+ def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
+ q_torque, current_limit):
+ self.J = J
+ self.G = G
+ self.q_pos = q_pos
+ self.q_vel = q_vel
+ self.q_torque = q_torque
+ self.kP = kP
+ self.kD = kD
+ self.kCompensationTimeconstant = kCompensationTimeconstant
+ self.r_pos = 0.001
+ self.current_limit = current_limit
-kWheel = SystemParams(J=0.0008,
- G=(1.25 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.20,
- q_torque=0.005,
- kP=7.0,
- kD=0.0,
- kCompensationTimeconstant=0.95,
- current_limit=4.5)
-kTrigger = SystemParams(J=0.00025,
- G=(0.925 * 2.0 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.1,
- q_torque=0.005,
- kP=120.0,
- kD=1.8,
- kCompensationTimeconstant=0.95,
- current_limit=3.0)
+ #[15.0, 0.25],
+ #[10.0, 0.2],
+ #[5.0, 0.13],
+ #[3.0, 0.10],
+ #[2.0, 0.08],
+ #[1.0, 0.06],
+ #[0.5, 0.05],
+ #[0.25, 0.025],
+
+
+kWheel = SystemParams(
+ J=0.0008,
+ G=(1.25 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.20,
+ q_torque=0.005,
+ kP=7.0,
+ kD=0.0,
+ kCompensationTimeconstant=0.95,
+ current_limit=4.5)
+kTrigger = SystemParams(
+ J=0.00025,
+ G=(0.925 * 2.0 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.1,
+ q_torque=0.005,
+ kP=120.0,
+ kD=1.8,
+ kCompensationTimeconstant=0.95,
+ current_limit=3.0)
+
class HapticInput(control_loop.ControlLoop):
- def __init__(self, params=None, name='HapticInput'):
- # The defaults are for the steering wheel.
- super(HapticInput, self).__init__(name)
- motor = self.motor = control_loop.MN3510()
- # Moment of inertia of the wheel in kg m^2
- self.J = params.J
+ def __init__(self, params=None, name='HapticInput'):
+ # The defaults are for the steering wheel.
+ super(HapticInput, self).__init__(name)
+ motor = self.motor = control_loop.MN3510()
- # Control loop time step
- self.dt = 0.001
+ # Moment of inertia of the wheel in kg m^2
+ self.J = params.J
- # Gear ratio from the motor to the input.
- self.G = params.G
+ # Control loop time step
+ self.dt = 0.001
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1, 1] = 0
- self.A_continuous[0, 1] = 1
+ # Gear ratio from the motor to the input.
+ self.G = params.G
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1, 0] = motor.Kt * self.G / self.J
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1, 1] = 0
+ self.A_continuous[0, 1] = 1
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1, 0] = motor.Kt * self.G / self.J
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
- self.U_max = numpy.matrix([[2.5]])
- self.U_min = numpy.matrix([[-2.5]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.L = numpy.matrix([[0.0], [0.0]])
- self.K = numpy.matrix([[0.0, 0.0]])
+ self.U_max = numpy.matrix([[2.5]])
+ self.U_min = numpy.matrix([[-2.5]])
- self.InitializeState()
+ self.L = numpy.matrix([[0.0], [0.0]])
+ self.K = numpy.matrix([[0.0, 0.0]])
+
+ self.InitializeState()
class IntegralHapticInput(HapticInput):
- def __init__(self, params=None, name="IntegralHapticInput"):
- super(IntegralHapticInput, self).__init__(name=name, params=params)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, params=None, name="IntegralHapticInput"):
+ super(IntegralHapticInput, self).__init__(name=name, params=params)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[1, 2] = (1 / self.J)
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[1, 2] = (1 / self.J)
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- self.Q = numpy.matrix([[(params.q_pos ** 2.0), 0.0, 0.0],
- [0.0, (params.q_vel ** 2.0), 0.0],
- [0.0, 0.0, (params.q_torque ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.R = numpy.matrix([[(params.r_pos ** 2.0)]])
+ self.Q = numpy.matrix([[(params.q_pos**2.0), 0.0, 0.0],
+ [0.0, (params.q_vel**2.0), 0.0],
+ [0.0, 0.0, (params.q_torque**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ self.R = numpy.matrix([[(params.r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+
+ self.InitializeState()
+
def ReadCan(filename):
- """Reads the candump in filename and returns the 4 fields."""
- trigger = []
- trigger_velocity = []
- trigger_torque = []
- trigger_current = []
- wheel = []
- wheel_velocity = []
- wheel_torque = []
- wheel_current = []
+ """Reads the candump in filename and returns the 4 fields."""
+ trigger = []
+ trigger_velocity = []
+ trigger_torque = []
+ trigger_current = []
+ wheel = []
+ wheel_velocity = []
+ wheel_torque = []
+ wheel_current = []
- trigger_request_time = [0.0]
- trigger_request_current = [0.0]
- wheel_request_time = [0.0]
- wheel_request_current = [0.0]
+ trigger_request_time = [0.0]
+ trigger_request_current = [0.0]
+ wheel_request_time = [0.0]
+ wheel_request_current = [0.0]
- with open(filename, 'r') as fd:
- for line in fd:
- data = line.split()
- can_id = int(data[1], 16)
- if can_id == 0:
- data = [int(d, 16) for d in data[3:]]
- trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
- trigger_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
- trigger_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- trigger_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
- elif can_id == 1:
- data = [int(d, 16) for d in data[3:]]
- wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
- wheel_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
- wheel_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- wheel_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
- elif can_id == 2:
- data = [int(d, 16) for d in data[3:]]
- trigger_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- trigger_request_time.append(len(trigger) * 0.001)
- elif can_id == 3:
- data = [int(d, 16) for d in data[3:]]
- wheel_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- wheel_request_time.append(len(wheel) * 0.001)
+ with open(filename, 'r') as fd:
+ for line in fd:
+ data = line.split()
+ can_id = int(data[1], 16)
+ if can_id == 0:
+ data = [int(d, 16) for d in data[3:]]
+ trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ trigger_velocity.append(
+ ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ trigger_torque.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_current.append(
+ ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 1:
+ data = [int(d, 16) for d in data[3:]]
+ wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ wheel_velocity.append(
+ ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ wheel_torque.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_current.append(
+ ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 2:
+ data = [int(d, 16) for d in data[3:]]
+ trigger_request_current.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_request_time.append(len(trigger) * 0.001)
+ elif can_id == 3:
+ data = [int(d, 16) for d in data[3:]]
+ wheel_request_current.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_request_time.append(len(wheel) * 0.001)
- trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
- wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
+ trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
+ wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
- # Extend out the data in the interpolation table.
- trigger_request_time.append(trigger_data_time[-1])
- trigger_request_current.append(trigger_request_current[-1])
- wheel_request_time.append(wheel_data_time[-1])
- wheel_request_current.append(wheel_request_current[-1])
+ # Extend out the data in the interpolation table.
+ trigger_request_time.append(trigger_data_time[-1])
+ trigger_request_current.append(trigger_request_current[-1])
+ wheel_request_time.append(wheel_data_time[-1])
+ wheel_request_current.append(wheel_request_current[-1])
- return (trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity,
- wheel_velocity, trigger_torque, wheel_torque, trigger_current,
- wheel_current, trigger_request_time, trigger_request_current,
- wheel_request_time, wheel_request_current)
+ return (trigger_data_time, wheel_data_time, trigger, wheel,
+ trigger_velocity, wheel_velocity, trigger_torque, wheel_torque,
+ trigger_current, wheel_current, trigger_request_time,
+ trigger_request_current, wheel_request_time, wheel_request_current)
-def rerun_and_plot_kf(data_time, data_radians, data_current, data_request_current,
- params, run_correct=True):
- kf_velocity = []
- dt_velocity = []
- kf_position = []
- adjusted_position = []
- last_angle = None
- haptic_observer = IntegralHapticInput(params=params)
- # Parameter sweep J.
- num_kf = 1
- min_J = max_J = params.J
+def rerun_and_plot_kf(data_time,
+ data_radians,
+ data_current,
+ data_request_current,
+ params,
+ run_correct=True):
+ kf_velocity = []
+ dt_velocity = []
+ kf_position = []
+ adjusted_position = []
+ last_angle = None
+ haptic_observer = IntegralHapticInput(params=params)
- # J = 0.0022
- #num_kf = 15
- #min_J = min_J / 2.0
- #max_J = max_J * 2.0
- initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+ # Parameter sweep J.
+ num_kf = 1
+ min_J = max_J = params.J
- def DupParamsWithJ(params, J):
- p = copy.copy(params)
- p.J = J
- return p
- haptic_observers = [IntegralHapticInput(params=DupParamsWithJ(params, j)) for j in
- numpy.logspace(numpy.log10(min_J),
- numpy.log10(max_J), num=num_kf)]
- # Initialize all the KF's.
- haptic_observer.X_hat[1, 0] = initial_velocity
- haptic_observer.X_hat[0, 0] = data_radians[0]
- for observer in haptic_observers:
- observer.X_hat[1, 0] = initial_velocity
- observer.X_hat[0, 0] = data_radians[0]
+ # J = 0.0022
+ #num_kf = 15
+ #min_J = min_J / 2.0
+ #max_J = max_J * 2.0
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
- last_request_current = data_request_current[0]
- kf_torques = [[] for i in xrange(num_kf)]
- for angle, current, request_current in zip(data_radians, data_current,
- data_request_current):
- # Predict and correct all the parameter swept observers.
- for i, observer in enumerate(haptic_observers):
- observer.Y = numpy.matrix([[angle]])
- if run_correct:
- observer.CorrectObserver(numpy.matrix([[current]]))
- kf_torques[i].append(-observer.X_hat[2, 0])
- observer.PredictObserver(numpy.matrix([[current]]))
- observer.PredictObserver(numpy.matrix([[current]]))
+ def DupParamsWithJ(params, J):
+ p = copy.copy(params)
+ p.J = J
+ return p
- # Predict and correct the main observer.
- haptic_observer.Y = numpy.matrix([[angle]])
- if run_correct:
- haptic_observer.CorrectObserver(numpy.matrix([[current]]))
- kf_position.append(haptic_observer.X_hat[0, 0])
- adjusted_position.append(kf_position[-1] - last_request_current / params.kP)
- last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (1.0 - params.kCompensationTimeconstant)
- kf_velocity.append(haptic_observer.X_hat[1, 0])
- if last_angle is None:
- last_angle = angle
- dt_velocity.append((angle - last_angle) / 0.001)
+ haptic_observers = [
+ IntegralHapticInput(params=DupParamsWithJ(params, j))
+ for j in numpy.logspace(
+ numpy.log10(min_J), numpy.log10(max_J), num=num_kf)
+ ]
+ # Initialize all the KF's.
+ haptic_observer.X_hat[1, 0] = initial_velocity
+ haptic_observer.X_hat[0, 0] = data_radians[0]
+ for observer in haptic_observers:
+ observer.X_hat[1, 0] = initial_velocity
+ observer.X_hat[0, 0] = data_radians[0]
- haptic_observer.PredictObserver(numpy.matrix([[current]]))
- last_angle = angle
+ last_request_current = data_request_current[0]
+ kf_torques = [[] for i in xrange(num_kf)]
+ for angle, current, request_current in zip(data_radians, data_current,
+ data_request_current):
+ # Predict and correct all the parameter swept observers.
+ for i, observer in enumerate(haptic_observers):
+ observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_torques[i].append(-observer.X_hat[2, 0])
+ observer.PredictObserver(numpy.matrix([[current]]))
+ observer.PredictObserver(numpy.matrix([[current]]))
- # Plot the wheel observers.
- fig, ax1 = pylab.subplots()
- ax1.plot(data_time, data_radians, '.', label='wheel')
- ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
- ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
- ax1.plot(data_time, kf_position, '.', label='kf_position')
- ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
+ # Predict and correct the main observer.
+ haptic_observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ haptic_observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_position.append(haptic_observer.X_hat[0, 0])
+ adjusted_position.append(kf_position[-1] -
+ last_request_current / params.kP)
+ last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (
+ 1.0 - params.kCompensationTimeconstant)
+ kf_velocity.append(haptic_observer.X_hat[1, 0])
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
- ax2 = ax1.twinx()
- ax2.plot(data_time, data_current, label='data_current')
- ax2.plot(data_time, data_request_current, label='request_current')
+ haptic_observer.PredictObserver(numpy.matrix([[current]]))
+ last_angle = angle
- for i, kf_torque in enumerate(kf_torques):
- ax2.plot(data_time, kf_torque,
- label='-kf_torque[%f]' % haptic_observers[i].J)
- fig.tight_layout()
- ax1.legend(loc=3)
- ax2.legend(loc=4)
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='wheel')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+ ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
+ ax1.plot(data_time, kf_position, '.', label='kf_position')
+ ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
-def plot_input(data_time, data_radians, data_velocity, data_torque,
- data_current, params, run_correct=True):
- dt_velocity = []
- last_angle = None
- initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_current, label='data_current')
+ ax2.plot(data_time, data_request_current, label='request_current')
- for angle in data_radians:
- if last_angle is None:
- last_angle = angle
- dt_velocity.append((angle - last_angle) / 0.001)
+ for i, kf_torque in enumerate(kf_torques):
+ ax2.plot(
+ data_time,
+ kf_torque,
+ label='-kf_torque[%f]' % haptic_observers[i].J)
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
- last_angle = angle
- # Plot the wheel observers.
- fig, ax1 = pylab.subplots()
- ax1.plot(data_time, data_radians, '.', label='angle')
- ax1.plot(data_time, data_velocity, '-', label='velocity')
- ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+def plot_input(data_time,
+ data_radians,
+ data_velocity,
+ data_torque,
+ data_current,
+ params,
+ run_correct=True):
+ dt_velocity = []
+ last_angle = None
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
- ax2 = ax1.twinx()
- ax2.plot(data_time, data_torque, label='data_torque')
- ax2.plot(data_time, data_current, label='data_current')
- fig.tight_layout()
- ax1.legend(loc=3)
- ax2.legend(loc=4)
+ for angle in data_radians:
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
+
+ last_angle = angle
+
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='angle')
+ ax1.plot(data_time, data_velocity, '-', label='velocity')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_torque, label='data_torque')
+ ax2.plot(data_time, data_current, label='data_current')
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
+
def main(argv):
- if FLAGS.plot:
- if FLAGS.data is None:
- haptic_wheel = HapticInput()
- haptic_wheel_controller = IntegralHapticInput()
- observer_haptic_wheel = IntegralHapticInput()
- observer_haptic_wheel.X_hat[2, 0] = 0.01
+ if FLAGS.plot:
+ if FLAGS.data is None:
+ haptic_wheel = HapticInput()
+ haptic_wheel_controller = IntegralHapticInput()
+ observer_haptic_wheel = IntegralHapticInput()
+ observer_haptic_wheel.X_hat[2, 0] = 0.01
- R = numpy.matrix([[0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [0.0], [0.0]])
- control_loop.TestSingleIntegralAxisSquareWave(
- R, 1.0, haptic_wheel, haptic_wheel_controller, observer_haptic_wheel)
+ control_loop.TestSingleIntegralAxisSquareWave(
+ R, 1.0, haptic_wheel, haptic_wheel_controller,
+ observer_haptic_wheel)
+ else:
+ # Read the CAN trace in.
+ trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
+ wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
+ wheel_current, trigger_request_time, trigger_request_current, \
+ wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+
+ wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
+ wheel_velocity = [w * 50.0 for w in wheel_velocity]
+ wheel_torque = [w / 2.0 for w in wheel_torque]
+ wheel_current = [w * 10.0 for w in wheel_current]
+ wheel_request_current = [w * 2.0 for w in wheel_request_current]
+ resampled_wheel_request_current = scipy.interpolate.interp1d(
+ wheel_request_time, wheel_request_current,
+ kind="zero")(wheel_data_time)
+
+ trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
+ trigger_velocity = [t * 50.0 for t in trigger_velocity]
+ trigger_torque = [t / 2.0 for t in trigger_torque]
+ trigger_current = [t * 10.0 for t in trigger_current]
+ trigger_request_current = [t * 2.0 for t in trigger_request_current]
+ resampled_trigger_request_current = scipy.interpolate.interp1d(
+ trigger_request_time, trigger_request_current,
+ kind="zero")(trigger_data_time)
+
+ if FLAGS.rerun_kf:
+ rerun_and_plot_kf(
+ trigger_data_time,
+ trigger_radians,
+ trigger_current,
+ resampled_trigger_request_current,
+ kTrigger,
+ run_correct=True)
+ rerun_and_plot_kf(
+ wheel_data_time,
+ wheel_radians,
+ wheel_current,
+ resampled_wheel_request_current,
+ kWheel,
+ run_correct=True)
+ else:
+ plot_input(trigger_data_time, trigger_radians, trigger_velocity,
+ trigger_torque, trigger_current, kTrigger)
+ plot_input(wheel_data_time, wheel_radians, wheel_velocity,
+ wheel_torque, wheel_current, kWheel)
+ pylab.show()
+
+ return
+
+ if len(argv) != 9:
+ glog.fatal('Expected .h file name and .cc file name')
else:
- # Read the CAN trace in.
- trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
- wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
- wheel_current, trigger_request_time, trigger_request_current, \
- wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
+ ('HapticTrigger', kTrigger, argv[5:9])]:
+ haptic_input = HapticInput(params=params, name=name)
+ loop_writer = control_loop.ControlLoopWriter(
+ name, [haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
+ loop_writer.Write(filenames[0], filenames[1])
- wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
- wheel_velocity = [w * 50.0 for w in wheel_velocity]
- wheel_torque = [w / 2.0 for w in wheel_torque]
- wheel_current = [w * 10.0 for w in wheel_current]
- wheel_request_current = [w * 2.0 for w in wheel_request_current]
- resampled_wheel_request_current = scipy.interpolate.interp1d(
- wheel_request_time, wheel_request_current, kind="zero")(wheel_data_time)
+ integral_haptic_input = IntegralHapticInput(
+ params=params, name='Integral' + name)
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'Integral' + name, [integral_haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
- trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
- trigger_velocity = [t * 50.0 for t in trigger_velocity]
- trigger_torque = [t / 2.0 for t in trigger_torque]
- trigger_current = [t * 10.0 for t in trigger_current]
- trigger_request_current = [t * 2.0 for t in trigger_request_current]
- resampled_trigger_request_current = scipy.interpolate.interp1d(
- trigger_request_time, trigger_request_current, kind="zero")(trigger_data_time)
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "Dt", "%f",
+ integral_haptic_input.dt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "FreeCurrent", "%f",
+ integral_haptic_input.motor.free_current))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "StallTorque", "%f",
+ integral_haptic_input.motor.stall_torque))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "J", "%f",
+ integral_haptic_input.J))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "R", "%f",
+ integral_haptic_input.motor.resistance))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "T", "%f",
+ integral_haptic_input.motor.Kt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "V", "%f",
+ integral_haptic_input.motor.Kv))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "P", "%f", params.kP))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "D", "%f", params.kD))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "G", "%f", params.G))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "CurrentLimit", "%f",
+ params.current_limit))
- if FLAGS.rerun_kf:
- rerun_and_plot_kf(trigger_data_time, trigger_radians, trigger_current,
- resampled_trigger_request_current, kTrigger, run_correct=True)
- rerun_and_plot_kf(wheel_data_time, wheel_radians, wheel_current,
- resampled_wheel_request_current, kWheel, run_correct=True)
- else:
- plot_input(trigger_data_time, trigger_radians, trigger_velocity,
- trigger_torque, trigger_current, kTrigger)
- plot_input(wheel_data_time, wheel_radians, wheel_velocity, wheel_torque,
- wheel_current, kWheel)
- pylab.show()
-
- return
-
- if len(argv) != 9:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['frc971', 'control_loops', 'drivetrain']
- for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
- ('HapticTrigger', kTrigger, argv[5:9])]:
- haptic_input = HapticInput(params=params, name=name)
- loop_writer = control_loop.ControlLoopWriter(name, [haptic_input],
- namespaces=namespaces,
- scalar_type='float')
- loop_writer.Write(filenames[0], filenames[1])
-
- integral_haptic_input = IntegralHapticInput(params=params,
- name='Integral' + name)
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'Integral' + name, [integral_haptic_input], namespaces=namespaces,
- scalar_type='float')
-
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "Dt", "%f",
- integral_haptic_input.dt))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "FreeCurrent", "%f",
- integral_haptic_input.motor.free_current))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "StallTorque", "%f",
- integral_haptic_input.motor.stall_torque))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "J", "%f",
- integral_haptic_input.J))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "R", "%f",
- integral_haptic_input.motor.resistance))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "T", "%f",
- integral_haptic_input.motor.Kt))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "V", "%f",
- integral_haptic_input.motor.Kv))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "P", "%f",
- params.kP))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "D", "%f",
- params.kD))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "G", "%f",
- params.G))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "CurrentLimit", "%f",
- params.current_limit))
-
- integral_loop_writer.Write(filenames[2], filenames[3])
+ integral_loop_writer.Write(filenames[2], filenames[3])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index e1f55cc..175fffa 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -9,6 +9,7 @@
class LinearSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -37,6 +38,7 @@
class LinearSystem(control_loop.ControlLoop):
+
def __init__(self, params, name='LinearSystem'):
super(LinearSystem, self).__init__(name)
self.params = params
@@ -57,10 +59,9 @@
# State is [position, velocity]
# Input is [Voltage]
C1 = self.motor.Kt / (self.G * self.G * self.radius * self.radius *
- self.motor.resistance * self.mass *
- self.motor.Kv)
- C2 = self.motor.Kt / (self.G * self.radius * self.motor.resistance *
- self.mass)
+ self.motor.resistance * self.mass * self.motor.Kv)
+ C2 = self.motor.Kt / (
+ self.G * self.radius * self.motor.resistance * self.mass)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -79,9 +80,10 @@
glog.debug('Controllability of %d',
numpy.linalg.matrix_rank(controllability))
glog.debug('Mass: %f', self.mass)
- glog.debug('Stall force: %f', self.motor.stall_torque / self.G / self.radius)
- glog.debug('Stall acceleration: %f', self.motor.stall_torque / self.G /
- self.radius / self.mass)
+ glog.debug('Stall force: %f',
+ self.motor.stall_torque / self.G / self.radius)
+ glog.debug('Stall acceleration: %f',
+ self.motor.stall_torque / self.G / self.radius / self.mass)
glog.debug('Free speed is %f',
-self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
@@ -122,6 +124,7 @@
class IntegralLinearSystem(LinearSystem):
+
def __init__(self, params, name='IntegralLinearSystem'):
super(IntegralLinearSystem, self).__init__(params, name=name)
@@ -142,10 +145,9 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
- self.Q = numpy.matrix(
- [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
- [0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+ self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+ [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+ [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -203,8 +205,8 @@
vbat = 12.0
- goal = numpy.concatenate(
- (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
profile = TrapezoidProfile(plant.dt)
profile.set_maximum_acceleration(max_acceleration)
@@ -263,8 +265,7 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -378,11 +379,11 @@
loop_writer = control_loop.ControlLoopWriter(
linear_system.name, [linear_system], namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', linear_system.motor.
- free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ linear_system.motor.free_speed))
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f', linear_system.G *
- linear_system.radius))
+ control_loop.Constant('kOutputRatio', '%f',
+ linear_system.G * linear_system.radius))
loop_writer.AddConstant(
control_loop.Constant('kRadius', '%f', linear_system.radius))
loop_writer.Write(plant_files[0], plant_files[1])
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 91dba3c..06a182e 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -10,516 +10,565 @@
import glog
+
def CoerceGoal(region, K, w, R):
- """Intersects a line with a region, and finds the closest point to R.
+ """Intersects a line with a region, and finds the closest point to R.
- Finds a point that is closest to R inside the region, and on the line
- defined by K X = w. If it is not possible to find a point on the line,
- finds a point that is inside the region and closest to the line. This
- function assumes that
+ Finds a point that is closest to R inside the region, and on the line
+ defined by K X = w. If it is not possible to find a point on the line,
+ finds a point that is inside the region and closest to the line. This
+ function assumes that
- Args:
- region: HPolytope, the valid goal region.
- K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
- w: float, the offset in the equation above.
- R: numpy.matrix (2 x 1), the point to be closest to.
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
- Returns:
- numpy.matrix (2 x 1), the point.
- """
- return DoCoerceGoal(region, K, w, R)[0]
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+ return DoCoerceGoal(region, K, w, R)[0]
+
def DoCoerceGoal(region, K, w, R):
- if region.IsInside(R):
- return (R, True)
+ if region.IsInside(R):
+ return (R, True)
- perpendicular_vector = K.T / numpy.linalg.norm(K)
- parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
- [-perpendicular_vector[0, 0]]])
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
- # We want to impose the constraint K * X = w on the polytope H * X <= k.
- # We do this by breaking X up into parallel and perpendicular components to
- # the half plane. This gives us the following equation.
- #
- # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
- #
- # Then, substitute this into the polytope.
- #
- # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
- #
- # Substitute K * X = w
- #
- # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
- #
- # Move all the knowns to the right side.
- #
- # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
- #
- # Let t = parallel.T \dot X, the component parallel to the surface.
- #
- # H * parallel * t <= k - H * perpendicular * w
- #
- # This is a polytope which we can solve, and use to figure out the range of X
- # that we care about!
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
- t_poly = polytope.HPolytope(
- region.H * parallel_vector,
- region.k - region.H * perpendicular_vector * w)
+ t_poly = polytope.HPolytope(region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
- vertices = t_poly.Vertices()
+ vertices = t_poly.Vertices()
- if vertices.shape[0]:
- # The region exists!
- # Find the closest vertex
- min_distance = numpy.infty
- closest_point = None
- for vertex in vertices:
- point = parallel_vector * vertex + perpendicular_vector * w
- length = numpy.linalg.norm(R - point)
- if length < min_distance:
- min_distance = length
- closest_point = point
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
- return (closest_point, True)
- else:
- # Find the vertex of the space that is closest to the line.
- region_vertices = region.Vertices()
- min_distance = numpy.infty
- closest_point = None
- for vertex in region_vertices:
- point = vertex.T
- length = numpy.abs((perpendicular_vector.T * point)[0, 0])
- if length < min_distance:
- min_distance = length
- closest_point = point
+ return (closest_point, True)
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
- return (closest_point, False)
+ return (closest_point, False)
+
class VelocityDrivetrainModel(control_loop.ControlLoop):
- def __init__(self, drivetrain_params, left_low=True, right_low=True,
- name="VelocityDrivetrainModel"):
- super(VelocityDrivetrainModel, self).__init__(name)
- self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
- left_low=left_low, right_low=right_low,
- drivetrain_params=drivetrain_params)
- self.dt = drivetrain_params.dt
- self.A_continuous = numpy.matrix(
- [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
- [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
- self.B_continuous = numpy.matrix(
- [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
- [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
- self.C = numpy.matrix(numpy.eye(2))
- self.D = numpy.matrix(numpy.zeros((2, 2)))
+ def __init__(self,
+ drivetrain_params,
+ left_low=True,
+ right_low=True,
+ name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
+ left_low=left_low,
+ right_low=right_low,
+ drivetrain_params=drivetrain_params)
+ self.dt = drivetrain_params.dt
+ self.A_continuous = numpy.matrix(
+ [[
+ self._drivetrain.A_continuous[1, 1],
+ self._drivetrain.A_continuous[1, 3]
+ ],
+ [
+ self._drivetrain.A_continuous[3, 1],
+ self._drivetrain.A_continuous[3, 3]
+ ]])
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
+ self.B_continuous = numpy.matrix(
+ [[
+ self._drivetrain.B_continuous[1, 0],
+ self._drivetrain.B_continuous[1, 1]
+ ],
+ [
+ self._drivetrain.B_continuous[3, 0],
+ self._drivetrain.B_continuous[3, 1]
+ ]])
+ self.C = numpy.matrix(numpy.eye(2))
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
- # FF * X = U (steady state)
- self.FF = self.B.I * (numpy.eye(2) - self.A)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.PlaceControllerPoles(drivetrain_params.controller_poles)
- # Build a kalman filter for the velocity. We don't care what the gains
- # are, but the hybrid kalman filter that we want to write to disk to get
- # access to A_continuous and B_continuous needs this for completeness.
- self.Q_continuous = numpy.matrix([[(0.5 ** 2.0), 0.0], [0.0, (0.5 ** 2.0)]])
- self.R_continuous = numpy.matrix([[(0.1 ** 2.0), 0.0], [0.0, (0.1 ** 2.0)]])
- self.PlaceObserverPoles(drivetrain_params.observer_poles)
- _, _, self.Q, self.R = controls.kalmd(
- A_continuous=self.A_continuous, B_continuous=self.B_continuous,
- Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
- dt=self.dt)
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.PlaceControllerPoles(drivetrain_params.controller_poles)
+ # Build a kalman filter for the velocity. We don't care what the gains
+ # are, but the hybrid kalman filter that we want to write to disk to get
+ # access to A_continuous and B_continuous needs this for completeness.
+ self.Q_continuous = numpy.matrix([[(0.5**2.0), 0.0], [0.0, (0.5**2.0)]])
+ self.R_continuous = numpy.matrix([[(0.1**2.0), 0.0], [0.0, (0.1**2.0)]])
+ self.PlaceObserverPoles(drivetrain_params.observer_poles)
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
- self.G_high = self._drivetrain.G_high
- self.G_low = self._drivetrain.G_low
- self.resistance = self._drivetrain.resistance
- self.r = self._drivetrain.r
- self.Kv = self._drivetrain.Kv
- self.Kt = self._drivetrain.Kt
+ self.KalmanGain, self.P_steady_state = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.U_max = self._drivetrain.U_max
- self.U_min = self._drivetrain.U_min
+ self.G_high = self._drivetrain.G_high
+ self.G_low = self._drivetrain.G_low
+ self.resistance = self._drivetrain.resistance
+ self.r = self._drivetrain.r
+ self.Kv = self._drivetrain.Kv
+ self.Kt = self._drivetrain.Kt
- @property
- def robot_radius_l(self):
- return self._drivetrain.robot_radius_l
- @property
- def robot_radius_r(self):
- return self._drivetrain.robot_radius_r
+ self.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+ @property
+ def robot_radius_l(self):
+ return self._drivetrain.robot_radius_l
+
+ @property
+ def robot_radius_r(self):
+ return self._drivetrain.robot_radius_r
+
class VelocityDrivetrain(object):
- HIGH = 'high'
- LOW = 'low'
- SHIFTING_UP = 'up'
- SHIFTING_DOWN = 'down'
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
- def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
- self.drivetrain_low_low = VelocityDrivetrainModel(
- left_low=True, right_low=True, name=name + 'LowLow',
- drivetrain_params=drivetrain_params)
- self.drivetrain_low_high = VelocityDrivetrainModel(
- left_low=True, right_low=False, name=name + 'LowHigh',
- drivetrain_params=drivetrain_params)
- self.drivetrain_high_low = VelocityDrivetrainModel(
- left_low=False, right_low=True, name = name + 'HighLow',
- drivetrain_params=drivetrain_params)
- self.drivetrain_high_high = VelocityDrivetrainModel(
- left_low=False, right_low=False, name = name + 'HighHigh',
- drivetrain_params=drivetrain_params)
+ def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True,
+ right_low=True,
+ name=name + 'LowLow',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_low_high = VelocityDrivetrainModel(
+ left_low=True,
+ right_low=False,
+ name=name + 'LowHigh',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_high_low = VelocityDrivetrainModel(
+ left_low=False,
+ right_low=True,
+ name=name + 'HighLow',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_high_high = VelocityDrivetrainModel(
+ left_low=False,
+ right_low=False,
+ name=name + 'HighHigh',
+ drivetrain_params=drivetrain_params)
- # X is [lvel, rvel]
- self.X = numpy.matrix(
- [[0.0],
- [0.0]])
+ # X is [lvel, rvel]
+ self.X = numpy.matrix([[0.0], [0.0]])
- self.U_poly = polytope.HPolytope(
- numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]]),
- numpy.matrix([[12],
- [12],
- [12],
- [12]]))
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
+ numpy.matrix([[12], [12], [12], [12]]))
- self.U_max = numpy.matrix(
- [[12.0],
- [12.0]])
- self.U_min = numpy.matrix(
- [[-12.0000000000],
- [-12.0000000000]])
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0000000000], [-12.0000000000]])
- self.dt = 0.00505
+ self.dt = 0.00505
- self.R = numpy.matrix(
- [[0.0],
- [0.0]])
+ self.R = numpy.matrix([[0.0], [0.0]])
- self.U_ideal = numpy.matrix(
- [[0.0],
- [0.0]])
+ self.U_ideal = numpy.matrix([[0.0], [0.0]])
- # ttrust is the comprimise between having full throttle negative inertia,
- # and having no throttle negative inertia. A value of 0 is full throttle
- # inertia. A value of 1 is no throttle negative inertia.
- self.ttrust = 1.0
+ # ttrust is the comprimise between having full throttle negative inertia,
+ # and having no throttle negative inertia. A value of 0 is full throttle
+ # inertia. A value of 1 is no throttle negative inertia.
+ self.ttrust = 1.0
- self.left_gear = VelocityDrivetrain.LOW
- self.right_gear = VelocityDrivetrain.LOW
- self.left_shifter_position = 0.0
- self.right_shifter_position = 0.0
- self.left_cim = CIM()
- self.right_cim = CIM()
+ self.left_gear = VelocityDrivetrain.LOW
+ self.right_gear = VelocityDrivetrain.LOW
+ self.left_shifter_position = 0.0
+ self.right_shifter_position = 0.0
+ self.left_cim = CIM()
+ self.right_cim = CIM()
- def IsInGear(self, gear):
- return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+ def IsInGear(self, gear):
+ return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
- def MotorRPM(self, shifter_position, velocity):
- if shifter_position > 0.5:
- return (velocity / self.CurrentDrivetrain().G_high /
- self.CurrentDrivetrain().r)
- else:
- return (velocity / self.CurrentDrivetrain().G_low /
- self.CurrentDrivetrain().r)
-
- def CurrentDrivetrain(self):
- if self.left_shifter_position > 0.5:
- if self.right_shifter_position > 0.5:
- return self.drivetrain_high_high
- else:
- return self.drivetrain_high_low
- else:
- if self.right_shifter_position > 0.5:
- return self.drivetrain_low_high
- else:
- return self.drivetrain_low_low
-
- def SimShifter(self, gear, shifter_position):
- if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
- shifter_position = min(shifter_position + 0.5, 1.0)
- else:
- shifter_position = max(shifter_position - 0.5, 0.0)
-
- if shifter_position == 1.0:
- gear = VelocityDrivetrain.HIGH
- elif shifter_position == 0.0:
- gear = VelocityDrivetrain.LOW
-
- return gear, shifter_position
-
- def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
- high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
- self.CurrentDrivetrain().r)
- low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
- self.CurrentDrivetrain().r)
- #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
- high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
- low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
- high_power = high_torque * high_omega
- low_power = low_torque * low_omega
- #if should_print:
- # print gear_name, "High omega", high_omega, "Low omega", low_omega
- # print gear_name, "High torque", high_torque, "Low torque", low_torque
- # print gear_name, "High power", high_power, "Low power", low_power
-
- # Shift algorithm improvements.
- # TODO(aschuh):
- # It takes time to shift. Shifting down for 1 cycle doesn't make sense
- # because you will end up slower than without shifting. Figure out how
- # to include that info.
- # If the driver is still in high gear, but isn't asking for the extra power
- # from low gear, don't shift until he asks for it.
- goal_gear_is_high = high_power > low_power
- #goal_gear_is_high = True
-
- if not self.IsInGear(current_gear):
- glog.debug('%s Not in gear.', gear_name)
- return current_gear
- else:
- is_high = current_gear is VelocityDrivetrain.HIGH
- if is_high != goal_gear_is_high:
- if goal_gear_is_high:
- glog.debug('%s Shifting up.', gear_name)
- return VelocityDrivetrain.SHIFTING_UP
+ def MotorRPM(self, shifter_position, velocity):
+ if shifter_position > 0.5:
+ return (velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
else:
- glog.debug('%s Shifting down.', gear_name)
- return VelocityDrivetrain.SHIFTING_DOWN
- else:
- return current_gear
+ return (velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
- def FilterVelocity(self, throttle):
- # Invert the plant to figure out how the velocity filter would have to work
- # out in order to filter out the forwards negative inertia.
- # This math assumes that the left and right power and velocity are equal.
+ def CurrentDrivetrain(self):
+ if self.left_shifter_position > 0.5:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_high_high
+ else:
+ return self.drivetrain_high_low
+ else:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_low_high
+ else:
+ return self.drivetrain_low_low
- # The throttle filter should filter such that the motor in the highest gear
- # should be controlling the time constant.
- # Do this by finding the index of FF that has the lowest value, and computing
- # the sums using that index.
- FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
- min_FF_sum_index = numpy.argmin(FF_sum)
- min_FF_sum = FF_sum[min_FF_sum_index, 0]
- min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
- # Compute the FF sum for high gear.
- high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+ def SimShifter(self, gear, shifter_position):
+ if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+ shifter_position = min(shifter_position + 0.5, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.5, 0.0)
- # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
- # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
- # - self.K[0, :].sum() * x_avg
+ if shifter_position == 1.0:
+ gear = VelocityDrivetrain.HIGH
+ elif shifter_position == 0.0:
+ gear = VelocityDrivetrain.LOW
- # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
- # (self.K[0, :].sum() + self.FF[0, :].sum())
+ return gear, shifter_position
- # U = (K + FF) * R - K * X
- # (K + FF) ^-1 * (U + K * X) = R
+ def ComputeGear(self,
+ wheel_velocity,
+ should_print=False,
+ current_gear=False,
+ gear_name=None):
+ high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+ #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+ high_torque = (
+ (12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ low_torque = (
+ (12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ high_power = high_torque * high_omega
+ low_power = low_torque * low_omega
+ #if should_print:
+ # print gear_name, "High omega", high_omega, "Low omega", low_omega
+ # print gear_name, "High torque", high_torque, "Low torque", low_torque
+ # print gear_name, "High power", high_power, "Low power", low_power
- # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
- # have the same velocity goal as high gear, and so that the robot will hold
- # the same speed for the same throttle for all gears.
- adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
- return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
- / (self.ttrust * min_K_sum + min_FF_sum))
+ # Shift algorithm improvements.
+ # TODO(aschuh):
+ # It takes time to shift. Shifting down for 1 cycle doesn't make sense
+ # because you will end up slower than without shifting. Figure out how
+ # to include that info.
+ # If the driver is still in high gear, but isn't asking for the extra power
+ # from low gear, don't shift until he asks for it.
+ goal_gear_is_high = high_power > low_power
+ #goal_gear_is_high = True
- def Update(self, throttle, steering):
- # Shift into the gear which sends the most power to the floor.
- # This is the same as sending the most torque down to the floor at the
- # wheel.
+ if not self.IsInGear(current_gear):
+ glog.debug('%s Not in gear.', gear_name)
+ return current_gear
+ else:
+ is_high = current_gear is VelocityDrivetrain.HIGH
+ if is_high != goal_gear_is_high:
+ if goal_gear_is_high:
+ glog.debug('%s Shifting up.', gear_name)
+ return VelocityDrivetrain.SHIFTING_UP
+ else:
+ glog.debug('%s Shifting down.', gear_name)
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
- self.left_gear = self.right_gear = True
- if True:
- self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
- current_gear=self.left_gear,
- gear_name="left")
- self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
- current_gear=self.right_gear,
- gear_name="right")
- if self.IsInGear(self.left_gear):
- self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ def FilterVelocity(self, throttle):
+ # Invert the plant to figure out how the velocity filter would have to work
+ # out in order to filter out the forwards negative inertia.
+ # This math assumes that the left and right power and velocity are equal.
- if self.IsInGear(self.right_gear):
- self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+ # The throttle filter should filter such that the motor in the highest gear
+ # should be controlling the time constant.
+ # Do this by finding the index of FF that has the lowest value, and computing
+ # the sums using that index.
+ FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ min_FF_sum_index = numpy.argmin(FF_sum)
+ min_FF_sum = FF_sum[min_FF_sum_index, 0]
+ min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+ # Compute the FF sum for high gear.
+ high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
- # Filter the throttle to provide a nicer response.
- fvel = self.FilterVelocity(throttle)
+ # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * x_avg
- # Constant radius means that angualar_velocity / linear_velocity = constant.
- # Compute the left and right velocities.
- steering_velocity = numpy.abs(fvel) * steering
- left_velocity = fvel - steering_velocity
- right_velocity = fvel + steering_velocity
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
- # Write this constraint in the form of K * R = w
- # angular velocity / linear velocity = constant
- # (left - right) / (left + right) = constant
- # left - right = constant * left + constant * right
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
- # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
- # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
- # constant
- # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
- # (-steering * sign(fvel)) = constant
- # (-steering * sign(fvel)) * (left + right) = left - right
- # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+ # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
+ # have the same velocity goal as high gear, and so that the robot will hold
+ # the same speed for the same throttle for all gears.
+ adjusted_ff_voltage = numpy.clip(
+ throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+ return ((adjusted_ff_voltage + self.ttrust * min_K_sum *
+ (self.X[0, 0] + self.X[1, 0]) / 2.0) /
+ (self.ttrust * min_K_sum + min_FF_sum))
- equality_k = numpy.matrix(
- [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
- equality_w = 0.0
+ def Update(self, throttle, steering):
+ # Shift into the gear which sends the most power to the floor.
+ # This is the same as sending the most torque down to the floor at the
+ # wheel.
- self.R[0, 0] = left_velocity
- self.R[1, 0] = right_velocity
+ self.left_gear = self.right_gear = True
+ if True:
+ self.left_gear = self.ComputeGear(
+ self.X[0, 0],
+ should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(
+ self.X[1, 0],
+ should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(
+ self.left_shifter_position, self.X[0, 0])
- # Construct a constraint on R by manipulating the constraint on U
- # Start out with H * U <= k
- # U = FF * R + K * (R - X)
- # H * (FF * R + K * R - K * X) <= k
- # H * (FF + K) * R <= k + H * K * X
- R_poly = polytope.HPolytope(
- self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
- self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(
+ self.right_shifter_position, self.X[0, 0])
- # Limit R back inside the box.
- self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
- FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
- self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
- else:
- glog.debug('Not all in gear')
- if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
- # TODO(austin): Use battery volts here.
- R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
- self.U_ideal[0, 0] = numpy.clip(
- self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
- self.left_cim.U_min, self.left_cim.U_max)
- self.left_cim.Update(self.U_ideal[0, 0])
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
- R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
- self.U_ideal[1, 0] = numpy.clip(
- self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
- self.right_cim.U_min, self.right_cim.U_max)
- self.right_cim.Update(self.U_ideal[1, 0])
- else:
- assert False
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
- self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
- # TODO(austin): Model the robot as not accelerating when you shift...
- # This hack only works when you shift at the same time.
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
- self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+ equality_k = numpy.matrix([[
+ 1 + steering * numpy.sign(fvel),
+ -(1 - steering * numpy.sign(fvel))
+ ]])
+ equality_w = 0.0
- self.left_gear, self.left_shifter_position = self.SimShifter(
- self.left_gear, self.left_shifter_position)
- self.right_gear, self.right_shifter_position = self.SimShifter(
- self.right_gear, self.right_shifter_position)
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
- glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
- glog.debug('Left shifter %s %d Right shifter %s %d',
- self.left_gear, self.left_shifter_position,
- self.right_gear, self.right_shifter_position)
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H *
+ (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+ self.U_poly.k +
+ self.U_poly.H * self.CurrentDrivetrain().K * self.X)
-def WritePolyDrivetrain(drivetrain_files, motor_files, hybrid_files,
- year_namespace, drivetrain_params,
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+ self.U_ideal = self.CurrentDrivetrain().K * (
+ self.boxed_R - self.X) + FF_volts
+ else:
+ glog.debug('Not all in gear')
+ if not self.IsInGear(self.left_gear) and not self.IsInGear(
+ self.right_gear):
+ # TODO(austin): Use battery volts here.
+ R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.U_ideal[0, 0] = numpy.clip(
+ self.left_cim.K * (R_left - self.left_cim.X) +
+ R_left / self.left_cim.Kv, self.left_cim.U_min,
+ self.left_cim.U_max)
+ self.left_cim.Update(self.U_ideal[0, 0])
+
+ R_right = self.MotorRPM(self.right_shifter_position,
+ self.X[1, 0])
+ self.U_ideal[1, 0] = numpy.clip(
+ self.right_cim.K * (R_right - self.right_cim.X) +
+ R_right / self.right_cim.Kv, self.right_cim.U_min,
+ self.right_cim.U_max)
+ self.right_cim.Update(self.U_ideal[1, 0])
+ else:
+ assert False
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+ # TODO(austin): Model the robot as not accelerating when you shift...
+ # This hack only works when you shift at the same time.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ self.X = self.CurrentDrivetrain(
+ ).A * self.X + self.CurrentDrivetrain().B * self.U
+
+ self.left_gear, self.left_shifter_position = self.SimShifter(
+ self.left_gear, self.left_shifter_position)
+ self.right_gear, self.right_shifter_position = self.SimShifter(
+ self.right_gear, self.right_shifter_position)
+
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d', self.left_gear,
+ self.left_shifter_position, self.right_gear,
+ self.right_shifter_position)
+
+
+def WritePolyDrivetrain(drivetrain_files,
+ motor_files,
+ hybrid_files,
+ year_namespace,
+ drivetrain_params,
scalar_type='double'):
- vdrivetrain = VelocityDrivetrain(drivetrain_params)
- hybrid_vdrivetrain = VelocityDrivetrain(drivetrain_params,
- name="HybridVelocityDrivetrain")
- if isinstance(year_namespace, list):
- namespaces = year_namespace
- else:
- namespaces = [year_namespace, 'control_loops', 'drivetrain']
- dog_loop_writer = control_loop.ControlLoopWriter(
- "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high],
- namespaces=namespaces,
- scalar_type=scalar_type)
+ vdrivetrain = VelocityDrivetrain(drivetrain_params)
+ hybrid_vdrivetrain = VelocityDrivetrain(
+ drivetrain_params, name="HybridVelocityDrivetrain")
+ if isinstance(year_namespace, list):
+ namespaces = year_namespace
+ else:
+ namespaces = [year_namespace, 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [
+ vdrivetrain.drivetrain_low_low, vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low, vdrivetrain.drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type)
- dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
+ dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
- hybrid_loop_writer = control_loop.ControlLoopWriter(
- "HybridVelocityDrivetrain", [hybrid_vdrivetrain.drivetrain_low_low,
- hybrid_vdrivetrain.drivetrain_low_high,
- hybrid_vdrivetrain.drivetrain_high_low,
- hybrid_vdrivetrain.drivetrain_high_high],
- namespaces=namespaces,
- scalar_type=scalar_type,
- plant_type='StateFeedbackHybridPlant',
- observer_type='HybridKalman')
+ hybrid_loop_writer = control_loop.ControlLoopWriter(
+ "HybridVelocityDrivetrain", [
+ hybrid_vdrivetrain.drivetrain_low_low,
+ hybrid_vdrivetrain.drivetrain_low_high,
+ hybrid_vdrivetrain.drivetrain_high_low,
+ hybrid_vdrivetrain.drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
- hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
+ hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()], scalar_type=scalar_type)
+ cim_writer = control_loop.ControlLoopWriter(
+ "CIM", [CIM()], scalar_type=scalar_type)
- cim_writer.Write(motor_files[0], motor_files[1])
+ cim_writer.Write(motor_files[0], motor_files[1])
+
def PlotPolyDrivetrainMotions(drivetrain_params):
- vdrivetrain = VelocityDrivetrain(drivetrain_params)
- vl_plot = []
- vr_plot = []
- ul_plot = []
- ur_plot = []
- radius_plot = []
- t_plot = []
- left_gear_plot = []
- right_gear_plot = []
- vdrivetrain.left_shifter_position = 0.0
- vdrivetrain.right_shifter_position = 0.0
- vdrivetrain.left_gear = VelocityDrivetrain.LOW
- vdrivetrain.right_gear = VelocityDrivetrain.LOW
+ vdrivetrain = VelocityDrivetrain(drivetrain_params)
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ left_gear_plot = []
+ right_gear_plot = []
+ vdrivetrain.left_shifter_position = 0.0
+ vdrivetrain.right_shifter_position = 0.0
+ vdrivetrain.left_gear = VelocityDrivetrain.LOW
+ vdrivetrain.right_gear = VelocityDrivetrain.LOW
- glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
- if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
- glog.debug('Left is high')
- else:
- glog.debug('Left is low')
- if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
- glog.debug('Right is high')
- else:
- glog.debug('Right is low')
-
- for t in numpy.arange(0, 1.7, vdrivetrain.dt):
- if t < 0.5:
- vdrivetrain.Update(throttle=0.00, steering=1.0)
- elif t < 1.2:
- vdrivetrain.Update(throttle=0.5, steering=1.0)
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Left is high')
else:
- vdrivetrain.Update(throttle=0.00, steering=1.0)
- t_plot.append(t)
- vl_plot.append(vdrivetrain.X[0, 0])
- vr_plot.append(vdrivetrain.X[1, 0])
- ul_plot.append(vdrivetrain.U[0, 0])
- ur_plot.append(vdrivetrain.U[1, 0])
- left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
- right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
-
- fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
- turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
- if abs(fwd_velocity) < 0.0000001:
- radius_plot.append(turn_velocity)
+ glog.debug('Left is low')
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Right is high')
else:
- radius_plot.append(turn_velocity / fwd_velocity)
+ glog.debug('Right is low')
- # TODO(austin):
- # Shifting compensation.
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
+ else:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ t_plot.append(t)
+ vl_plot.append(vdrivetrain.X[0, 0])
+ vr_plot.append(vdrivetrain.X[1, 0])
+ ul_plot.append(vdrivetrain.U[0, 0])
+ ur_plot.append(vdrivetrain.U[1, 0])
+ left_gear_plot.append(
+ (vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+ right_gear_plot.append(
+ (vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
- # Tighten the turn.
- # Closed loop drive.
+ fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+ turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+ if abs(fwd_velocity) < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
- pylab.plot(t_plot, vl_plot, label='left velocity')
- pylab.plot(t_plot, vr_plot, label='right velocity')
- pylab.plot(t_plot, ul_plot, label='left voltage')
- pylab.plot(t_plot, ur_plot, label='right voltage')
- pylab.plot(t_plot, radius_plot, label='radius')
- pylab.plot(t_plot, left_gear_plot, label='left gear high')
- pylab.plot(t_plot, right_gear_plot, label='right gear high')
- pylab.legend()
- pylab.show()
+ # TODO(austin):
+ # Shifting compensation.
+
+ # Tighten the turn.
+ # Closed loop drive.
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left voltage')
+ pylab.plot(t_plot, ur_plot, label='right voltage')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.plot(t_plot, left_gear_plot, label='left gear high')
+ pylab.plot(t_plot, right_gear_plot, label='right gear high')
+ pylab.legend()
+ pylab.show()
diff --git a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
index caa3082..839677e 100644
--- a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
+++ b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
@@ -39,11 +39,9 @@
# Write the generated constants out to a file.
if len(argv) != 5:
- glog.fatal(
- 'Expected .h file name and .cc file name for the \
+ glog.fatal('Expected .h file name and .cc file name for the \
static_zeroing_single_dof_profiled_subsystem_test and integral \
- static_zeroing_single_dof_profiled_subsystem_test.'
- )
+ static_zeroing_single_dof_profiled_subsystem_test.')
else:
namespaces = ['frc971', 'control_loops']
linear_system.WriteLinearSystem(kIntake, argv[1:3], argv[3:5],