Run yapf on all python files in the repo
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I221e04c3f517fab8535b22551553799e0fee7a80
diff --git a/motors/decode_dump.py b/motors/decode_dump.py
index 27e1757..5d44694 100755
--- a/motors/decode_dump.py
+++ b/motors/decode_dump.py
@@ -12,51 +12,56 @@
data = bytes()
while len(data) < TOTAL_SIZE:
- read_now = sys.stdin.buffer.read(TOTAL_SIZE - len(data))
- if not read_now:
- print('EOF before data finished', file=sys.stderr)
- sys.exit(1)
- data += read_now
+ read_now = sys.stdin.buffer.read(TOTAL_SIZE - len(data))
+ if not read_now:
+ print('EOF before data finished', file=sys.stderr)
+ sys.exit(1)
+ data += read_now
print('%s' % len(data))
readable, _, _ = select.select([sys.stdin.buffer], [], [], 1)
if readable:
- print('Extra bytes', file=sys.stderr)
- sys.exit(1)
+ print('Extra bytes', file=sys.stderr)
+ sys.exit(1)
decoded = []
for i in range(DATAPOINTS):
- datapoint = DatapointStruct.unpack_from(data, i * DatapointStruct.size)
- decoded.append(datapoint)
+ datapoint = DatapointStruct.unpack_from(data, i * DatapointStruct.size)
+ decoded.append(datapoint)
+
def current(reading, ref):
- reading_voltage = reading / 4096 * 3.3 / 1.47 * (0.768 + 1.47)
- reading_voltage = reading / 4096 * 3.3 / 18.0 * (18.0 + 10.0)
- #reading_ref = ref / 4096 * 3.3
- reading_ref = 2.5
- #reading_ref = 0
- #return (reading_voltage - reading_ref) / 50 / 0.0003
- return (reading_voltage - reading_ref) / 0.195
+ reading_voltage = reading / 4096 * 3.3 / 1.47 * (0.768 + 1.47)
+ reading_voltage = reading / 4096 * 3.3 / 18.0 * (18.0 + 10.0)
+ #reading_ref = ref / 4096 * 3.3
+ reading_ref = 2.5
+ #reading_ref = 0
+ #return (reading_voltage - reading_ref) / 50 / 0.0003
+ return (reading_voltage - reading_ref) / 0.195
+
with open(sys.argv[1], 'w') as out:
- out.write('balanced0,balanced1,balanced2,current0.0,current1.0,current2.0,current0.1,current1.1,current2.1,count\n')
- #for point in decoded[2000:7200]:
- for point in decoded:
- out.write(','.join(str(d) for d in (
- current(point[0], point[6]),
- current(point[1], point[6]),
- current(point[2], point[6]),
- current(point[3], point[6]),
- current(point[4], point[6]),
- current(point[5], point[6]),
- current(point[6], point[6]),
- current(point[7], point[6]),
- current(point[8], point[6]),
- #point[6] / 100.0,
- #point[7] / 100.0,
- #point[8] / 100.0,
- point[9] / 100.0,
- point[10] / 100.0,
- )) + '\n')
+ out.write(
+ 'balanced0,balanced1,balanced2,current0.0,current1.0,current2.0,current0.1,current1.1,current2.1,count\n'
+ )
+ #for point in decoded[2000:7200]:
+ for point in decoded:
+ out.write(','.join(
+ str(d) for d in (
+ current(point[0], point[6]),
+ current(point[1], point[6]),
+ current(point[2], point[6]),
+ current(point[3], point[6]),
+ current(point[4], point[6]),
+ current(point[5], point[6]),
+ current(point[6], point[6]),
+ current(point[7], point[6]),
+ current(point[8], point[6]),
+ #point[6] / 100.0,
+ #point[7] / 100.0,
+ #point[8] / 100.0,
+ point[9] / 100.0,
+ point[10] / 100.0,
+ )) + '\n')
print('all done')
diff --git a/motors/fet12/calib_sensors.py b/motors/fet12/calib_sensors.py
index 7d882de..16c0ef7 100755
--- a/motors/fet12/calib_sensors.py
+++ b/motors/fet12/calib_sensors.py
@@ -6,8 +6,9 @@
# calib_data_60*.csv has each output channel set at a constant value of 60.
# calib_data_6030*.csv actuates two channels.
+
def calibrate(fnames):
- """Do fitting to calibrate ADC data given csv files.
+ """Do fitting to calibrate ADC data given csv files.
CSVs should be of format:
command_a, command_b, command_c, reading0, reading1, reading2
@@ -17,24 +18,25 @@
only care about the averaged samples because otherwise the solution matrix
can't be solved for in a stable manner.
"""
- data = np.zeros((1, 6))
- for fname in fnames:
- data = np.vstack((data, np.genfromtxt(fname, delimiter=',')))
- data = data[1:, :]
+ data = np.zeros((1, 6))
+ for fname in fnames:
+ data = np.vstack((data, np.genfromtxt(fname, delimiter=',')))
+ data = data[1:, :]
- data = data[:, :6]
+ data = data[:, :6]
- b = data[:, 0:3]
- b = b - np.tile(np.mean(b, axis=1), (3, 1)).T
- # Vcc / 3000 / R
- # 3000 converts duty cycle in FTM ticks to fraction of full.
- b *= 20.9 / 3000.0 / 0.0079
- A = data[:, 3:]
+ b = data[:, 0:3]
+ b = b - np.tile(np.mean(b, axis=1), (3, 1)).T
+ # Vcc / 3000 / R
+ # 3000 converts duty cycle in FTM ticks to fraction of full.
+ b *= 20.9 / 3000.0 / 0.0079
+ A = data[:, 3:]
- return np.linalg.lstsq(A, b[:])[0].T
+ return np.linalg.lstsq(A, b[:])[0].T
+
if __name__ == "__main__":
- if len(sys.argv) < 2:
- print("Need filenames for data")
- sys.exit(1)
- print(calibrate(sys.argv[1:]))
+ if len(sys.argv) < 2:
+ print("Need filenames for data")
+ sys.exit(1)
+ print(calibrate(sys.argv[1:]))
diff --git a/motors/fet12/current_equalize.py b/motors/fet12/current_equalize.py
index ea44916..114d53f 100755
--- a/motors/fet12/current_equalize.py
+++ b/motors/fet12/current_equalize.py
@@ -4,6 +4,7 @@
import sys
import calib_sensors
+
def manual_calibrate():
# 38 27 -84
# 36 -64 39
@@ -20,11 +21,12 @@
transform = I * numpy.linalg.inv(Is)
return transform
+
def main():
transform = manual_calibrate()
if len(sys.argv) > 1:
- transform = calib_sensors.calibrate(sys.argv[1:])
+ transform = calib_sensors.calibrate(sys.argv[1:])
print("#ifndef MOTORS_FET12_CURRENT_MATRIX_")
print("#define MOTORS_FET12_CURRENT_MATRIX_")
@@ -35,7 +37,8 @@
print("namespace motors {")
print("")
print(
- "inline ::std::array<float, 3> DecoupleCurrents(int16_t currents[3]) {")
+ "inline ::std::array<float, 3> DecoupleCurrents(int16_t currents[3]) {"
+ )
print(" ::std::array<float, 3> ans;")
for i in range(3):
@@ -54,5 +57,6 @@
return 0
+
if __name__ == '__main__':
sys.exit(main())
diff --git a/motors/pistol_grip/generate_cogging.py b/motors/pistol_grip/generate_cogging.py
index d889064..5b0a6e6 100644
--- a/motors/pistol_grip/generate_cogging.py
+++ b/motors/pistol_grip/generate_cogging.py
@@ -5,71 +5,73 @@
# TODO(austin): Plot flag.
+
def main(argv):
- if len(argv) < 4:
- print 'Args: input output.cc struct_name'
- return 1
- data_sum = [0.0] * 4096
- data_count = [0] * 4096
- data_list_absolute = []
- data_list_current = []
+ if len(argv) < 4:
+ print 'Args: input output.cc struct_name'
+ return 1
+ data_sum = [0.0] * 4096
+ data_count = [0] * 4096
+ data_list_absolute = []
+ data_list_current = []
- with open(argv[1], 'r') as fd:
- for line in fd:
- if line.startswith('reading'):
- split_line = line.split()
- data_absolute = int(split_line[1])
- data_index = int(split_line[3][2:])
- data_current = int(split_line[2]) / 10000.0
- data_sum[data_index] += data_current
- data_count[data_index] += 1
- data_list_absolute.append(data_absolute)
- data_list_current.append(data_current)
- data = [0.0] * 4096
- min_zero = 4096
- max_zero = 0
- for i in range(0, 4096):
- if data_count[i] == 0:
- min_zero = min(i, min_zero)
- max_zero = max(i, min_zero)
-
- for i in range(0, 4096):
- if data_count[i] != 0:
- data[i] = data_sum[i] / data_count[i]
- if min_zero == 0 and max_zero == 4095:
+ with open(argv[1], 'r') as fd:
+ for line in fd:
+ if line.startswith('reading'):
+ split_line = line.split()
+ data_absolute = int(split_line[1])
+ data_index = int(split_line[3][2:])
+ data_current = int(split_line[2]) / 10000.0
+ data_sum[data_index] += data_current
+ data_count[data_index] += 1
+ data_list_absolute.append(data_absolute)
+ data_list_current.append(data_current)
+ data = [0.0] * 4096
+ min_zero = 4096
+ max_zero = 0
for i in range(0, 4096):
- if data_count[i] != 0:
- while i > 0:
- data[i - 1] = data[i]
- i -= 1
- break;
+ if data_count[i] == 0:
+ min_zero = min(i, min_zero)
+ max_zero = max(i, min_zero)
- for i in reversed(range(0, 4096)):
- if data_count[i] != 0:
- while i < 4095:
- data[i + 1] = data[i]
- i += 1
- break;
- else:
for i in range(0, 4096):
- if data_count[i] == 0:
- if i < (min_zero + max_zero) / 2:
- data[i] = data[min_zero - 1]
- else:
- data[i] = data[max_zero + 1]
+ if data_count[i] != 0:
+ data[i] = data_sum[i] / data_count[i]
+ if min_zero == 0 and max_zero == 4095:
+ for i in range(0, 4096):
+ if data_count[i] != 0:
+ while i > 0:
+ data[i - 1] = data[i]
+ i -= 1
+ break
- pylab.plot(range(0, 4096), data)
- pylab.figure()
- pylab.plot(data_list_absolute, data_list_current)
- pylab.show()
- with open(argv[2], 'w') as out_fd:
- out_fd.write('extern const float %s[4096];\n' % argv[3])
- out_fd.write('const float %s[4096] = {\n' % argv[3])
- for datapoint in data:
- out_fd.write(' %ff,\n' % datapoint)
- out_fd.write('};')
+ for i in reversed(range(0, 4096)):
+ if data_count[i] != 0:
+ while i < 4095:
+ data[i + 1] = data[i]
+ i += 1
+ break
+ else:
+ for i in range(0, 4096):
+ if data_count[i] == 0:
+ if i < (min_zero + max_zero) / 2:
+ data[i] = data[min_zero - 1]
+ else:
+ data[i] = data[max_zero + 1]
- return 0
+ pylab.plot(range(0, 4096), data)
+ pylab.figure()
+ pylab.plot(data_list_absolute, data_list_current)
+ pylab.show()
+ with open(argv[2], 'w') as out_fd:
+ out_fd.write('extern const float %s[4096];\n' % argv[3])
+ out_fd.write('const float %s[4096] = {\n' % argv[3])
+ for datapoint in data:
+ out_fd.write(' %ff,\n' % datapoint)
+ out_fd.write('};')
+
+ return 0
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/motors/plot.py b/motors/plot.py
index 3d43080..c5b02a6 100755
--- a/motors/plot.py
+++ b/motors/plot.py
@@ -3,9 +3,7 @@
import numpy
from matplotlib import pylab
-data = numpy.loadtxt('/tmp/jkalsdjflsd.csv',
- delimiter=',',
- skiprows=1)
+data = numpy.loadtxt('/tmp/jkalsdjflsd.csv', delimiter=',', skiprows=1)
x = range(len(data))
pylab.subplot(1, 1, 1)
diff --git a/motors/print/itm_read.py b/motors/print/itm_read.py
index d616da4..ba12b90 100755
--- a/motors/print/itm_read.py
+++ b/motors/print/itm_read.py
@@ -11,97 +11,105 @@
import os
import sys
+
def open_file_for_bytes(path):
- '''Returns a file-like object which reads bytes without buffering.'''
- # Not using `open` because it's unclear from the docs how (if it's possible at
- # all) to get something that will only do one read call and return what that
- # gets on a fifo.
- try:
- return io.FileIO(path, 'r')
- except FileNotFoundError:
- # If it wasn't found, try (once) to create it and then open again.
+ '''Returns a file-like object which reads bytes without buffering.'''
+ # Not using `open` because it's unclear from the docs how (if it's possible at
+ # all) to get something that will only do one read call and return what that
+ # gets on a fifo.
try:
- os.mkfifo(path)
- except FileExistsError:
- pass
- return io.FileIO(path, 'r')
+ return io.FileIO(path, 'r')
+ except FileNotFoundError:
+ # If it wasn't found, try (once) to create it and then open again.
+ try:
+ os.mkfifo(path)
+ except FileExistsError:
+ pass
+ return io.FileIO(path, 'r')
+
def read_bytes(path):
- '''Reads bytes from a file. This is appropriate both for regular files and
+ '''Reads bytes from a file. This is appropriate both for regular files and
fifos.
Args:
path: A path-like object to open.
Yields:
Individual bytes from the file, until hitting EOF.
'''
- with open_file_for_bytes(path) as f:
- while True:
- buf = f.read(1024)
- if not buf:
- return
- for byte in buf:
- yield byte
+ with open_file_for_bytes(path) as f:
+ while True:
+ buf = f.read(1024)
+ if not buf:
+ return
+ for byte in buf:
+ yield byte
+
def parse_packets(source):
- '''Parses a stream of bytes into packets.
+ '''Parses a stream of bytes into packets.
Args:
source: A generator of individual bytes.
Generates:
Packets as bytes objects.
'''
- try:
- while True:
- header = next(source)
- if header == 0:
- # Synchronization packets consist of a bunch of 0 bits (not necessarily
- # a whole number of bytes), followed by a 128 byte. This is for hardware
- # to synchronize on, but we're not in a position to do that, so
- # presumably those should get filtered out before getting here?
- raise 'Not sure how to handle synchronization packets'
- packet = bytearray()
- packet.append(header)
- header_size = header & 3
- if header_size == 0:
- while packet[-1] & 128 and len(packet) < 7:
- packet.append(next(source))
- else:
- if header_size == 3:
- header_size = 4
- for _ in range(header_size):
- packet.append(next(source))
- yield bytes(packet)
- except StopIteration:
- return
+ try:
+ while True:
+ header = next(source)
+ if header == 0:
+ # Synchronization packets consist of a bunch of 0 bits (not necessarily
+ # a whole number of bytes), followed by a 128 byte. This is for hardware
+ # to synchronize on, but we're not in a position to do that, so
+ # presumably those should get filtered out before getting here?
+ raise 'Not sure how to handle synchronization packets'
+ packet = bytearray()
+ packet.append(header)
+ header_size = header & 3
+ if header_size == 0:
+ while packet[-1] & 128 and len(packet) < 7:
+ packet.append(next(source))
+ else:
+ if header_size == 3:
+ header_size = 4
+ for _ in range(header_size):
+ packet.append(next(source))
+ yield bytes(packet)
+ except StopIteration:
+ return
+
class PacketParser(object):
- def __init__(self):
- self.stimulus_handlers = {}
- def register_stimulus_handler(self, port_number, handler):
- '''Registers a function to call on packets to the specified port.'''
- self.stimulus_handlers[port_number] = handler
+ def __init__(self):
+ self.stimulus_handlers = {}
- def process(self, path):
- for packet in parse_packets(read_bytes(path)):
- header = packet[0]
- header_size = header & 3
- if header_size == 0:
- # TODO(Brian): At least handle overflow packets here.
- pass
- else:
- port_number = header >> 3
- if port_number in self.stimulus_handlers:
- self.stimulus_handlers[port_number](packet[1:])
- else:
- print('Warning: unhandled stimulus port %d' % port_number,
- file=sys.stderr)
- self.stimulus_handlers[port_number] = lambda _: None
+ def register_stimulus_handler(self, port_number, handler):
+ '''Registers a function to call on packets to the specified port.'''
+ self.stimulus_handlers[port_number] = handler
+
+ def process(self, path):
+ for packet in parse_packets(read_bytes(path)):
+ header = packet[0]
+ header_size = header & 3
+ if header_size == 0:
+ # TODO(Brian): At least handle overflow packets here.
+ pass
+ else:
+ port_number = header >> 3
+ if port_number in self.stimulus_handlers:
+ self.stimulus_handlers[port_number](packet[1:])
+ else:
+ print('Warning: unhandled stimulus port %d' % port_number,
+ file=sys.stderr)
+ self.stimulus_handlers[port_number] = lambda _: None
+
if __name__ == '__main__':
- parser = PacketParser()
- def print_byte(payload):
- sys.stdout.write(payload.decode('ascii'))
- parser.register_stimulus_handler(0, print_byte)
+ parser = PacketParser()
- for path in sys.argv[1:]:
- parser.process(path)
+ def print_byte(payload):
+ sys.stdout.write(payload.decode('ascii'))
+
+ parser.register_stimulus_handler(0, print_byte)
+
+ for path in sys.argv[1:]:
+ parser.process(path)
diff --git a/motors/python/big_phase_current.py b/motors/python/big_phase_current.py
index 8cbce3f..d31fc72 100755
--- a/motors/python/big_phase_current.py
+++ b/motors/python/big_phase_current.py
@@ -57,51 +57,62 @@
#switching_pattern = 'centered front shifted'
#switching_pattern = 'anticentered'
-Vconv = numpy.matrix([[2.0, -1.0, -1.0],
- [-1.0, 2.0, -1.0],
- [-1.0, -1.0, 2.0]]) / 3.0
+Vconv = numpy.matrix([[2.0, -1.0, -1.0], [-1.0, 2.0, -1.0], [-1.0, -1.0, 2.0]
+ ]) / 3.0
+
def f_single(theta):
- return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+
def g_single(theta):
- return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+
def gdot_single(theta):
- """Derivitive of the current.
+ """Derivitive of the current.
Must be multiplied by omega externally.
"""
- return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
+ return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
-f = numpy.vectorize(f_single, otypes=(numpy.float,))
-g = numpy.vectorize(g_single, otypes=(numpy.float,))
-gdot = numpy.vectorize(gdot_single, otypes=(numpy.float,))
+
+f = numpy.vectorize(f_single, otypes=(numpy.float, ))
+g = numpy.vectorize(g_single, otypes=(numpy.float, ))
+gdot = numpy.vectorize(gdot_single, otypes=(numpy.float, ))
+
def torque(theta):
- return f(theta) * g(theta)
+ return f(theta) * g(theta)
+
def phase_a(function, theta):
- return function(theta)
+ return function(theta)
+
def phase_b(function, theta):
- return function(theta + 2 * numpy.pi / 3)
+ return function(theta + 2 * numpy.pi / 3)
+
def phase_c(function, theta):
- return function(theta + 4 * numpy.pi / 3)
+ return function(theta + 4 * numpy.pi / 3)
+
def phases(function, theta):
- return numpy.matrix([[phase_a(function, theta)],
- [phase_b(function, theta)],
- [phase_c(function, theta)]])
+ return numpy.matrix([[phase_a(function,
+ theta)], [phase_b(function, theta)],
+ [phase_c(function, theta)]])
+
def all_phases(function, theta_range):
- return (phase_a(function, theta_range) +
- phase_b(function, theta_range) +
- phase_c(function, theta_range))
+ return (phase_a(function, theta_range) + phase_b(function, theta_range) +
+ phase_c(function, theta_range))
+
theta_range = numpy.linspace(start=0, stop=4 * numpy.pi, num=10000)
-one_amp_driving_voltage = R * g(theta_range) + (L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) + M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
+one_amp_driving_voltage = R * g(theta_range) + (
+ L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) +
+ M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
max_one_amp_driving_voltage = max(one_amp_driving_voltage)
@@ -111,7 +122,8 @@
print('Max BEMF', max(f(theta_range)))
print('Max current', max(g(theta_range)))
-print('Max drive voltage (one_amp_driving_voltage)', max(one_amp_driving_voltage))
+print('Max drive voltage (one_amp_driving_voltage)',
+ max(one_amp_driving_voltage))
print('one_amp_scalar', one_amp_scalar)
pylab.figure()
@@ -119,12 +131,14 @@
pylab.plot(theta_range, f(theta_range), label='bemf')
pylab.plot(theta_range, g(theta_range), label='phase_current')
pylab.plot(theta_range, torque(theta_range), label='phase_torque')
-pylab.plot(theta_range, all_phases(torque, theta_range), label='sum_torque/current')
+pylab.plot(theta_range,
+ all_phases(torque, theta_range),
+ label='sum_torque/current')
pylab.legend()
def full_sample_times(Ton, Toff, dt, n, start_time):
- """Returns n + 4 samples for the provided switching times.
+ """Returns n + 4 samples for the provided switching times.
We need the timesteps and Us to integrate.
@@ -139,235 +153,262 @@
array of [t, U matrix]
"""
- assert((Toff <= 1.0).all())
- assert((Ton <= 1.0).all())
- assert((Toff >= 0.0).all())
- assert((Ton >= 0.0).all())
+ assert ((Toff <= 1.0).all())
+ assert ((Ton <= 1.0).all())
+ assert ((Toff >= 0.0).all())
+ assert ((Ton >= 0.0).all())
- if (Ton <= Toff).all():
- on_before_off = True
- else:
- # Verify that they are all ordered correctly.
- assert(not (Ton <= Toff).any())
- on_before_off = False
-
- Toff = Toff.copy() * dt
- Toff[Toff < 100e-9] = -1.0
- Toff[Toff > dt] = dt
-
- Ton = Ton.copy() * dt
- Ton[Ton < 100e-9] = -1.0
- Ton[Ton > dt - 100e-9] = dt + 1.0
-
- result = []
- t = 0
-
- result_times = numpy.concatenate(
- (numpy.linspace(0, dt, num=n),
- numpy.reshape(numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]), (-1,)),
- numpy.reshape(numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]), (-1,))
- ))
- result_times.sort()
- assert((result_times >= 0).all())
- assert((result_times <= dt).all())
-
- for t in result_times:
- if on_before_off:
- U = numpy.matrix([[vcc], [vcc], [vcc]])
- U[t <= Ton] = 0.0
- U[Toff < t] = 0.0
+ if (Ton <= Toff).all():
+ on_before_off = True
else:
- U = numpy.matrix([[0.0], [0.0], [0.0]])
- U[t > Ton] = vcc
- U[t <= Toff] = vcc
- result.append((float(t + start_time), U.copy()))
+ # Verify that they are all ordered correctly.
+ assert (not (Ton <= Toff).any())
+ on_before_off = False
- return result
+ Toff = Toff.copy() * dt
+ Toff[Toff < 100e-9] = -1.0
+ Toff[Toff > dt] = dt
+
+ Ton = Ton.copy() * dt
+ Ton[Ton < 100e-9] = -1.0
+ Ton[Ton > dt - 100e-9] = dt + 1.0
+
+ result = []
+ t = 0
+
+ result_times = numpy.concatenate(
+ (numpy.linspace(0, dt, num=n),
+ numpy.reshape(
+ numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]),
+ (-1, )),
+ numpy.reshape(
+ numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]),
+ (-1, ))))
+ result_times.sort()
+ assert ((result_times >= 0).all())
+ assert ((result_times <= dt).all())
+
+ for t in result_times:
+ if on_before_off:
+ U = numpy.matrix([[vcc], [vcc], [vcc]])
+ U[t <= Ton] = 0.0
+ U[Toff < t] = 0.0
+ else:
+ U = numpy.matrix([[0.0], [0.0], [0.0]])
+ U[t > Ton] = vcc
+ U[t <= Toff] = vcc
+ result.append((float(t + start_time), U.copy()))
+
+ return result
+
def sample_times(T, dt, n, start_time):
- if switching_pattern == 'rear':
- T = 1.0 - T
- ans = full_sample_times(T, numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n, start_time)
- elif switching_pattern == 'centered front shifted':
- # Centered, but shifted to the beginning of the cycle.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ if switching_pattern == 'rear':
+ T = 1.0 - T
+ ans = full_sample_times(T,
+ numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n,
+ start_time)
+ elif switching_pattern == 'centered front shifted':
+ # Centered, but shifted to the beginning of the cycle.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- tn = min(Ton)[0, 0]
- Ton -= tn
- Toff -= tn
+ tn = min(Ton)[0, 0]
+ Ton -= tn
+ Toff -= tn
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'centered':
- # Centered, looks waaay better.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'centered':
+ # Centered, looks waaay better.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'anticentered':
- # Centered, looks waaay better.
- Toff = T / 2.0
- Ton = 1.0 - T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'anticentered':
+ # Centered, looks waaay better.
+ Toff = T / 2.0
+ Ton = 1.0 - T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'front':
- ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n, start_time)
- else:
- assert(False)
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'front':
+ ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n,
+ start_time)
+ else:
+ assert (False)
- return ans
+ return ans
+
class DataLogger(object):
- def __init__(self, title=None):
- self.title = title
- self.ia = []
- self.ib = []
- self.ic = []
- self.ia_goal = []
- self.ib_goal = []
- self.ic_goal = []
- self.ia_controls = []
- self.ib_controls = []
- self.ic_controls = []
- self.isensea = []
- self.isenseb = []
- self.isensec = []
- self.va = []
- self.vb = []
- self.vc = []
- self.van = []
- self.vbn = []
- self.vcn = []
+ def __init__(self, title=None):
+ self.title = title
+ self.ia = []
+ self.ib = []
+ self.ic = []
+ self.ia_goal = []
+ self.ib_goal = []
+ self.ic_goal = []
+ self.ia_controls = []
+ self.ib_controls = []
+ self.ic_controls = []
+ self.isensea = []
+ self.isenseb = []
+ self.isensec = []
- self.ea = []
- self.eb = []
- self.ec = []
+ self.va = []
+ self.vb = []
+ self.vc = []
+ self.van = []
+ self.vbn = []
+ self.vcn = []
- self.theta = []
- self.omega = []
+ self.ea = []
+ self.eb = []
+ self.ec = []
- self.i_goal = []
+ self.theta = []
+ self.omega = []
- self.time = []
- self.controls_time = []
- self.predicted_time = []
+ self.i_goal = []
- self.ia_pred = []
- self.ib_pred = []
- self.ic_pred = []
+ self.time = []
+ self.controls_time = []
+ self.predicted_time = []
- self.voltage_time = []
- self.estimated_velocity = []
- self.U_last = numpy.matrix(numpy.zeros((3, 1)))
+ self.ia_pred = []
+ self.ib_pred = []
+ self.ic_pred = []
- def log_predicted(self, current_time, p):
- self.predicted_time.append(current_time)
- self.ia_pred.append(p[0, 0])
- self.ib_pred.append(p[1, 0])
- self.ic_pred.append(p[2, 0])
+ self.voltage_time = []
+ self.estimated_velocity = []
+ self.U_last = numpy.matrix(numpy.zeros((3, 1)))
- def log_controls(self, current_time, measured_current, In, E, estimated_velocity):
- self.controls_time.append(current_time)
- self.ia_controls.append(measured_current[0, 0])
- self.ib_controls.append(measured_current[1, 0])
- self.ic_controls.append(measured_current[2, 0])
+ def log_predicted(self, current_time, p):
+ self.predicted_time.append(current_time)
+ self.ia_pred.append(p[0, 0])
+ self.ib_pred.append(p[1, 0])
+ self.ic_pred.append(p[2, 0])
- self.ea.append(E[0, 0])
- self.eb.append(E[1, 0])
- self.ec.append(E[2, 0])
+ def log_controls(self, current_time, measured_current, In, E,
+ estimated_velocity):
+ self.controls_time.append(current_time)
+ self.ia_controls.append(measured_current[0, 0])
+ self.ib_controls.append(measured_current[1, 0])
+ self.ic_controls.append(measured_current[2, 0])
- self.ia_goal.append(In[0, 0])
- self.ib_goal.append(In[1, 0])
- self.ic_goal.append(In[2, 0])
- self.estimated_velocity.append(estimated_velocity)
+ self.ea.append(E[0, 0])
+ self.eb.append(E[1, 0])
+ self.ec.append(E[2, 0])
- def log_data(self, X, U, current_time, Vn, i_goal):
- self.ia.append(X[0, 0])
- self.ib.append(X[1, 0])
- self.ic.append(X[2, 0])
+ self.ia_goal.append(In[0, 0])
+ self.ib_goal.append(In[1, 0])
+ self.ic_goal.append(In[2, 0])
+ self.estimated_velocity.append(estimated_velocity)
- self.i_goal.append(i_goal)
+ def log_data(self, X, U, current_time, Vn, i_goal):
+ self.ia.append(X[0, 0])
+ self.ib.append(X[1, 0])
+ self.ic.append(X[2, 0])
- self.isensea.append(X[5, 0])
- self.isenseb.append(X[6, 0])
- self.isensec.append(X[7, 0])
+ self.i_goal.append(i_goal)
- self.theta.append(X[3, 0])
- self.omega.append(X[4, 0])
+ self.isensea.append(X[5, 0])
+ self.isenseb.append(X[6, 0])
+ self.isensec.append(X[7, 0])
- self.time.append(current_time)
+ self.theta.append(X[3, 0])
+ self.omega.append(X[4, 0])
- self.van.append(Vn[0, 0])
- self.vbn.append(Vn[1, 0])
- self.vcn.append(Vn[2, 0])
+ self.time.append(current_time)
- if (self.U_last != U).any():
- self.va.append(self.U_last[0, 0])
- self.vb.append(self.U_last[1, 0])
- self.vc.append(self.U_last[2, 0])
- self.voltage_time.append(current_time)
+ self.van.append(Vn[0, 0])
+ self.vbn.append(Vn[1, 0])
+ self.vcn.append(Vn[2, 0])
- self.va.append(U[0, 0])
- self.vb.append(U[1, 0])
- self.vc.append(U[2, 0])
- self.voltage_time.append(current_time)
- self.U_last = U.copy()
+ if (self.U_last != U).any():
+ self.va.append(self.U_last[0, 0])
+ self.vb.append(self.U_last[1, 0])
+ self.vc.append(self.U_last[2, 0])
+ self.voltage_time.append(current_time)
- def plot(self):
- fig = pylab.figure()
- pylab.subplot(3, 1, 1)
- pylab.plot(self.controls_time, self.ia_controls, 'ro', label='ia_controls')
- pylab.plot(self.controls_time, self.ib_controls, 'go', label='ib_controls')
- pylab.plot(self.controls_time, self.ic_controls, 'bo', label='ic_controls')
- pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
- pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
- pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
+ self.va.append(U[0, 0])
+ self.vb.append(U[1, 0])
+ self.vc.append(U[2, 0])
+ self.voltage_time.append(current_time)
+ self.U_last = U.copy()
- #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
- #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
- #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
- pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
- pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
- pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
- pylab.plot(self.time, self.ia, 'r', label='ia')
- pylab.plot(self.time, self.ib, 'g', label='ib')
- pylab.plot(self.time, self.ic, 'b', label='ic')
- pylab.plot(self.time, self.i_goal, label='i_goal')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
+ def plot(self):
+ fig = pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.controls_time,
+ self.ia_controls,
+ 'ro',
+ label='ia_controls')
+ pylab.plot(self.controls_time,
+ self.ib_controls,
+ 'go',
+ label='ib_controls')
+ pylab.plot(self.controls_time,
+ self.ic_controls,
+ 'bo',
+ label='ic_controls')
+ pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
+ pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
+ pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
- pylab.subplot(3, 1, 2)
- pylab.plot(self.voltage_time, self.va, label='va')
- pylab.plot(self.voltage_time, self.vb, label='vb')
- pylab.plot(self.voltage_time, self.vc, label='vc')
- pylab.plot(self.time, self.van, label='van')
- pylab.plot(self.time, self.vbn, label='vbn')
- pylab.plot(self.time, self.vcn, label='vcn')
- pylab.plot(self.controls_time, self.ea, label='ea')
- pylab.plot(self.controls_time, self.eb, label='eb')
- pylab.plot(self.controls_time, self.ec, label='ec')
- pylab.legend()
+ #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
+ #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
+ #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
+ pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
+ pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
+ pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
+ pylab.plot(self.time, self.ia, 'r', label='ia')
+ pylab.plot(self.time, self.ib, 'g', label='ib')
+ pylab.plot(self.time, self.ic, 'b', label='ic')
+ pylab.plot(self.time, self.i_goal, label='i_goal')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.time, self.theta, label='theta')
- pylab.plot(self.time, self.omega, label='omega')
- pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.voltage_time, self.va, label='va')
+ pylab.plot(self.voltage_time, self.vb, label='vb')
+ pylab.plot(self.voltage_time, self.vc, label='vc')
+ pylab.plot(self.time, self.van, label='van')
+ pylab.plot(self.time, self.vbn, label='vbn')
+ pylab.plot(self.time, self.vcn, label='vcn')
+ pylab.plot(self.controls_time, self.ea, label='ea')
+ pylab.plot(self.controls_time, self.eb, label='eb')
+ pylab.plot(self.controls_time, self.ec, label='ec')
+ pylab.legend()
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.time, self.theta, label='theta')
+ pylab.plot(self.time, self.omega, label='omega')
+ pylab.plot(self.controls_time,
+ self.estimated_velocity,
+ label='estimated omega')
- fig = pylab.figure()
- pylab.plot(self.controls_time,
- map(operator.sub, self.ia_goal, self.ia_controls), 'r', label='ia_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ib_goal, self.ib_controls), 'g', label='ib_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ic_goal, self.ic_controls), 'b', label='ic_error')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
- pylab.show()
+ pylab.legend()
+
+ fig = pylab.figure()
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ia_goal, self.ia_controls),
+ 'r',
+ label='ia_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ib_goal, self.ib_controls),
+ 'g',
+ label='ib_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ic_goal, self.ic_controls),
+ 'b',
+ label='ic_error')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+ pylab.show()
# So, from running a bunch of math, we know the following:
@@ -400,180 +441,204 @@
# inv(L_matrix) * (Vconv * V - E - R_matrix * I) = I_dot
# B * V - inv(L_matrix) * E - A * I = I_dot
class Simulation(object):
- def __init__(self):
- self.R_matrix = numpy.matrix(numpy.eye(3)) * R
- self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
- self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
- self.A = self.L_matrix_inv * self.R_matrix
- self.B = self.L_matrix_inv * Vconv
- self.A_discrete, self.B_discrete = controls.c2d(-self.A, self.B, 1.0 / hz)
- self.B_discrete_inverse = numpy.matrix(numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- self.R_model = R * 1.0
- self.L_model = L * 1.0
- self.M_model = M * 1.0
- self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
- self.L_matrix_model = numpy.matrix([[self.L_model, self.M_model, self.M_model],
- [self.M_model, self.L_model, self.M_model],
- [self.M_model, self.M_model, self.L_model]])
- self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
- self.A_model = self.L_matrix_inv_model * self.R_matrix_model
- self.B_model = self.L_matrix_inv_model * Vconv
- self.A_discrete_model, self.B_discrete_model = \
- controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
- self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
+ def __init__(self):
+ self.R_matrix = numpy.matrix(numpy.eye(3)) * R
+ self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
+ self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
+ self.A = self.L_matrix_inv * self.R_matrix
+ self.B = self.L_matrix_inv * Vconv
+ self.A_discrete, self.B_discrete = controls.c2d(
+ -self.A, self.B, 1.0 / hz)
+ self.B_discrete_inverse = numpy.matrix(
+ numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- print('constexpr double kL = %g;' % self.L_model)
- print('constexpr double kM = %g;' % self.M_model)
- print('constexpr double kR = %g;' % self.R_model)
- print('constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[0, 0])
- print('constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[1, 0])
- print('constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[0, 0])
- print('constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[1, 0])
- print('constexpr double kOneAmpScalar = %g;' % one_amp_scalar)
- print('constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage)
- print('A_discrete', self.A_discrete)
- print('B_discrete', self.B_discrete)
- print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
- print('B_discrete_inv', self.B_discrete_inverse)
+ self.R_model = R * 1.0
+ self.L_model = L * 1.0
+ self.M_model = M * 1.0
+ self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
+ self.L_matrix_model = numpy.matrix(
+ [[self.L_model, self.M_model, self.M_model],
+ [self.M_model, self.L_model, self.M_model],
+ [self.M_model, self.M_model, self.L_model]])
+ self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
+ self.A_model = self.L_matrix_inv_model * self.R_matrix_model
+ self.B_model = self.L_matrix_inv_model * Vconv
+ self.A_discrete_model, self.B_discrete_model = \
+ controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
+ self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (
+ self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
- # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
- # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
- self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 / (R_sense1 * C_sense))
- self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0)
+ print('constexpr double kL = %g;' % self.L_model)
+ print('constexpr double kM = %g;' % self.M_model)
+ print('constexpr double kR = %g;' % self.R_model)
+ print('constexpr float kAdiscrete_diagonal = %gf;' %
+ self.A_discrete_model[0, 0])
+ print('constexpr float kAdiscrete_offdiagonal = %gf;' %
+ self.A_discrete_model[1, 0])
+ print('constexpr float kBdiscrete_inv_diagonal = %gf;' %
+ self.B_discrete_inverse_model[0, 0])
+ print('constexpr float kBdiscrete_inv_offdiagonal = %gf;' %
+ self.B_discrete_inverse_model[1, 0])
+ print('constexpr double kOneAmpScalar = %g;' % one_amp_scalar)
+ print('constexpr double kMaxOneAmpDrivingVoltage = %g;' %
+ max_one_amp_driving_voltage)
+ print('A_discrete', self.A_discrete)
+ print('B_discrete', self.B_discrete)
+ print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
+ print('B_discrete_inv', self.B_discrete_inverse)
- # ia, ib, ic, theta, omega, isensea, isenseb, isensec
- self.X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
+ # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
+ # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
+ self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 /
+ (R_sense1 * C_sense))
+ self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (
+ R_sense1 / R_sense2 + 1.0)
- self.K = 0.05 * Vconv
- print('A %s' % repr(self.A))
- print('B %s' % repr(self.B))
- print('K %s' % repr(self.K))
+ # ia, ib, ic, theta, omega, isensea, isenseb, isensec
+ self.X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0],
+ [0.0]])
- print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
- print('Poles are %s' % repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.K = 0.05 * Vconv
+ print('A %s' % repr(self.A))
+ print('B %s' % repr(self.B))
+ print('K %s' % repr(self.K))
- controllability = controls.ctrb(self.A, self.B)
- print('Rank of augmented controlability matrix. %d' % numpy.linalg.matrix_rank(
- controllability))
+ print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
+ print('Poles are %s' %
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.data_logger = DataLogger(switching_pattern)
- self.current_time = 0.0
+ controllability = controls.ctrb(self.A, self.B)
+ print('Rank of augmented controlability matrix. %d' %
+ numpy.linalg.matrix_rank(controllability))
- self.estimated_velocity = self.X[4, 0]
+ self.data_logger = DataLogger(switching_pattern)
+ self.current_time = 0.0
- def motor_diffeq(self, x, t, U):
- I = numpy.matrix(x[0:3]).T
- theta = x[3]
- omega = x[4]
- Isense = numpy.matrix(x[5:]).T
+ self.estimated_velocity = self.X[4, 0]
- dflux = phases(f_single, theta) / Kv
+ def motor_diffeq(self, x, t, U):
+ I = numpy.matrix(x[0:3]).T
+ theta = x[3]
+ omega = x[4]
+ Isense = numpy.matrix(x[5:]).T
- Xdot = numpy.matrix(numpy.zeros((8, 1)))
- di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
- torque = I.T * dflux
- Xdot[0:3, :] = di_dt
- Xdot[3, :] = omega
- Xdot[4, :] = torque / J
+ dflux = phases(f_single, theta) / Kv
- Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
- return numpy.squeeze(numpy.asarray(Xdot))
+ Xdot = numpy.matrix(numpy.zeros((8, 1)))
+ di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
+ torque = I.T * dflux
+ Xdot[0:3, :] = di_dt
+ Xdot[3, :] = omega
+ Xdot[4, :] = torque / J
- def DoControls(self, goal_current):
- theta = self.X[3, 0]
- # Use the actual angular velocity.
- omega = self.X[4, 0]
+ Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
+ return numpy.squeeze(numpy.asarray(Xdot))
- measured_current = self.X[5:, :].copy()
+ def DoControls(self, goal_current):
+ theta = self.X[3, 0]
+ # Use the actual angular velocity.
+ omega = self.X[4, 0]
- # Ok, lets now fake it.
- E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
+ measured_current = self.X[5:, :].copy()
+
+ # Ok, lets now fake it.
+ E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)]])
- E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
+ E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)]])
- overall_measured_current = ((E_imag1 + E_imag2).real.T * measured_current / one_amp_scalar)[0, 0]
+ overall_measured_current = ((E_imag1 + E_imag2).real.T *
+ measured_current / one_amp_scalar)[0, 0]
- current_error = goal_current - overall_measured_current
- #print(current_error)
- self.estimated_velocity += current_error * 1.0
- omega = self.estimated_velocity
+ current_error = goal_current - overall_measured_current
+ #print(current_error)
+ self.estimated_velocity += current_error * 1.0
+ omega = self.estimated_velocity
- # Now, apply the transfer function of the inductor.
- # Use that to difference the current across the cycle.
- Icurrent = self.Ilast
- # No history:
- #Icurrent = phases(g_single, theta) * goal_current
- Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
+ # Now, apply the transfer function of the inductor.
+ # Use that to difference the current across the cycle.
+ Icurrent = self.Ilast
+ # No history:
+ #Icurrent = phases(g_single, theta) * goal_current
+ Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
- deltaI = Inext - Icurrent
+ deltaI = Inext - Icurrent
- H1 = -numpy.linalg.inv(1j * omega * self.L_matrix + self.R_matrix) * omega / Kv
- H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix + self.R_matrix) * omega / Kv
- p_imag = H1 * E_imag1 + H2 * E_imag2
- p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
- numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
- p = p_imag.real
+ H1 = -numpy.linalg.inv(1j * omega * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ p_imag = H1 * E_imag1 + H2 * E_imag2
+ p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
+ numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
+ p = p_imag.real
- # So, we now know how much the change in current is due to changes in BEMF.
- # Subtract that, and then run the stock statespace equation.
- Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete * (Icurrent - p) - p_next_imag.real)
- print('Vn_ff', Vn_ff)
- print('Inext', Inext)
- Vn = Vn_ff + self.K * (Icurrent - measured_current)
+ # So, we now know how much the change in current is due to changes in BEMF.
+ # Subtract that, and then run the stock statespace equation.
+ Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete *
+ (Icurrent - p) - p_next_imag.real)
+ print('Vn_ff', Vn_ff)
+ print('Inext', Inext)
+ Vn = Vn_ff + self.K * (Icurrent - measured_current)
- E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
- self.data_logger.log_controls(self.current_time, measured_current, Icurrent, E, self.estimated_velocity)
+ E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
+ self.data_logger.log_controls(self.current_time, measured_current,
+ Icurrent, E, self.estimated_velocity)
- self.Ilast = Inext
+ self.Ilast = Inext
- return Vn
+ return Vn
- def Simulate(self):
- start_wall_time = time.time()
- self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
- for n in range(200):
- goal_current = 10.0
- max_current = (vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- min_current = (-vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- goal_current = max(min_current, min(max_current, goal_current))
+ def Simulate(self):
+ start_wall_time = time.time()
+ self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
+ for n in range(200):
+ goal_current = 10.0
+ max_current = (
+ vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ min_current = (
+ -vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ goal_current = max(min_current, min(max_current, goal_current))
- Vn = self.DoControls(goal_current)
+ Vn = self.DoControls(goal_current)
- #Vn = numpy.matrix([[0.20], [0.0], [0.0]])
- #Vn = numpy.matrix([[0.00], [0.20], [0.0]])
- #Vn = numpy.matrix([[0.00], [0.0], [0.20]])
+ #Vn = numpy.matrix([[0.20], [0.0], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.20], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.0], [0.20]])
- # T is the fractional rate.
- T = Vn / vcc
- tn = -numpy.min(T)
- T += tn
- if (T > 1.0).any():
- T = T / numpy.max(T)
+ # T is the fractional rate.
+ T = Vn / vcc
+ tn = -numpy.min(T)
+ T += tn
+ if (T > 1.0).any():
+ T = T / numpy.max(T)
- for t, U in sample_times(T = T,
- dt = 1.0 / hz, n = 10,
- start_time = self.current_time):
- # Analog amplifier mode!
- #U = Vn
+ for t, U in sample_times(T=T,
+ dt=1.0 / hz,
+ n=10,
+ start_time=self.current_time):
+ # Analog amplifier mode!
+ #U = Vn
- self.data_logger.log_data(self.X, (U - min(U)), self.current_time, Vn, goal_current)
- t_array = numpy.array([self.current_time, t])
- self.X = numpy.matrix(scipy.integrate.odeint(
- self.motor_diffeq,
- numpy.squeeze(numpy.asarray(self.X)),
- t_array, args=(U,)))[1, :].T
+ self.data_logger.log_data(self.X, (U - min(U)),
+ self.current_time, Vn, goal_current)
+ t_array = numpy.array([self.current_time, t])
+ self.X = numpy.matrix(
+ scipy.integrate.odeint(self.motor_diffeq,
+ numpy.squeeze(numpy.asarray(
+ self.X)),
+ t_array,
+ args=(U, )))[1, :].T
- self.current_time = t
+ self.current_time = t
- print('Took %f to simulate' % (time.time() - start_wall_time))
+ print('Took %f to simulate' % (time.time() - start_wall_time))
- self.data_logger.plot()
+ self.data_logger.plot()
+
simulation = Simulation()
simulation.Simulate()
diff --git a/motors/python/haptic_phase_current.py b/motors/python/haptic_phase_current.py
index b17c514..fec909d 100755
--- a/motors/python/haptic_phase_current.py
+++ b/motors/python/haptic_phase_current.py
@@ -54,51 +54,62 @@
#switching_pattern = 'centered front shifted'
#switching_pattern = 'anticentered'
-Vconv = numpy.matrix([[2.0, -1.0, -1.0],
- [-1.0, 2.0, -1.0],
- [-1.0, -1.0, 2.0]]) / 3.0
+Vconv = numpy.matrix([[2.0, -1.0, -1.0], [-1.0, 2.0, -1.0], [-1.0, -1.0, 2.0]
+ ]) / 3.0
+
def f_single(theta):
- return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+
def g_single(theta):
- return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+
def gdot_single(theta):
- """Derivitive of the current.
+ """Derivitive of the current.
Must be multiplied by omega externally.
"""
- return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
+ return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
-f = numpy.vectorize(f_single, otypes=(numpy.float,))
-g = numpy.vectorize(g_single, otypes=(numpy.float,))
-gdot = numpy.vectorize(gdot_single, otypes=(numpy.float,))
+
+f = numpy.vectorize(f_single, otypes=(numpy.float, ))
+g = numpy.vectorize(g_single, otypes=(numpy.float, ))
+gdot = numpy.vectorize(gdot_single, otypes=(numpy.float, ))
+
def torque(theta):
- return f(theta) * g(theta)
+ return f(theta) * g(theta)
+
def phase_a(function, theta):
- return function(theta)
+ return function(theta)
+
def phase_b(function, theta):
- return function(theta + 2 * numpy.pi / 3)
+ return function(theta + 2 * numpy.pi / 3)
+
def phase_c(function, theta):
- return function(theta + 4 * numpy.pi / 3)
+ return function(theta + 4 * numpy.pi / 3)
+
def phases(function, theta):
- return numpy.matrix([[phase_a(function, theta)],
- [phase_b(function, theta)],
- [phase_c(function, theta)]])
+ return numpy.matrix([[phase_a(function,
+ theta)], [phase_b(function, theta)],
+ [phase_c(function, theta)]])
+
def all_phases(function, theta_range):
- return (phase_a(function, theta_range) +
- phase_b(function, theta_range) +
- phase_c(function, theta_range))
+ return (phase_a(function, theta_range) + phase_b(function, theta_range) +
+ phase_c(function, theta_range))
+
theta_range = numpy.linspace(start=0, stop=4 * numpy.pi, num=10000)
-one_amp_driving_voltage = R * g(theta_range) + (L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) + M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
+one_amp_driving_voltage = R * g(theta_range) + (
+ L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) +
+ M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
max_one_amp_driving_voltage = max(one_amp_driving_voltage)
@@ -108,7 +119,8 @@
print 'Max BEMF', max(f(theta_range))
print 'Max current', max(g(theta_range))
-print 'Max drive voltage (one_amp_driving_voltage)', max(one_amp_driving_voltage)
+print 'Max drive voltage (one_amp_driving_voltage)', max(
+ one_amp_driving_voltage)
print 'one_amp_scalar', one_amp_scalar
pylab.figure()
@@ -116,12 +128,14 @@
pylab.plot(theta_range, f(theta_range), label='bemf')
pylab.plot(theta_range, g(theta_range), label='phase_current')
pylab.plot(theta_range, torque(theta_range), label='phase_torque')
-pylab.plot(theta_range, all_phases(torque, theta_range), label='sum_torque/current')
+pylab.plot(theta_range,
+ all_phases(torque, theta_range),
+ label='sum_torque/current')
pylab.legend()
def full_sample_times(Ton, Toff, dt, n, start_time):
- """Returns n + 4 samples for the provided switching times.
+ """Returns n + 4 samples for the provided switching times.
We need the timesteps and Us to integrate.
@@ -136,235 +150,260 @@
array of [t, U matrix]
"""
- assert((Toff <= 1.0).all())
- assert((Ton <= 1.0).all())
- assert((Toff >= 0.0).all())
- assert((Ton >= 0.0).all())
+ assert ((Toff <= 1.0).all())
+ assert ((Ton <= 1.0).all())
+ assert ((Toff >= 0.0).all())
+ assert ((Ton >= 0.0).all())
- if (Ton <= Toff).all():
- on_before_off = True
- else:
- # Verify that they are all ordered correctly.
- assert(not (Ton <= Toff).any())
- on_before_off = False
-
- Toff = Toff.copy() * dt
- Toff[Toff < 100e-9] = -1.0
- Toff[Toff > dt] = dt
-
- Ton = Ton.copy() * dt
- Ton[Ton < 100e-9] = -1.0
- Ton[Ton > dt - 100e-9] = dt + 1.0
-
- result = []
- t = 0
-
- result_times = numpy.concatenate(
- (numpy.linspace(0, dt, num=n),
- numpy.reshape(numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]), (-1,)),
- numpy.reshape(numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]), (-1,))
- ))
- result_times.sort()
- assert((result_times >= 0).all())
- assert((result_times <= dt).all())
-
- for t in result_times:
- if on_before_off:
- U = numpy.matrix([[vcc], [vcc], [vcc]])
- U[t <= Ton] = 0.0
- U[Toff < t] = 0.0
+ if (Ton <= Toff).all():
+ on_before_off = True
else:
- U = numpy.matrix([[0.0], [0.0], [0.0]])
- U[t > Ton] = vcc
- U[t <= Toff] = vcc
- result.append((float(t + start_time), U.copy()))
+ # Verify that they are all ordered correctly.
+ assert (not (Ton <= Toff).any())
+ on_before_off = False
- return result
+ Toff = Toff.copy() * dt
+ Toff[Toff < 100e-9] = -1.0
+ Toff[Toff > dt] = dt
+
+ Ton = Ton.copy() * dt
+ Ton[Ton < 100e-9] = -1.0
+ Ton[Ton > dt - 100e-9] = dt + 1.0
+
+ result = []
+ t = 0
+
+ result_times = numpy.concatenate(
+ (numpy.linspace(0, dt, num=n),
+ numpy.reshape(
+ numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]),
+ (-1, )),
+ numpy.reshape(
+ numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]),
+ (-1, ))))
+ result_times.sort()
+ assert ((result_times >= 0).all())
+ assert ((result_times <= dt).all())
+
+ for t in result_times:
+ if on_before_off:
+ U = numpy.matrix([[vcc], [vcc], [vcc]])
+ U[t <= Ton] = 0.0
+ U[Toff < t] = 0.0
+ else:
+ U = numpy.matrix([[0.0], [0.0], [0.0]])
+ U[t > Ton] = vcc
+ U[t <= Toff] = vcc
+ result.append((float(t + start_time), U.copy()))
+
+ return result
+
def sample_times(T, dt, n, start_time):
- if switching_pattern == 'rear':
- T = 1.0 - T
- ans = full_sample_times(T, numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n, start_time)
- elif switching_pattern == 'centered front shifted':
- # Centered, but shifted to the beginning of the cycle.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ if switching_pattern == 'rear':
+ T = 1.0 - T
+ ans = full_sample_times(T,
+ numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n,
+ start_time)
+ elif switching_pattern == 'centered front shifted':
+ # Centered, but shifted to the beginning of the cycle.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- tn = min(Ton)[0, 0]
- Ton -= tn
- Toff -= tn
+ tn = min(Ton)[0, 0]
+ Ton -= tn
+ Toff -= tn
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'centered':
- # Centered, looks waaay better.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'centered':
+ # Centered, looks waaay better.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'anticentered':
- # Centered, looks waaay better.
- Toff = T / 2.0
- Ton = 1.0 - T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'anticentered':
+ # Centered, looks waaay better.
+ Toff = T / 2.0
+ Ton = 1.0 - T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'front':
- ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n, start_time)
- else:
- assert(False)
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'front':
+ ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n,
+ start_time)
+ else:
+ assert (False)
- return ans
+ return ans
+
class DataLogger(object):
- def __init__(self, title=None):
- self.title = title
- self.ia = []
- self.ib = []
- self.ic = []
- self.ia_goal = []
- self.ib_goal = []
- self.ic_goal = []
- self.ia_controls = []
- self.ib_controls = []
- self.ic_controls = []
- self.isensea = []
- self.isenseb = []
- self.isensec = []
- self.va = []
- self.vb = []
- self.vc = []
- self.van = []
- self.vbn = []
- self.vcn = []
+ def __init__(self, title=None):
+ self.title = title
+ self.ia = []
+ self.ib = []
+ self.ic = []
+ self.ia_goal = []
+ self.ib_goal = []
+ self.ic_goal = []
+ self.ia_controls = []
+ self.ib_controls = []
+ self.ic_controls = []
+ self.isensea = []
+ self.isenseb = []
+ self.isensec = []
- self.ea = []
- self.eb = []
- self.ec = []
+ self.va = []
+ self.vb = []
+ self.vc = []
+ self.van = []
+ self.vbn = []
+ self.vcn = []
- self.theta = []
- self.omega = []
+ self.ea = []
+ self.eb = []
+ self.ec = []
- self.i_goal = []
+ self.theta = []
+ self.omega = []
- self.time = []
- self.controls_time = []
- self.predicted_time = []
+ self.i_goal = []
- self.ia_pred = []
- self.ib_pred = []
- self.ic_pred = []
+ self.time = []
+ self.controls_time = []
+ self.predicted_time = []
- self.voltage_time = []
- self.estimated_velocity = []
- self.U_last = numpy.matrix(numpy.zeros((3, 1)))
+ self.ia_pred = []
+ self.ib_pred = []
+ self.ic_pred = []
- def log_predicted(self, current_time, p):
- self.predicted_time.append(current_time)
- self.ia_pred.append(p[0, 0])
- self.ib_pred.append(p[1, 0])
- self.ic_pred.append(p[2, 0])
+ self.voltage_time = []
+ self.estimated_velocity = []
+ self.U_last = numpy.matrix(numpy.zeros((3, 1)))
- def log_controls(self, current_time, measured_current, In, E, estimated_velocity):
- self.controls_time.append(current_time)
- self.ia_controls.append(measured_current[0, 0])
- self.ib_controls.append(measured_current[1, 0])
- self.ic_controls.append(measured_current[2, 0])
+ def log_predicted(self, current_time, p):
+ self.predicted_time.append(current_time)
+ self.ia_pred.append(p[0, 0])
+ self.ib_pred.append(p[1, 0])
+ self.ic_pred.append(p[2, 0])
- self.ea.append(E[0, 0])
- self.eb.append(E[1, 0])
- self.ec.append(E[2, 0])
+ def log_controls(self, current_time, measured_current, In, E,
+ estimated_velocity):
+ self.controls_time.append(current_time)
+ self.ia_controls.append(measured_current[0, 0])
+ self.ib_controls.append(measured_current[1, 0])
+ self.ic_controls.append(measured_current[2, 0])
- self.ia_goal.append(In[0, 0])
- self.ib_goal.append(In[1, 0])
- self.ic_goal.append(In[2, 0])
- self.estimated_velocity.append(estimated_velocity)
+ self.ea.append(E[0, 0])
+ self.eb.append(E[1, 0])
+ self.ec.append(E[2, 0])
- def log_data(self, X, U, current_time, Vn, i_goal):
- self.ia.append(X[0, 0])
- self.ib.append(X[1, 0])
- self.ic.append(X[2, 0])
+ self.ia_goal.append(In[0, 0])
+ self.ib_goal.append(In[1, 0])
+ self.ic_goal.append(In[2, 0])
+ self.estimated_velocity.append(estimated_velocity)
- self.i_goal.append(i_goal)
+ def log_data(self, X, U, current_time, Vn, i_goal):
+ self.ia.append(X[0, 0])
+ self.ib.append(X[1, 0])
+ self.ic.append(X[2, 0])
- self.isensea.append(X[5, 0])
- self.isenseb.append(X[6, 0])
- self.isensec.append(X[7, 0])
+ self.i_goal.append(i_goal)
- self.theta.append(X[3, 0])
- self.omega.append(X[4, 0])
+ self.isensea.append(X[5, 0])
+ self.isenseb.append(X[6, 0])
+ self.isensec.append(X[7, 0])
- self.time.append(current_time)
+ self.theta.append(X[3, 0])
+ self.omega.append(X[4, 0])
- self.van.append(Vn[0, 0])
- self.vbn.append(Vn[1, 0])
- self.vcn.append(Vn[2, 0])
+ self.time.append(current_time)
- if (self.U_last != U).any():
- self.va.append(self.U_last[0, 0])
- self.vb.append(self.U_last[1, 0])
- self.vc.append(self.U_last[2, 0])
- self.voltage_time.append(current_time)
+ self.van.append(Vn[0, 0])
+ self.vbn.append(Vn[1, 0])
+ self.vcn.append(Vn[2, 0])
- self.va.append(U[0, 0])
- self.vb.append(U[1, 0])
- self.vc.append(U[2, 0])
- self.voltage_time.append(current_time)
- self.U_last = U.copy()
+ if (self.U_last != U).any():
+ self.va.append(self.U_last[0, 0])
+ self.vb.append(self.U_last[1, 0])
+ self.vc.append(self.U_last[2, 0])
+ self.voltage_time.append(current_time)
- def plot(self):
- fig = pylab.figure()
- pylab.subplot(3, 1, 1)
- pylab.plot(self.controls_time, self.ia_controls, 'ro', label='ia_controls')
- pylab.plot(self.controls_time, self.ib_controls, 'go', label='ib_controls')
- pylab.plot(self.controls_time, self.ic_controls, 'bo', label='ic_controls')
- pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
- pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
- pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
+ self.va.append(U[0, 0])
+ self.vb.append(U[1, 0])
+ self.vc.append(U[2, 0])
+ self.voltage_time.append(current_time)
+ self.U_last = U.copy()
- #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
- #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
- #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
- pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
- pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
- pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
- pylab.plot(self.time, self.ia, 'r', label='ia')
- pylab.plot(self.time, self.ib, 'g', label='ib')
- pylab.plot(self.time, self.ic, 'b', label='ic')
- pylab.plot(self.time, self.i_goal, label='i_goal')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
+ def plot(self):
+ fig = pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.controls_time,
+ self.ia_controls,
+ 'ro',
+ label='ia_controls')
+ pylab.plot(self.controls_time,
+ self.ib_controls,
+ 'go',
+ label='ib_controls')
+ pylab.plot(self.controls_time,
+ self.ic_controls,
+ 'bo',
+ label='ic_controls')
+ pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
+ pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
+ pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
- pylab.subplot(3, 1, 2)
- pylab.plot(self.voltage_time, self.va, label='va')
- pylab.plot(self.voltage_time, self.vb, label='vb')
- pylab.plot(self.voltage_time, self.vc, label='vc')
- pylab.plot(self.time, self.van, label='van')
- pylab.plot(self.time, self.vbn, label='vbn')
- pylab.plot(self.time, self.vcn, label='vcn')
- pylab.plot(self.controls_time, self.ea, label='ea')
- pylab.plot(self.controls_time, self.eb, label='eb')
- pylab.plot(self.controls_time, self.ec, label='ec')
- pylab.legend()
+ #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
+ #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
+ #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
+ pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
+ pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
+ pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
+ pylab.plot(self.time, self.ia, 'r', label='ia')
+ pylab.plot(self.time, self.ib, 'g', label='ib')
+ pylab.plot(self.time, self.ic, 'b', label='ic')
+ pylab.plot(self.time, self.i_goal, label='i_goal')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.time, self.theta, label='theta')
- pylab.plot(self.time, self.omega, label='omega')
- #pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.voltage_time, self.va, label='va')
+ pylab.plot(self.voltage_time, self.vb, label='vb')
+ pylab.plot(self.voltage_time, self.vc, label='vc')
+ pylab.plot(self.time, self.van, label='van')
+ pylab.plot(self.time, self.vbn, label='vbn')
+ pylab.plot(self.time, self.vcn, label='vcn')
+ pylab.plot(self.controls_time, self.ea, label='ea')
+ pylab.plot(self.controls_time, self.eb, label='eb')
+ pylab.plot(self.controls_time, self.ec, label='ec')
+ pylab.legend()
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.time, self.theta, label='theta')
+ pylab.plot(self.time, self.omega, label='omega')
+ #pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
- fig = pylab.figure()
- pylab.plot(self.controls_time,
- map(operator.sub, self.ia_goal, self.ia_controls), 'r', label='ia_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ib_goal, self.ib_controls), 'g', label='ib_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ic_goal, self.ic_controls), 'b', label='ic_error')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
- pylab.show()
+ pylab.legend()
+
+ fig = pylab.figure()
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ia_goal, self.ia_controls),
+ 'r',
+ label='ia_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ib_goal, self.ib_controls),
+ 'g',
+ label='ib_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ic_goal, self.ic_controls),
+ 'b',
+ label='ic_error')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+ pylab.show()
# So, from running a bunch of math, we know the following:
@@ -397,180 +436,203 @@
# inv(L_matrix) * (Vconv * V - E - R_matrix * I) = I_dot
# B * V - inv(L_matrix) * E - A * I = I_dot
class Simulation(object):
- def __init__(self):
- self.R_matrix = numpy.matrix(numpy.eye(3)) * R
- self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
- self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
- self.A = self.L_matrix_inv * self.R_matrix
- self.B = self.L_matrix_inv * Vconv
- self.A_discrete, self.B_discrete = controls.c2d(-self.A, self.B, 1.0 / hz)
- self.B_discrete_inverse = numpy.matrix(numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- self.R_model = R * 1.0
- self.L_model = L * 1.0
- self.M_model = M * 1.0
- self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
- self.L_matrix_model = numpy.matrix([[self.L_model, self.M_model, self.M_model],
- [self.M_model, self.L_model, self.M_model],
- [self.M_model, self.M_model, self.L_model]])
- self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
- self.A_model = self.L_matrix_inv_model * self.R_matrix_model
- self.B_model = self.L_matrix_inv_model * Vconv
- self.A_discrete_model, self.B_discrete_model = \
- controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
- self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
+ def __init__(self):
+ self.R_matrix = numpy.matrix(numpy.eye(3)) * R
+ self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
+ self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
+ self.A = self.L_matrix_inv * self.R_matrix
+ self.B = self.L_matrix_inv * Vconv
+ self.A_discrete, self.B_discrete = controls.c2d(
+ -self.A, self.B, 1.0 / hz)
+ self.B_discrete_inverse = numpy.matrix(
+ numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- print 'constexpr double kL = %g;' % self.L_model
- print 'constexpr double kM = %g;' % self.M_model
- print 'constexpr double kR = %g;' % self.R_model
- print 'constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[0, 0]
- print 'constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[1, 0]
- print 'constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[0, 0]
- print 'constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[1, 0]
- print 'constexpr double kOneAmpScalar = %g;' % one_amp_scalar
- print 'constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage
- print('A_discrete', self.A_discrete)
- print('B_discrete', self.B_discrete)
- print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
- print('B_discrete_inv', self.B_discrete_inverse)
+ self.R_model = R * 1.0
+ self.L_model = L * 1.0
+ self.M_model = M * 1.0
+ self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
+ self.L_matrix_model = numpy.matrix(
+ [[self.L_model, self.M_model, self.M_model],
+ [self.M_model, self.L_model, self.M_model],
+ [self.M_model, self.M_model, self.L_model]])
+ self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
+ self.A_model = self.L_matrix_inv_model * self.R_matrix_model
+ self.B_model = self.L_matrix_inv_model * Vconv
+ self.A_discrete_model, self.B_discrete_model = \
+ controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
+ self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (
+ self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
- # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
- # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
- self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 / (R_sense1 * C_sense))
- self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0)
+ print 'constexpr double kL = %g;' % self.L_model
+ print 'constexpr double kM = %g;' % self.M_model
+ print 'constexpr double kR = %g;' % self.R_model
+ print 'constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[
+ 0, 0]
+ print 'constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[
+ 1, 0]
+ print 'constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[
+ 0, 0]
+ print 'constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[
+ 1, 0]
+ print 'constexpr double kOneAmpScalar = %g;' % one_amp_scalar
+ print 'constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage
+ print('A_discrete', self.A_discrete)
+ print('B_discrete', self.B_discrete)
+ print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
+ print('B_discrete_inv', self.B_discrete_inverse)
- # ia, ib, ic, theta, omega, isensea, isenseb, isensec
- self.X = numpy.matrix([[0.0], [0.0], [0.0], [-2.0 * numpy.pi / 3.0], [0.0], [0.0], [0.0], [0.0]])
+ # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
+ # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
+ self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 /
+ (R_sense1 * C_sense))
+ self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (
+ R_sense1 / R_sense2 + 1.0)
- self.K = 0.05 * Vconv
- print('A %s' % repr(self.A))
- print('B %s' % repr(self.B))
- print('K %s' % repr(self.K))
+ # ia, ib, ic, theta, omega, isensea, isenseb, isensec
+ self.X = numpy.matrix([[0.0], [0.0], [0.0], [-2.0 * numpy.pi / 3.0],
+ [0.0], [0.0], [0.0], [0.0]])
- print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
- print('Poles are %s' % repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.K = 0.05 * Vconv
+ print('A %s' % repr(self.A))
+ print('B %s' % repr(self.B))
+ print('K %s' % repr(self.K))
- controllability = controls.ctrb(self.A, self.B)
- print('Rank of augmented controlability matrix. %d' % numpy.linalg.matrix_rank(
- controllability))
+ print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
+ print('Poles are %s' %
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.data_logger = DataLogger(switching_pattern)
- self.current_time = 0.0
+ controllability = controls.ctrb(self.A, self.B)
+ print('Rank of augmented controlability matrix. %d' %
+ numpy.linalg.matrix_rank(controllability))
- self.estimated_velocity = self.X[4, 0]
+ self.data_logger = DataLogger(switching_pattern)
+ self.current_time = 0.0
- def motor_diffeq(self, x, t, U):
- I = numpy.matrix(x[0:3]).T
- theta = x[3]
- omega = x[4]
- Isense = numpy.matrix(x[5:]).T
+ self.estimated_velocity = self.X[4, 0]
- dflux = phases(f_single, theta) / Kv
+ def motor_diffeq(self, x, t, U):
+ I = numpy.matrix(x[0:3]).T
+ theta = x[3]
+ omega = x[4]
+ Isense = numpy.matrix(x[5:]).T
- Xdot = numpy.matrix(numpy.zeros((8, 1)))
- di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
- torque = I.T * dflux
- Xdot[0:3, :] = di_dt
- Xdot[3, :] = omega
- Xdot[4, :] = torque / J
+ dflux = phases(f_single, theta) / Kv
- Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
- return numpy.squeeze(numpy.asarray(Xdot))
+ Xdot = numpy.matrix(numpy.zeros((8, 1)))
+ di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
+ torque = I.T * dflux
+ Xdot[0:3, :] = di_dt
+ Xdot[3, :] = omega
+ Xdot[4, :] = torque / J
- def DoControls(self, goal_current):
- theta = self.X[3, 0]
- # Use the actual angular velocity.
- omega = self.X[4, 0]
+ Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
+ return numpy.squeeze(numpy.asarray(Xdot))
- measured_current = self.X[5:, :].copy()
+ def DoControls(self, goal_current):
+ theta = self.X[3, 0]
+ # Use the actual angular velocity.
+ omega = self.X[4, 0]
- # Ok, lets now fake it.
- E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
+ measured_current = self.X[5:, :].copy()
+
+ # Ok, lets now fake it.
+ E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)]])
- E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
+ E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)]])
- overall_measured_current = ((E_imag1 + E_imag2).real.T * measured_current / one_amp_scalar)[0, 0]
+ overall_measured_current = ((E_imag1 + E_imag2).real.T *
+ measured_current / one_amp_scalar)[0, 0]
- current_error = goal_current - overall_measured_current
- #print(current_error)
- self.estimated_velocity += current_error * 1.0
- omega = self.estimated_velocity
+ current_error = goal_current - overall_measured_current
+ #print(current_error)
+ self.estimated_velocity += current_error * 1.0
+ omega = self.estimated_velocity
- # Now, apply the transfer function of the inductor.
- # Use that to difference the current across the cycle.
- Icurrent = self.Ilast
- # No history:
- #Icurrent = phases(g_single, theta) * goal_current
- Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
+ # Now, apply the transfer function of the inductor.
+ # Use that to difference the current across the cycle.
+ Icurrent = self.Ilast
+ # No history:
+ #Icurrent = phases(g_single, theta) * goal_current
+ Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
- deltaI = Inext - Icurrent
+ deltaI = Inext - Icurrent
- H1 = -numpy.linalg.inv(1j * omega * self.L_matrix + self.R_matrix) * omega / Kv
- H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix + self.R_matrix) * omega / Kv
- p_imag = H1 * E_imag1 + H2 * E_imag2
- p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
- numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
- p = p_imag.real
+ H1 = -numpy.linalg.inv(1j * omega * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ p_imag = H1 * E_imag1 + H2 * E_imag2
+ p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
+ numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
+ p = p_imag.real
- # So, we now know how much the change in current is due to changes in BEMF.
- # Subtract that, and then run the stock statespace equation.
- Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete * (Icurrent - p) - p_next_imag.real)
- print 'Vn_ff', Vn_ff
- print 'Inext', Inext
- Vn = Vn_ff + self.K * (Icurrent - measured_current)
+ # So, we now know how much the change in current is due to changes in BEMF.
+ # Subtract that, and then run the stock statespace equation.
+ Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete *
+ (Icurrent - p) - p_next_imag.real)
+ print 'Vn_ff', Vn_ff
+ print 'Inext', Inext
+ Vn = Vn_ff + self.K * (Icurrent - measured_current)
- E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
- self.data_logger.log_controls(self.current_time, measured_current, Icurrent, E, self.estimated_velocity)
+ E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
+ self.data_logger.log_controls(self.current_time, measured_current,
+ Icurrent, E, self.estimated_velocity)
- self.Ilast = Inext
+ self.Ilast = Inext
- return Vn
+ return Vn
- def Simulate(self):
- start_wall_time = time.time()
- self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
- for n in range(200):
- goal_current = 1.0
- max_current = (vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- min_current = (-vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- goal_current = max(min_current, min(max_current, goal_current))
+ def Simulate(self):
+ start_wall_time = time.time()
+ self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
+ for n in range(200):
+ goal_current = 1.0
+ max_current = (
+ vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ min_current = (
+ -vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ goal_current = max(min_current, min(max_current, goal_current))
- Vn = self.DoControls(goal_current)
+ Vn = self.DoControls(goal_current)
- #Vn = numpy.matrix([[1.00], [0.0], [0.0]])
- Vn = numpy.matrix([[0.00], [1.00], [0.0]])
- #Vn = numpy.matrix([[0.00], [0.0], [1.00]])
+ #Vn = numpy.matrix([[1.00], [0.0], [0.0]])
+ Vn = numpy.matrix([[0.00], [1.00], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.0], [1.00]])
- # T is the fractional rate.
- T = Vn / vcc
- tn = -numpy.min(T)
- T += tn
- if (T > 1.0).any():
- T = T / numpy.max(T)
+ # T is the fractional rate.
+ T = Vn / vcc
+ tn = -numpy.min(T)
+ T += tn
+ if (T > 1.0).any():
+ T = T / numpy.max(T)
- for t, U in sample_times(T = T,
- dt = 1.0 / hz, n = 10,
- start_time = self.current_time):
- # Analog amplifier mode!
- #U = Vn
+ for t, U in sample_times(T=T,
+ dt=1.0 / hz,
+ n=10,
+ start_time=self.current_time):
+ # Analog amplifier mode!
+ #U = Vn
- self.data_logger.log_data(self.X, (U - min(U)), self.current_time, Vn, goal_current)
- t_array = numpy.array([self.current_time, t])
- self.X = numpy.matrix(scipy.integrate.odeint(
- self.motor_diffeq,
- numpy.squeeze(numpy.asarray(self.X)),
- t_array, args=(U,)))[1, :].T
+ self.data_logger.log_data(self.X, (U - min(U)),
+ self.current_time, Vn, goal_current)
+ t_array = numpy.array([self.current_time, t])
+ self.X = numpy.matrix(
+ scipy.integrate.odeint(self.motor_diffeq,
+ numpy.squeeze(numpy.asarray(
+ self.X)),
+ t_array,
+ args=(U, )))[1, :].T
- self.current_time = t
+ self.current_time = t
- print 'Took %f to simulate' % (time.time() - start_wall_time)
+ print 'Took %f to simulate' % (time.time() - start_wall_time)
- self.data_logger.plot()
+ self.data_logger.plot()
+
simulation = Simulation()
simulation.Simulate()
diff --git a/motors/seems_reasonable/drivetrain.py b/motors/seems_reasonable/drivetrain.py
index 52b3920..ad3d92a 100644
--- a/motors/seems_reasonable/drivetrain.py
+++ b/motors/seems_reasonable/drivetrain.py
@@ -30,11 +30,10 @@
glog.error("Expected .h file name and .cc file name")
else:
# Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(
- argv[1:3],
- argv[3:5], ['motors', 'seems_reasonable'],
- kDrivetrain,
- scalar_type='float')
+ drivetrain.WriteDrivetrain(argv[1:3],
+ argv[3:5], ['motors', 'seems_reasonable'],
+ kDrivetrain,
+ scalar_type='float')
if __name__ == '__main__':
diff --git a/motors/seems_reasonable/polydrivetrain.py b/motors/seems_reasonable/polydrivetrain.py
index 452b3fb..665739f 100644
--- a/motors/seems_reasonable/polydrivetrain.py
+++ b/motors/seems_reasonable/polydrivetrain.py
@@ -16,19 +16,19 @@
except gflags.DuplicateFlagError:
pass
+
def main(argv):
if FLAGS.plot:
polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
elif len(argv) != 7:
glog.fatal('Expected .h file name and .cc file name')
else:
- polydrivetrain.WritePolyDrivetrain(
- argv[1:3],
- argv[3:5],
- argv[5:7],
- ['motors', 'seems_reasonable'],
- drivetrain.kDrivetrain,
- scalar_type='float')
+ polydrivetrain.WritePolyDrivetrain(argv[1:3],
+ argv[3:5],
+ argv[5:7],
+ ['motors', 'seems_reasonable'],
+ drivetrain.kDrivetrain,
+ scalar_type='float')
if __name__ == '__main__':