Run yapf on all python files in the repo
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I221e04c3f517fab8535b22551553799e0fee7a80
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index df6c0f6..6772e71 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,
@@ -20,7 +21,7 @@
kalman_q_vel,
kalman_q_voltage,
kalman_r_position,
- radius = None,
+ radius=None,
dt=0.00505):
"""Constructs an AngularSystemParams object.
@@ -44,6 +45,7 @@
class AngularSystem(control_loop.ControlLoop):
+
def __init__(self, params, name="AngularSystem"):
super(AngularSystem, self).__init__(name)
self.params = params
@@ -61,8 +63,8 @@
# 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]])
@@ -86,8 +88,9 @@
if self.params.radius is not None:
glog.debug('Stall force: %f (N)',
self.motor.stall_torque / self.G / self.params.radius)
- glog.debug('Stall force: %f (lbf)',
- self.motor.stall_torque / self.G / self.params.radius * 0.224809)
+ glog.debug(
+ 'Stall force: %f (lbf)', self.motor.stall_torque / self.G /
+ self.params.radius * 0.224809)
glog.debug('Stall acceleration: %f (rad/s^2)',
self.motor.stall_torque / self.G / self.J)
@@ -117,8 +120,11 @@
self.R = numpy.matrix([[(self.params.kalman_r_position**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.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
glog.debug('Kal %s', repr(self.KalmanGain))
@@ -131,6 +137,7 @@
class IntegralAngularSystem(AngularSystem):
+
def __init__(self, params, name="IntegralAngularSystem"):
super(IntegralAngularSystem, self).__init__(params, name=name)
@@ -153,13 +160,16 @@
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)]])
+ [0.0, 0.0,
+ (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**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.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.K_unaugmented = self.K
self.K = numpy.matrix(numpy.zeros((1, 3)))
@@ -232,10 +242,10 @@
offset_plot.append(observer.X_hat[2, 0])
x_hat_plot.append(observer.X_hat[0, 0])
- next_goal = numpy.concatenate(
- (profile.Update(end_goal[0, 0], end_goal[1, 0]),
- numpy.matrix(numpy.zeros((1, 1)))),
- axis=0)
+ next_goal = numpy.concatenate((profile.Update(
+ end_goal[0, 0], end_goal[1, 0]), numpy.matrix(numpy.zeros(
+ (1, 1)))),
+ axis=0)
ff_U = controller.Kff * (next_goal - observer.A * goal)
@@ -252,8 +262,8 @@
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- motor_current = (U[0, 0] - plant.X[1, 0] / plant.G / plant.motor.Kv
- ) / plant.motor.resistance
+ motor_current = (U[0, 0] - plant.X[1, 0] / plant.G /
+ plant.motor.Kv) / plant.motor.resistance
motor_current_plot.append(motor_current)
battery_current = U[0, 0] * motor_current / 12.0
battery_current_plot.append(battery_current)
@@ -281,8 +291,8 @@
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))
@@ -330,15 +340,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=0.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=0.0)
def PlotKick(params, R, plant_params=None):
@@ -357,15 +366,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=2.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=2.0)
def PlotMotion(params,
@@ -391,15 +399,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=True,
- max_velocity=max_velocity,
- max_acceleration=max_acceleration)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=True,
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteAngularSystem(params, plant_files, controller_files, year_namespaces):
@@ -431,8 +438,9 @@
integral_angular_systems.append(
IntegralAngularSystem(params, 'Integral' + params.name))
- loop_writer = control_loop.ControlLoopWriter(
- name, angular_systems, namespaces=year_namespaces)
+ loop_writer = control_loop.ControlLoopWriter(name,
+ angular_systems,
+ namespaces=year_namespaces)
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', angular_systems[0].G))
loop_writer.AddConstant(
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index b8d49a9..44e4f49 100755
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -1,5 +1,6 @@
#!/usr/bin/python3
import gi
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
from gi.repository import GLib
@@ -13,6 +14,7 @@
# Override the matrix of a cairo context.
class OverrideMatrix(object):
+
def __init__(self, cr, matrix):
self.cr = cr
self.matrix = matrix
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index ac527de..f8a6e9a 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -5,6 +5,7 @@
class CIM(control_loop.ControlLoop):
+
def __init__(self):
super(CIM, self).__init__("CIM")
# Stall Torque in N m
diff --git a/frc971/control_loops/python/color.py b/frc971/control_loops/python/color.py
index 5634042..a66040c 100644
--- a/frc971/control_loops/python/color.py
+++ b/frc971/control_loops/python/color.py
@@ -1,4 +1,5 @@
class Color:
+
def __init__(self, r, g, b, a=1.0):
self.r = r
self.g = g
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index e038cb2..b2d9d57 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -17,8 +17,7 @@
FieldType = namedtuple(
'Field', ['name', 'tags', 'year', 'width', 'length', 'robot', 'field_id'])
-RobotType = namedtuple(
- "Robot", ['width', 'length'])
+RobotType = namedtuple("Robot", ['width', 'length'])
GALACTIC_SEARCH = "Galactic Search"
ARED = "A Red"
@@ -31,101 +30,91 @@
BARREL = "Barrel"
Robot2019 = RobotType(width=0.65, length=0.8)
-Robot2020 = RobotType(width=0.8128, length=0.8636) # 32 in x 34 in
+Robot2020 = RobotType(width=0.8128, length=0.8636) # 32 in x 34 in
Robot2021 = Robot2020
Robot2022 = RobotType(width=0.8763, length=0.96647)
FIELDS = {
"2019 Field":
- FieldType(
- "2019 Field",
- tags=[],
- year=2019,
- width=8.258302,
- length=8.258302,
- robot=Robot2019,
- field_id="2019"),
+ FieldType("2019 Field",
+ tags=[],
+ year=2019,
+ width=8.258302,
+ length=8.258302,
+ robot=Robot2019,
+ field_id="2019"),
"2020 Field":
- FieldType(
- "2020 Field",
- tags=[],
- year=2020,
- width=15.98295,
- length=8.21055,
- robot=Robot2020,
- field_id="2020"),
+ FieldType("2020 Field",
+ tags=[],
+ year=2020,
+ width=15.98295,
+ length=8.21055,
+ robot=Robot2020,
+ field_id="2020"),
"2021 Galactic Search BRed":
- FieldType(
- "2021 Galactic Search BRed",
- tags=[GALACTIC_SEARCH, BRED],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="red_b"),
+ FieldType("2021 Galactic Search BRed",
+ tags=[GALACTIC_SEARCH, BRED],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="red_b"),
"2021 Galactic Search ARed":
- FieldType(
- "2021 Galactic Search ARed",
- tags=[GALACTIC_SEARCH, ARED],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="red_a"),
+ FieldType("2021 Galactic Search ARed",
+ tags=[GALACTIC_SEARCH, ARED],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="red_a"),
"2021 Galactic Search BBlue":
- FieldType(
- "2021 Galactic Search BBlue",
- tags=[GALACTIC_SEARCH, BBLUE],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="blue_b"),
+ FieldType("2021 Galactic Search BBlue",
+ tags=[GALACTIC_SEARCH, BBLUE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="blue_b"),
"2021 Galactic Search ABlue":
- FieldType(
- "2021 Galactic Search ABlue",
- tags=[GALACTIC_SEARCH, ABLUE],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="blue_a"),
+ FieldType("2021 Galactic Search ABlue",
+ tags=[GALACTIC_SEARCH, ABLUE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="blue_a"),
"2021 AutoNav Barrel":
- FieldType(
- "2021 AutoNav Barrel",
- tags=[AUTONAV, BARREL],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="autonav_barrel"),
+ FieldType("2021 AutoNav Barrel",
+ tags=[AUTONAV, BARREL],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="autonav_barrel"),
"2021 AutoNav Slalom":
- FieldType(
- "2021 AutoNav Slalom",
- tags=[AUTONAV, SLALOM],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="autonav_slalom"),
+ FieldType("2021 AutoNav Slalom",
+ tags=[AUTONAV, SLALOM],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="autonav_slalom"),
"2021 AutoNav Bounce":
- FieldType(
- "2021 AutoNav Bounce",
- tags=[AUTONAV, BOUNCE],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="autonav_bounce"),
+ FieldType("2021 AutoNav Bounce",
+ tags=[AUTONAV, BOUNCE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="autonav_bounce"),
"2022 Field":
- FieldType(
- "2022 Field",
- tags=[],
- year=2022,
- width=16.4592,
- length=8.2296,
- robot=Robot2022,
- field_id="2022"),
+ FieldType("2022 Field",
+ tags=[],
+ year=2022,
+ width=16.4592,
+ length=8.2296,
+ robot=Robot2022,
+ field_id="2022"),
}
FIELD = FIELDS["2022 Field"]
@@ -139,5 +128,6 @@
else:
return "frc971/control_loops/python/spline_jsons"
+
def inToM(i):
return (i * 0.0254)
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 1649dd2..e836d1a 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -4,6 +4,7 @@
class Constant(object):
+
def __init__(self, name, formatt, value, comment=None):
self.name = name
self.formatt = formatt
@@ -25,6 +26,7 @@
class ControlLoopWriter(object):
+
def __init__(self,
gain_schedule_name,
loops,
@@ -76,8 +78,9 @@
return self._namespaces[0]
def _HeaderGuard(self, header_file):
- return ('_'.join([namespace.upper() for namespace in self._namespaces])
- + '_' + os.path.basename(header_file).upper().replace(
+ return ('_'.join([namespace.upper()
+ for namespace in self._namespaces]) + '_' +
+ os.path.basename(header_file).upper().replace(
'.', '_').replace('/', '_') + '_')
def Write(self, header_file, cc_file):
@@ -163,17 +166,17 @@
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')
@@ -182,8 +185,8 @@
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('#include \"%s/%s\"\n' %
+ (os.path.join(*self._namespaces), header_file_name))
fd.write('\n')
fd.write('#include <chrono>\n')
fd.write('#include <vector>\n')
@@ -208,19 +211,20 @@
self._scalar_type))
fd.write('\n')
- fd.write('%s Make%sPlant() {\n' % (self._PlantType(),
- self._gain_schedule_name))
+ 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(
+ ' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
+ loop.PlantFunction()))
fd.write(' return %s(std::move(plants));\n' % self._PlantType())
fd.write('}\n\n')
- fd.write('%s Make%sController() {\n' % (self._ControllerType(),
- self._gain_schedule_name))
+ 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)))
@@ -233,21 +237,22 @@
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)))
+ 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(std::move(observers));\n' % self._ObserverType())
+ ' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ObserverCoeffType(),
+ self._ObserverCoeffType(), loop.ObserverFunction()))
+ fd.write(' return %s(std::move(observers));\n' %
+ self._ObserverType())
fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
- self._gain_schedule_name))
+ 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,
@@ -259,6 +264,7 @@
class ControlLoop(object):
+
def __init__(self, name):
"""Constructs a control loop object.
@@ -291,7 +297,8 @@
self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
- self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
+ self.last_U = numpy.matrix(
+ numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
@@ -368,8 +375,8 @@
if '.' not in write_type and 'e' not in write_type:
write_type += '.0'
write_type += 'f'
- ans.append(
- ' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
+ ans.append(' %s(%d, %d) = %s;\n' %
+ (matrix_name, x, y, write_type))
return ''.join(ans)
@@ -389,8 +396,8 @@
string, The function which will create the object.
"""
ans = [
- '%s Make%sPlantCoefficients() {\n' % (plant_coefficient_type,
- self._name)
+ '%s Make%sPlantCoefficients() {\n' %
+ (plant_coefficient_type, self._name)
]
ans.append(self._DumpMatrix('C', self.C, scalar_type))
@@ -402,11 +409,11 @@
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(
- ' const std::chrono::nanoseconds dt(%d);\n' % (self.dt * 1e9))
- ans.append(
- ' return %s'
- '(A, B, C, D, U_max, U_min, dt, %s);\n' % (plant_coefficient_type, delayed_u_string))
+ ans.append(' const std::chrono::nanoseconds dt(%d);\n' %
+ (self.dt * 1e9))
+ ans.append(' return %s'
+ '(A, B, C, D, U_max, U_min, dt, %s);\n' %
+ (plant_coefficient_type, delayed_u_string))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
ans.append(
self._DumpMatrix('A_continuous', self.A_continuous,
@@ -414,9 +421,10 @@
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, %s);\n' %
- (plant_coefficient_type, delayed_u_string))
+ ans.append(
+ ' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min, %s);\n' %
+ (plant_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported plant type %s', plant_coefficient_type)
@@ -521,8 +529,8 @@
self._DumpMatrix('P_steady_state', self.P_steady_state,
scalar_type))
ans.append(
- ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n' %
- (observer_coefficient_type, delayed_u_string))
+ ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n'
+ % (observer_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported observer type %s',
observer_coefficient_type)
@@ -532,6 +540,7 @@
class HybridControlLoop(ControlLoop):
+
def __init__(self, name):
super(HybridControlLoop, self).__init__(name=name)
@@ -569,6 +578,7 @@
class CIM(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 2.42
@@ -581,13 +591,14 @@
# 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
class MiniCIM(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 1.41
@@ -600,8 +611,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
@@ -610,6 +621,7 @@
class NMotor(object):
+
def __init__(self, motor, n):
"""Gangs together n motors."""
self.motor = motor
@@ -625,6 +637,7 @@
class Vex775Pro(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 0.71
@@ -637,8 +650,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
@@ -659,8 +672,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))
+ 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
@@ -668,6 +681,7 @@
class MN3510(object):
+
def __init__(self):
# http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
# Free Current in Amps
@@ -702,8 +716,8 @@
# 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 / (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
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 1dfd061..4cb10a2 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -33,8 +33,8 @@
Returns:
numpy.matrix(m x n), K
"""
- return scipy.signal.place_poles(
- A=A, B=B, poles=numpy.array(poles)).gain_matrix
+ return scipy.signal.place_poles(A=A, B=B,
+ poles=numpy.array(poles)).gain_matrix
def c2d(A, B, dt):
@@ -48,8 +48,8 @@
em_upper = numpy.hstack((a, b))
# Need to stack zeros under the a and b matrices
- em_lower = numpy.hstack((numpy.zeros((b.shape[1], a.shape[0])),
- numpy.zeros((b.shape[1], b.shape[1]))))
+ em_lower = numpy.hstack((numpy.zeros(
+ (b.shape[1], a.shape[0])), numpy.zeros((b.shape[1], b.shape[1]))))
em = numpy.vstack((em_upper, em_lower))
ms = scipy.linalg.expm(dt * em)
@@ -164,8 +164,8 @@
axis=0)
phi = numpy.matrix(scipy.linalg.expm(M * dt))
phi12 = phi[0:number_of_states, number_of_states:(2 * number_of_states)]
- phi22 = phi[number_of_states:2 * number_of_states, number_of_states:2 *
- number_of_states]
+ phi22 = phi[number_of_states:2 * number_of_states,
+ number_of_states:2 * number_of_states]
Q_discrete = phi22.T * phi12
Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
R_discrete = R_continuous / dt
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 84cdfa9..956a2b3 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -18,6 +18,7 @@
class DownEstimator(control_loop.ControlLoop):
+
def __init__(self, name='DownEstimator'):
super(DownEstimator, self).__init__(name)
self.dt = 0.005
@@ -47,8 +48,11 @@
self.U_min = numpy.matrix([[-numpy.pi]])
self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ 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
@@ -93,8 +97,8 @@
angle = math.pi / 2
velocity = 1
for i in range(100):
- measured_velocity = velocity + (
- random.random() - 0.5) * 0.01 + 0.05
+ 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,
@@ -115,8 +119,9 @@
glog.error("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 = control_loop.ControlLoopWriter("DownEstimator",
+ [estimator],
+ namespaces=namespaces)
kf_loop_writer.Write(argv[1], argv[2])
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index 644e449..431abd7 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -58,7 +58,8 @@
cr.show_text(text)
cr.scale(widthb, -heightb)
+
def draw_points(cr, p, size):
for i in range(0, len(p)):
- draw_px_cross(cr, p[i][0], p[i][1], size, Color(
- 0, np.sqrt(0.2 * i), 0))
+ draw_px_cross(cr, p[i][0], p[i][1], size,
+ Color(0, np.sqrt(0.2 * i), 0))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 264b4a6..80a4a53 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -9,6 +9,7 @@
class DrivetrainParams(object):
+
def __init__(self,
J,
mass,
@@ -109,6 +110,7 @@
class Drivetrain(control_loop.ControlLoop):
+
def __init__(self,
drivetrain_params,
name="Drivetrain",
@@ -192,8 +194,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
@@ -206,10 +208,10 @@
self.msnr = self.robot_radius_l / ( self.robot_radius_r * self.mass ) - \
self.robot_radius_l * self.robot_radius_r / self.J
# The calculations which we will need for A and B.
- self.tcl = self.Kt / self.Kv / (
- self.Gl * self.Gl * self.resistance * self.r * self.r)
- self.tcr = self.Kt / self.Kv / (
- self.Gr * self.Gr * self.resistance * self.r * self.r)
+ self.tcl = self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance *
+ self.r * self.r)
+ self.tcr = self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance *
+ self.r * self.r)
self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
@@ -268,6 +270,7 @@
class KFDrivetrain(Drivetrain):
+
def __init__(self,
drivetrain_params,
name="KFDrivetrain",
@@ -323,31 +326,36 @@
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.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]])
@@ -378,14 +386,17 @@
[0.0, 0.0, (r_gyro**2.0)]])
# Solving for kf gains.
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
# 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))
@@ -415,8 +426,8 @@
self.Qff[2, 2] = 1.0 / qff_pos**2.0
self.Qff[3, 3] = 1.0 / qff_vel**2.0
self.Kff = numpy.matrix(numpy.zeros((2, 7)))
- self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(
- self.B[0:4, :], self.Qff)
+ self.Kff[0:2,
+ 0:4] = controls.TwoStateFeedForwards(self.B[0:4, :], self.Qff)
self.InitializeState()
@@ -428,59 +439,50 @@
scalar_type='double'):
# Write the generated constants out to a file.
- drivetrain_low_low = Drivetrain(
- name="DrivetrainLowLow",
- left_low=True,
- right_low=True,
- drivetrain_params=drivetrain_params)
- drivetrain_low_high = Drivetrain(
- name="DrivetrainLowHigh",
- left_low=True,
- right_low=False,
- drivetrain_params=drivetrain_params)
- drivetrain_high_low = Drivetrain(
- name="DrivetrainHighLow",
- left_low=False,
- right_low=True,
- drivetrain_params=drivetrain_params)
- drivetrain_high_high = Drivetrain(
- name="DrivetrainHighHigh",
- left_low=False,
- right_low=False,
- drivetrain_params=drivetrain_params)
+ drivetrain_low_low = Drivetrain(name="DrivetrainLowLow",
+ left_low=True,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh",
+ left_low=True,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ drivetrain_high_low = Drivetrain(name="DrivetrainHighLow",
+ left_low=False,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh",
+ left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
- kf_drivetrain_low_low = KFDrivetrain(
- name="KFDrivetrainLowLow",
- left_low=True,
- right_low=True,
- drivetrain_params=drivetrain_params)
- kf_drivetrain_low_high = KFDrivetrain(
- name="KFDrivetrainLowHigh",
- left_low=True,
- right_low=False,
- drivetrain_params=drivetrain_params)
- kf_drivetrain_high_low = KFDrivetrain(
- name="KFDrivetrainHighLow",
- left_low=False,
- right_low=True,
- drivetrain_params=drivetrain_params)
- kf_drivetrain_high_high = KFDrivetrain(
- name="KFDrivetrainHighHigh",
- left_low=False,
- right_low=False,
- drivetrain_params=drivetrain_params)
+ kf_drivetrain_low_low = KFDrivetrain(name="KFDrivetrainLowLow",
+ left_low=True,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_low_high = KFDrivetrain(name="KFDrivetrainLowHigh",
+ left_low=True,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_high_low = KFDrivetrain(name="KFDrivetrainHighLow",
+ left_low=False,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_high_high = KFDrivetrain(name="KFDrivetrainHighHigh",
+ left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
if isinstance(year_namespace, list):
namespaces = year_namespace
else:
namespaces = [year_namespace, 'control_loops', 'drivetrain']
- dog_loop_writer = control_loop.ControlLoopWriter(
- "Drivetrain", [
- drivetrain_low_low, drivetrain_low_high, drivetrain_high_low,
- drivetrain_high_high
- ],
- namespaces=namespaces,
- scalar_type=scalar_type)
+ dog_loop_writer = control_loop.ControlLoopWriter("Drivetrain", [
+ drivetrain_low_low, drivetrain_low_high, drivetrain_high_low,
+ drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type)
dog_loop_writer.AddConstant(
control_loop.Constant("kDt", "%f", drivetrain_low_low.dt))
dog_loop_writer.AddConstant(
@@ -522,20 +524,20 @@
dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
- kf_loop_writer = control_loop.ControlLoopWriter(
- "KFDrivetrain", [
- kf_drivetrain_low_low, kf_drivetrain_low_high,
- kf_drivetrain_high_low, kf_drivetrain_high_high
- ],
- namespaces=namespaces,
- scalar_type=scalar_type)
+ kf_loop_writer = control_loop.ControlLoopWriter("KFDrivetrain", [
+ kf_drivetrain_low_low, kf_drivetrain_low_high, kf_drivetrain_high_low,
+ kf_drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type)
kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
def PlotDrivetrainMotions(drivetrain_params):
# Test out the voltage error.
- drivetrain = KFDrivetrain(
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
close_loop_left = []
close_loop_right = []
left_power = []
@@ -562,8 +564,9 @@
pylab.show()
# Simulate the response of the system to a step input.
- drivetrain = KFDrivetrain(
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
simulated_left = []
simulated_right = []
for _ in range(100):
@@ -579,8 +582,9 @@
pylab.show()
# Simulate forwards motion.
- drivetrain = KFDrivetrain(
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
close_loop_left = []
close_loop_right = []
left_power = []
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index b85b996..178b63d 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -1,4 +1,5 @@
import gi
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
import numpy as np
@@ -14,6 +15,7 @@
class Graph(Gtk.Bin):
+
def __init__(self):
super(Graph, self).__init__()
fig = Figure(figsize=(5, 4), dpi=100)
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 7221870..99a2d60 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -22,6 +22,7 @@
class SystemParams(object):
+
def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
q_torque, current_limit):
self.J = J
@@ -45,29 +46,28 @@
#[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)
+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)
@@ -107,6 +107,7 @@
class IntegralHapticInput(HapticInput):
+
def __init__(self, params=None, name="IntegralHapticInput"):
super(IntegralHapticInput, self).__init__(name=name, params=params)
@@ -133,8 +134,11 @@
self.R = numpy.matrix([[(params.r_pos**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.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.K_unaugmented = self.K
@@ -238,9 +242,8 @@
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)
+ 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
@@ -292,10 +295,9 @@
ax2.plot(data_time, data_request_current, label='request_current')
for i, kf_torque in enumerate(kf_torques):
- ax2.plot(
- data_time,
- kf_torque,
- label='-kf_torque[%f]' % haptic_observers[i].J)
+ 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)
@@ -374,20 +376,18 @@
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)
+ 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,
@@ -403,17 +403,16 @@
else:
namespaces = ['frc971', 'control_loops', 'drivetrain']
for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
- ('HapticTrigger', kTrigger,
- argv[5:9])]:
+ ('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 = 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_haptic_input = IntegralHapticInput(params=params,
+ name='Integral' + name)
integral_loop_writer = control_loop.ControlLoopWriter(
'Integral' + name, [integral_haptic_input],
namespaces=namespaces,
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index de44d72..7413896 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -9,6 +9,7 @@
class TestSpline(unittest.TestCase):
+
def testSimpleSpline(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
[2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
@@ -18,6 +19,7 @@
class TestDistanceSpline(unittest.TestCase):
+
def testDistanceSpline(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
[2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
@@ -27,6 +29,7 @@
class TestTrajectory(unittest.TestCase):
+
def testTrajectory(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 5.0],
[2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 9ba6f4f..5aded23 100755
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -46,9 +46,8 @@
assert points.shape == (2, 6)
self.__points = points
self.__spline = ct.c_void_p(
- libSpline.NewSpline(
- np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1])))
+ libSpline.NewSpline(np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1])))
def __del__(self):
libSpline.deleteSpline(self.__spline)
@@ -58,9 +57,8 @@
self.__points[1, index] = y
libSpline.deleteSpline(self.__spline)
self.__spline = ct.c_void_p(
- libSpline.newSpline(
- np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1])))
+ libSpline.newSpline(np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1])))
def Point(self, alpha):
result = np.zeros(2)
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 0391d93..3bda4e1 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
@@ -56,11 +58,11 @@
# 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)
+ 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.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -109,8 +111,11 @@
self.R = numpy.matrix([[(self.params.kalman_r_position**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.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
glog.debug('Kal %s', repr(self.KalmanGain))
@@ -123,6 +128,7 @@
class IntegralLinearSystem(LinearSystem):
+
def __init__(self, params, name='IntegralLinearSystem'):
super(IntegralLinearSystem, self).__init__(params, name=name)
@@ -145,13 +151,16 @@
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)]])
+ [0.0, 0.0,
+ (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**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.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.K_unaugmented = self.K
self.K = numpy.matrix(numpy.zeros((1, 3)))
@@ -222,10 +231,10 @@
offset_plot.append(observer.X_hat[2, 0])
x_hat_plot.append(observer.X_hat[0, 0])
- next_goal = numpy.concatenate(
- (profile.Update(end_goal[0, 0], end_goal[1, 0]),
- numpy.matrix(numpy.zeros((1, 1)))),
- axis=0)
+ next_goal = numpy.concatenate((profile.Update(
+ end_goal[0, 0], end_goal[1, 0]), numpy.matrix(numpy.zeros(
+ (1, 1)))),
+ axis=0)
ff_U = controller.Kff * (next_goal - observer.A * goal)
@@ -264,8 +273,8 @@
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))
@@ -305,15 +314,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=0.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=0.0)
def PlotKick(params, R, plant_params=None):
@@ -332,15 +340,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=2.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=2.0)
def PlotMotion(params,
@@ -366,15 +373,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=True,
- max_velocity=max_velocity,
- max_acceleration=max_acceleration)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=True,
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteLinearSystem(params, plant_files, controller_files, year_namespaces):
@@ -405,8 +411,9 @@
integral_linear_systems.append(
IntegralLinearSystem(params, 'Integral' + params.name))
- loop_writer = control_loop.ControlLoopWriter(
- name, linear_systems, namespaces=year_namespaces)
+ loop_writer = control_loop.ControlLoopWriter(name,
+ linear_systems,
+ namespaces=year_namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f',
linear_systems[0].motor.free_speed))
diff --git a/frc971/control_loops/python/multispline.py b/frc971/control_loops/python/multispline.py
index 1441793..5551587 100644
--- a/frc971/control_loops/python/multispline.py
+++ b/frc971/control_loops/python/multispline.py
@@ -21,6 +21,7 @@
class Multispline():
+
def __init__(self):
self.staged_points = [] # Holds all points not yet in spline
self.libsplines = [] # Formatted for libspline library usage
@@ -97,8 +98,8 @@
# the three points on either side are entirely constrained by this spline
if next_spline is not None:
- f = spline[5] # the end of the spline
- e = spline[4] # determines the heading
+ f = spline[5] # the end of the spline
+ e = spline[4] # determines the heading
d = spline[3]
if next_multispline is None:
next_spline[0] = f
@@ -106,7 +107,9 @@
next_spline[2] = d + f * 4 + e * -4
else:
if snap:
- Multispline.snapSplines(spline, next_spline, match_first_to_second=False)
+ Multispline.snapSplines(spline,
+ next_spline,
+ match_first_to_second=False)
next_spline[0] = f
next_multispline.update_lib_spline()
@@ -121,7 +124,9 @@
prev_spline[3] = c + a * 4 + b * -4
else:
if snap:
- Multispline.snapSplines(prev_spline, spline, match_first_to_second=True)
+ Multispline.snapSplines(prev_spline,
+ spline,
+ match_first_to_second=True)
prev_spline[5] = a
prev_multispline.update_lib_spline()
@@ -189,7 +194,6 @@
spline[index_of_edit] = mouse
-
# all three points move together with the endpoint
if index_of_edit == 5:
spline[3] += diffs
@@ -246,6 +250,7 @@
Returns the closest multispline and the distance along that multispline
"""
+
def distance(t, distance_spline, point):
return np.sum((distance_spline.XY(t) - point)**2)
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 2210f9d..b66a668 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -6,6 +6,7 @@
from graph import Graph
import gi
import numpy as np
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
@@ -29,6 +30,7 @@
class FieldWidget(Gtk.DrawingArea):
"""Create a GTK+ widget on which we will draw using Cairo"""
+
def __init__(self):
super(FieldWidget, self).__init__()
self.set_field(FIELD)
@@ -192,7 +194,8 @@
):
self.draw_splines(cr)
- for i, points in enumerate(self.active_multispline.getSplines()):
+ for i, points in enumerate(
+ self.active_multispline.getSplines()):
points = [np.array([x, y]) for (x, y) in points]
draw_control_points(cr,
points,
@@ -432,7 +435,6 @@
# Move nearest to clicked
cur_p = [self.mousex, self.mousey]
-
# Get the distance between each for x and y
# Save the index of the point closest
nearest = 0.4 # Max distance away a the selected point can be in meters
@@ -452,9 +454,11 @@
index_multisplines, index_splines,
index_points)
- multispline, result = Multispline.nearest_distance(self.multisplines, cur_p)
+ multispline, result = Multispline.nearest_distance(
+ self.multisplines, cur_p)
if result and result.fun < 0.1:
- self.active_multispline_index = self.multisplines.index(multispline)
+ self.active_multispline_index = self.multisplines.index(
+ multispline)
self.queue_draw()
@@ -468,8 +472,7 @@
self.control_point_index.multispline_index]
multispline.updates_for_mouse_move(self.multisplines,
- self.control_point_index,
- mouse)
+ self.control_point_index, mouse)
multispline.update_lib_spline()
self.graph.schedule_recalculate(self.multisplines)
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 39fa33d..a45d738 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -98,6 +98,7 @@
class VelocityDrivetrainModel(control_loop.ControlLoop):
+
def __init__(self,
drivetrain_params,
left_low=True,
@@ -109,25 +110,27 @@
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_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.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)))
@@ -141,20 +144,22 @@
# 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.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.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.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.P_steady_state = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
@@ -277,12 +282,12 @@
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_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:
@@ -357,19 +362,18 @@
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")
+ 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])
+ self.left_cim.X[0,
+ 0] = self.MotorRPM(self.left_shifter_position,
+ self.X[0, 0])
if self.IsInGear(self.right_gear):
self.right_cim.X[0, 0] = self.MotorRPM(
@@ -422,23 +426,23 @@
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
+ 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])
+ 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])
+ 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,
@@ -473,19 +477,18 @@
drivetrain_params,
scalar_type='double'):
vdrivetrain = VelocityDrivetrain(drivetrain_params)
- hybrid_vdrivetrain = VelocityDrivetrain(
- drivetrain_params, name="HybridVelocityDrivetrain")
+ 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 = 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])
@@ -503,8 +506,8 @@
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])
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
index e5b8d5f..e0a8f61 100755
--- a/frc971/control_loops/python/polytope_test.py
+++ b/frc971/control_loops/python/polytope_test.py
@@ -15,6 +15,7 @@
class TestHPolytope(unittest.TestCase):
+
def setUp(self):
"""Builds a simple box polytope."""
self.H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
@@ -43,14 +44,12 @@
]
for inside_point in inside_points:
- self.assertTrue(
- self.p.IsInside(inside_point),
- msg='Point is' + str(inside_point))
+ self.assertTrue(self.p.IsInside(inside_point),
+ msg='Point is' + str(inside_point))
for outside_point in outside_points:
- self.assertFalse(
- self.p.IsInside(outside_point),
- msg='Point is' + str(outside_point))
+ self.assertFalse(self.p.IsInside(outside_point),
+ msg='Point is' + str(outside_point))
def AreVertices(self, p, vertices):
"""Checks that all the vertices are on corners of the set."""
@@ -80,10 +79,10 @@
found_points.add(actual_index)
break
- self.assertEqual(
- len(found_points),
- actual.shape[0],
- msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
+ self.assertEqual(len(found_points),
+ actual.shape[0],
+ msg="Expected:\n" + str(expected) + "\nActual:\n" +
+ str(actual))
def test_Skewed_Nonsym_Vertices(self):
"""Tests the vertices of a severely skewed space."""
diff --git a/frc971/control_loops/python/spline_drawing.py b/frc971/control_loops/python/spline_drawing.py
index 4e8b04b..7bfae29 100644
--- a/frc971/control_loops/python/spline_drawing.py
+++ b/frc971/control_loops/python/spline_drawing.py
@@ -3,6 +3,7 @@
import numpy as np
from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+
WIDTH_OF_FIELD_IN_METERS = 8.258302
PIXELS_ON_SCREEN = 800
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index fad635f..b7bc5ab 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -6,6 +6,7 @@
from path_edit import FieldWidget
from basic_window import RunApp
from constants import FIELDS, FIELD, SCREEN_SIZE
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
@@ -14,7 +15,9 @@
class GridWindow(Gtk.Window):
+
def method_connect(self, event, cb):
+
def handler(obj, *args):
cb(*args)
diff --git a/frc971/control_loops/python/spline_writer.py b/frc971/control_loops/python/spline_writer.py
index df622f3..a3d27cc 100644
--- a/frc971/control_loops/python/spline_writer.py
+++ b/frc971/control_loops/python/spline_writer.py
@@ -3,6 +3,7 @@
class SplineWriter(object):
+
def __init__(self, namespaces=None, filename="auto_splines.cc"):
if namespaces:
self._namespaces = namespaces