def action(args):
make_list = (lambda x: list(x)
if (type(x) == list or type(x) == tuple) else [x])
def make_int(x):
return [int(y) for y in x]
board_ids = make_int(make_list(ast.literal_eval(args.board_ids)))
actuations = make_list(ast.literal_eval(args.actuations))
mode = args.mode
ser = serial.Serial(port=args.serial,
baudrate=args.baud_rate,
timeout=0.05)
client = comms.BLDCControllerClient(ser)
boards.initBoards(client, board_ids)
client.leaveBootloader(board_ids)
client.resetInputBuffer()
boards.initMotor(client, board_ids)
def updateCurrent(i):
data = []
for board_id in board_ids:
try:
boards.driveMotor(client, board_ids, actuations, mode)
# Read the iq calulated
data.append(
struct.unpack(
"<f",
client.readRegisters([board_id], [0x3003], [1])[0],
))
# Read the iq command
data.append(
struct.unpack(
"<f",
client.readRegisters([board_id], [0x3020], [1])[0],
))
except (comms.MalformedPacketError, comms.ProtocolError):
print("Failed to communicate with board: ", board_id)
return time.time(), None if len(data) != (2 * len(board_ids)) else data
def flatten(item_list):
return [item for sublist in item_list for item in sublist]
labels = []
labels.extend([[str(bid) + "'s iq Reading",
str(bid) + "'s iq PID output"] for bid in board_ids])
labels = flatten(labels)
graph = livegraph.LiveGraph(updateCurrent,
labels,
sample_interval=1,
window_size=2000)
graph.start()
def action(args):
board_ids = [int(bid) for bid in args.board_ids.split(",")]
ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=2.0)
time.sleep(0.1)
client = comms.BLDCControllerClient(ser)
initialized = boards.initBoards(client, board_ids)
ser.reset_input_buffer()
if initialized:
crashed = client.checkWDGRST()
if crashed:
print(
"Some boards have crashed, please power cycle before upload:",
crashed,
)
if not crashed:
for board_id in board_ids:
client.leaveBootloader([board_id])
time.sleep(0.2) # Wait for the controller to reset
ser.reset_input_buffer()
flash_sector_maps = client.getFlashSectorMap([board_id])
with open(args.bin_file, "rb") as bin_file:
firmware_image = bin_file.read()
success = False
while not success:
try:
success = client.writeFlash(
[board_id],
args.offset,
firmware_image,
sector_map=flash_sector_maps,
print_progress=True,
)
except (
comms.MalformedPacketError,
comms.ProtocolError,
) as e:
print(f"Upload to board {board_id} failed with error:")
print(e)
print("Retrying...")
continue
if success:
print("Upload to Board", board_id, "Succeeded")
else:
print("Upload to Board", board_id, "Failed")
break
ser.close()
def action(args):
def make_list(x):
return list(x) if (type(x) == list or type(x) == tuple) else [x]
def make_type(x, to_type):
return [to_type(y) for y in x]
board_ids = make_type(make_list(ast.literal_eval(args.board_ids)), int)
actuations = make_list(ast.literal_eval(args.actuations))
mode = args.mode
ser = serial.Serial(port=args.serial,
baudrate=args.baud_rate,
timeout=0.001)
client = comms.BLDCControllerClient(ser)
boards.initBoards(client, board_ids)
client.leaveBootloader(board_ids)
client.resetInputBuffer()
boards.initMotor(client, board_ids)
def callback() -> bool:
boards.clearWDGRST(client)
try:
boards.driveMotor(client, board_ids, actuations, mode)
except (comms.ProtocolError, comms.MalformedPacketError):
return False
return True
loop = utils.DebugLoop(callback, args.num_iters, iters_per_print=1000)
loop.loop()
def action(args):
ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=2.0)
time.sleep(0.2)
ser.reset_input_buffer()
def make_list(x):
return list(x) if (type(x) == list or type(x) == tuple) else [x]
def make_int(x):
return [int(y) for y in x]
board_ids = make_int(make_list(ast.literal_eval(args.board_ids)))
# Load in Custom Values
with open(args.calibration_file) as json_file:
calibration = json.load(json_file)
client = comms.BLDCControllerClient(ser)
initialized = boards.initBoards(client, board_ids)
client.resetInputBuffer()
if initialized:
for board_id, calib in zip(board_ids, calibration):
print(board_id, "-", calib)
client.leaveBootloader([board_id])
# Reset Calibration on Board
client.clearCalibration([board_id])
boards.loadCalibrationFromJSON(client, board_id, calib)
client.setWatchdogTimeout([board_id], [1000])
# Setting gains for motor
client.setDirectCurrentKp([board_id], [0.5])
client.setDirectCurrentKi([board_id], [0.1])
client.setQuadratureCurrentKp([board_id], [1.0])
client.setQuadratureCurrentKi([board_id], [0.2])
# Store Calibration struct to Parameters
client.storeCalibration([board_id])
ser.close()
def action(args):
#
# Data collection
#
board_ids = [args.board_id]
ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=0.1)
time.sleep(0.1)
client = comms.BLDCControllerClient(ser)
boards.initBoards(client, board_ids)
client.leaveBootloader(board_ids)
time.sleep(0.2)
client.resetInputBuffer()
def set_phase_state(phase_state):
a, b, c = phase_state
targets = [
a * args.duty_cycle,
b * args.duty_cycle,
c * args.duty_cycle,
]
while True:
try:
boards.driveMotor(client, board_ids, targets, "phase")
break
except (comms.MalformedPacketError, comms.ProtocolError):
print("Phase state set failed. Retrying.")
# Clear currently loaded current offsets
offset_data = struct.pack("<fff", 0, 0, 0)
client.writeRegisters(board_ids, [0x1050], [3], [offset_data])
client.setWatchdogTimeout(board_ids, [1000])
set_phase_state((0, 0, 0))
time.sleep(0.2)
# First, read floating currents to calculate offset
reset = client.resetRecorderBuffer(board_ids)[0]
print("reset: %u" % reset)
success = client.startRecorder(board_ids)[0]
print("started: %u" % success)
data = []
data_length = 0
while data_length == 0:
try:
data_length = client.getRecorderLength(board_ids)[0]
except comms.MalformedPacketError as e:
print(e)
continue
time.sleep(0.1)
for i in range(0, data_length, comms.COMM_NUM_RECORDER_ELEMENTS):
# Grab the recorder data
try:
data.extend(client.getRecorderElement(board_ids, [i]))
except (comms.MalformedPacketError, comms.ProtocolError):
print("Missed packet")
f, axarr = plt.subplots(1, sharex=True)
ia = [e[0] for e in data]
ib = [e[1] for e in data]
ic = [e[2] for e in data]
len_data = len(data)
ia_offset = sum(ia) / len_data
ib_offset = sum(ib) / len_data
ic_offset = sum(ic) / len_data
print("Phase A Offset:", ia_offset)
print("Phase B Offset:", ib_offset)
print("Phase C Offset:", ic_offset)
# Start one step before phase A to avoid boundary issues
set_phase_state(phase_state_list[-1])
time.sleep(args.delay)
# Forward
forward_raw_angles = []
for i in range(args.max_steps):
set_phase_state(phase_state_list[i % 6])
time.sleep(args.delay)
while True:
try:
raw_angle = client.getRawRotorPosition(board_ids)[0]
break
except (comms.MalformedPacketError, comms.ProtocolError):
print("Missed packet")
if (i > 4 and abs(forward_raw_angles[0] - raw_angle) <
abs(forward_raw_angles[1] - forward_raw_angles[0]) / 3.0):
break
forward_raw_angles.append(raw_angle)
step_count = len(forward_raw_angles)
# Take one more step to avoid boundary issues
set_phase_state(phase_state_list[step_count % 6])
time.sleep(args.delay)
# Backward
backward_raw_angles = []
for i in range(step_count - 1, -1, -1):
set_phase_state(phase_state_list[i % 6])
time.sleep(args.delay)
while True:
try:
raw_angle = client.getRawRotorPosition(board_ids)[0]
break
except (comms.MalformedPacketError, comms.ProtocolError):
print("Missed packet")
backward_raw_angles.append(raw_angle)
set_phase_state((0, 0, 0))
ser.close()
#
# Data analysis
#
# Flip backward raw angles to correspond with forward raw angles
backward_raw_angles = backward_raw_angles[::-1]
forward_raw_angles = np.array(forward_raw_angles)
backward_raw_angles = np.array(backward_raw_angles)
# Convert to radians
forward_angles = forward_raw_angles / encoder_ticks_per_rev * 2 * np.pi
backward_angles = backward_raw_angles / encoder_ticks_per_rev * 2 * np.pi
# Phase unwrapping
forward_angles = np.unwrap(forward_angles)
backward_angles = np.unwrap(backward_angles)
# Average forward and backward measurements
angles = (forward_angles + backward_angles) / 2
# Shift smallest encoder value to front of array
wrapped_angles = angles % (2 * np.pi)
zero_index = np.argmin(wrapped_angles)
angles = np.unwrap(np.roll(wrapped_angles, -zero_index))
# Guess erevs/mrev
angle_slope = np.mean(np.diff(angles))
steps_per_mrev = int(np.round(2 * np.pi / angle_slope))
erevs_per_mrev = steps_per_mrev // 6 # Could be negative
# Convert to electrical angle
elec_angles = angles * erevs_per_mrev
# Subtract expected trend
elec_angle_residuals = elec_angles - (np.r_[:len(elec_angles)] +
zero_index) * (2 * np.pi / 6)
# Find smallest raw angle aligned with phase A
elec_angle_offset = np.mean(elec_angle_residuals)
wrapped_elec_angle_offset = elec_angle_offset % (2 * np.pi)
if erevs_per_mrev < 0:
wrapped_elec_angle_offset -= 2 * np.pi
angle_offset = wrapped_elec_angle_offset / erevs_per_mrev
erev_start = angle_offset / (2 * np.pi) * encoder_ticks_per_rev
print(" erev_start: {:5d}".format(int(round(erev_start))))
print("erevs_per_mrev: {:5d}".format(abs(erevs_per_mrev)))
print(" flip_phases: {:5d}".format(int(erevs_per_mrev > 0)))
size = str(input("What size is the motor? (S/L)\n"))
upload_data = {
"inv": int(erevs_per_mrev > 0),
"epm": abs(erevs_per_mrev),
"angle": int(erev_start),
"torque": (1.45, 0.6)[size.lower() == "s"],
"zero": 0.0,
"ia_off": ia_offset,
"ib_off": ib_offset,
"ic_off": ic_offset,
}
print("Calibration")
print(upload_data)
with open("calibrations.json", "w") as outfile:
json.dump([upload_data], outfile)
def make_list(x):
return list(x) if (type(x) == list or type(x) == tuple) else [x]
def make_type(x, to_type):
return [to_type(y) for y in x]
board_ids = make_type(make_list(ast.literal_eval(args.board_ids)), int)
actuations = make_list(ast.literal_eval(args.actuations))
mode = args.mode
ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=0.1)
client = comms.BLDCControllerClient(ser)
initialized = boards.initBoards(client, board_ids)
client.leaveBootloader(board_ids)
client.resetInputBuffer()
boards.initMotor(client, board_ids)
start_time = time.time()
count = 0
rollover = 1000
def getIMU(bids):
return client.readRegisters(bids, [comms.COMM_ROR_ACC_X] * len(bids),
[3] * len(bids))
def action(args):
ser = serial.Serial(port=args.serial, baudrate=args.baud_rate, timeout=2.0)
time.sleep(0.2)
ser.reset_input_buffer()
def make_list(x):
return list(x) if (type(x) == list or type(x) == tuple) else [x]
def make_int(x):
return [int(y) for y in x]
board_ids = make_int(make_list(ast.literal_eval(args.board_ids)))
client = comms.BLDCControllerClient(ser)
initialized = boards.initBoards(client, board_ids)
client.resetInputBuffer()
large_motor_tc = 1.45
if initialized:
for board_id in board_ids:
client.leaveBootloader([board_id])
torque_const = client.getTorqueConstant([board_id])[0]
client.setWatchdogTimeout([board_id], [1000])
# Setting gains for motor
client.setDirectCurrentKp([board_id], [0.5])
client.setDirectCurrentKi([board_id], [0.1])
client.setQuadratureCurrentKp([board_id], [1.0])
client.setQuadratureCurrentKi([board_id], [0.2])
# Velocity controller is not used right now. Tunings need to be
# adjusted.
client.setVelocityKp([board_id], [0.5])
client.setVelocityKd([board_id], [0.01])
if torque_const > large_motor_tc - 0.01:
# Big motors
client.setPositionKp([board_id], [1.0])
client.setPositionKd([board_id], [1000.0])
else:
# Small motors
client.setPositionKp([board_id], [0.5])
client.setPositionKd([board_id], [100.0])
# Modifying Limits
client.setCurrentLimit([board_id], [2.0])
client.setTorqueLimit([board_id], [3.0])
client.setVelocityLimit([board_id], [10.0])
# Store Calibration struct to Parameters
client.storeCalibration([board_id])
print("Updated:", board_id)
print("Finished Updating Calibrations")
ser.close()
def action(args):
ser = serial.Serial(port=args.serial,
baudrate=args.baud_rate,
timeout=0.004)
board_ids = [int(bid) for bid in args.board_ids.split(",")]
client = comms.BLDCControllerClient(ser)
boards.initBoards(client, board_ids)
for bid in board_ids:
client.leaveBootloader([bid])
sen = args.sensor
address = ReadOnlyRegs[sen]
decode = "<f"
num_regs = 1
message = "{0}: {1[0]}"
if sen == "encoder_raw":
decode = "<H"
message = "{0}: {1[0]} ticks"
elif sen == "encoder":
message = "{0}: {1[0]} radians"
elif sen == "velocity":
message = "{0}: {1[0]} rad/s"
elif sen == "id":
message = "{0}: {1[0]} amps"
elif sen == "iq":
message = "{0}: {1[0]} amps"
elif sen == "supply":
message = "{0}: {1[0]} volts"
elif sen == "temp":
message = "{0}: {1[0]} degC"
if sen == "imu":
decode = "<hhh"
num_regs = 3
message = "{0} -> x:{1[0]}, y:{1[1]}, z:{1[2]}"
num_boards = len(board_ids)
def callback() -> bool:
boards.clearWDGRST(client)
try:
responses = client.readRegisters(board_ids, [address] * num_boards,
[num_regs] * num_boards)
for i in range(len(responses)):
if not responses[i]:
print("Board {} could not be read.".format(i))
continue
val = struct.unpack(decode, responses[i])
bid = board_ids[i]
print("Board:", bid, message.format(args.sensor, val))
except (comms.MalformedPacketError, comms.ProtocolError) as e:
print(e)
return False
loop = utils.DebugLoop(callback=callback,
num_iters=args.num_iters,
iters_per_print=1000)
loop.loop()