def connect_to(): # global odrive_boards # find the odrives odrive_boards[0] = odrive.find_any(serial_number=board_1_num) odrive_boards[1] = odrive.find_any(serial_number=board_2_num) odrive_boards[2] = odrive.find_any(serial_number=board_3_num)
def _connect_to_odrive(self):
"""
Connect to the odrive
"""
print("finding YZ odrive...")
self.odrv_YZ = odrive.find_any(serial_number="208037743548")
assert self.odrv_YZ != None
assert not isinstance(self.odrv_YZ, list)
print("finding X odrive...")
self.odrv_X = odrive.find_any(serial_number="3762364A3137")
assert self.odrv_X != None
assert not isinstance(self.odrv_X, list)
self.axes = {
"X": self.odrv_X.axis0,
"Y": self.odrv_YZ.axis1,
"Z": self.odrv_YZ.axis0
}
print("ODrives are connected, dumping previous errors")
# ODrive dose not clear errors with a reconnection, and will refuse to take action until they are cleared
print("YZ Odrive Errors:")
dump_errors(self.odrv_YZ, True)
print("X Odrive Errors:")
dump_errors(self.odrv_X, True)
print("\n\n")
# If there is an error, it configures it back to idle
for axis_id in self.axes:
self._set_state(axis_id, AXIS_STATE_CLOSED_LOOP_CONTROL)
# If it errers from a set state, then the configuration is broken
self.check_errors()
def DriveSync():
print("connecting")
global odrv0
global odrv1
odrv0 = odrive.find_any(serial_number = "208037743548") #Connect ot Odrive0
print("odrv0 connected")
odrv1 = odrive.find_any(serial_number = "3762364A3137") #Connect ot Odrive1
print("odrv1 connected")
def connect_to_motors():
global driver
if constants.O_DRIVE_SERIAL is None:
driver = odrive.find_any(timeout=60)
else: # todo: check these configs
driver = odrive.find_any(path='usb',
serial_number=constants.O_DRIVE_SERIAL,
search_cancellation_token=None,
channel_termination_token=None,
timeout=60)
def odrive_worker(serial, conn):
search_serial = format(int(serial), 'x').upper()
print(f'searching for odrive {search_serial}')
od = odrive.find_any(serial_number=search_serial)
print(f'found odrive {search_serial}')
conn.send(0)
while True:
command = conn.recv()
out_data = {}
out_data['axis_0'] = {}
out_data['axis_1'] = {}
out_data['odrive'] = {}
#run given functions on assigned axes if available
if 'command' in command['axis_0']:
#print('axis 0 commandflash_drive_params')
command['axis_0']['command'](od.axis0, out_data['axis_0'])
if 'command' in command['axis_1']:
#print('axis 1 command')
command['axis_1']['command'](od.axis1, out_data['axis_1'])
if 'command' in command:
#print('odrive command')
command['command'](od, out_data['odrive'])
if 'reset_command' in command:
try:
od.reboot()
except Exception as e:
print(e)
od = odrive.find_any(serial_number=search_serial)
print(f'odrive {search_serial} found')
#index command is necessary. added in odrive handler
out_data['index'] = command['index']
#pos, curr commands automatically done. must be sent in control packets
od.axis0.controller.input_pos = command['axis_0']['pos_command']
od.axis1.controller.input_pos = command['axis_1']['pos_command']
# od.axis0.motor.config.current_lim = command['axis_0']['curr_command']
# od.axis1.motor.config.current_lim = command['axis_1']['curr_command']
out_data['axis_0']['data'] = {
'pos': od.axis0.encoder.pos_estimate,
'vel': od.axis0.encoder.vel_estimate,
'current': od.axis0.motor.current_control.Iq_measured
}
out_data['axis_1']['data'] = {
'pos': od.axis1.encoder.pos_estimate,
'vel': od.axis1.encoder.vel_estimate,
'current': od.axis1.motor.current_control.Iq_measured
}
conn.send(out_data)
def __init__(self):
self.left_offset = 0
self.right_offset = 0
self.left_chain_offset = 0
self.right_chain_offset = 0
self.odrv0 = odrive.find_any()
self.odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
self.odrv0.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
self.odrv1 = odrive.find_any()
self.odrv1.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
self.odrv1.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
self.hold_chain_bar()
def full_reset_and_calibrate(targetID, i=0):
"""Completely resets the Odrive, calibrates axis0 and configures axis0 to only encoder index search on startup and be ready in AXIS_STATE_CLOSED_LOOP_CONTROL"""
print("------------- CALIBRATION ODRV " + str(i) + " -------------\n")
print("Searching for Serial number: " + targetID)
odrv0 = odrive.find_any(serial_number=targetID)
odrv0.erase_configuration()
print("ODRV NO." + str(i) + " " + "Erased [1/7]")
try: # Reboot causes loss of connection, use try to supress errors
odrv0.reboot()
except:
pass
print("ODRV NO." + str(i) + " " + "Rebooted [2/7]")
odrv0 = odrive.find_any(serial_number=targetID) # Reconnect to the Odrive
print("ODRV NO." + str(i) + " " + "Connected [3/7]")
odrv0.axis0.motor.config.pre_calibrated = True # Set all the flags required for pre calibration
odrv0.axis0.encoder.config.pre_calibrated = True
odrv0.axis0.encoder.config.use_index = True
odrv0.axis0.config.startup_encoder_index_search = True # Change startup sequence
odrv0.axis0.config.startup_closed_loop_control = True
odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE # Calibrate
print("ODRV NO." + str(i) + " " + "Started calibration 1 [4/7]", end="")
while odrv0.axis0.current_state != AXIS_STATE_IDLE: # Wait for calibration to be done
time.sleep(0.1)
print(".", end="")
odrv0.save_configuration()
print("\nODRV NO." + str(i) + " " +
"AXIS 1. Calibration complete [5/7]\n")
print("now will begin calibration sequence for second axis for ODRV NO." +
str(i))
time.sleep(3)
odrv0.axis1.motor.config.pre_calibrated = True # Set all the flags required for pre calibration
odrv0.axis1.encoder.config.pre_calibrated = True
odrv0.axis1.encoder.config.use_index = True
odrv0.axis1.config.startup_encoder_index_search = True # Change startup sequence
odrv0.axis1.config.startup_closed_loop_control = True
odrv0.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE # Calibrate
print("ODRV NO." + str(i) + " " + "Started calibration 2 [6/7]", end="")
while odrv0.axis1.current_state != AXIS_STATE_IDLE: # Wait for calibration to be done
time.sleep(0.5)
print(".", end="")
print("\nODRV NO." + str(i) + " " +
"AXIS 2. Calibration 2 complete [7/7]")
#closed loop control for both axis
odrv0.save_configuration()
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
return odrv0
def run_odrive(name, serial_number, d):
atomic_print("looking for "+name+" odrive")
odv = odrive.find_any(serial_number=serial_number)
atomic_print("found " +name+ " odrive")
send_state(odv, AXIS_STATE_IDLE)
try:
while True:
# read from USB Block
lostConnection = True
try:
UDPLock.acquire()
msg = cmd.get()
atomic_print(msg)
lostConnection = False
except timeout:
lostConnection = True
msg = {'t':0, 'f':0}
finally:
UDPLock.release()
# Write to Odrives block
try:
clear_errors(odv)
if lostConnection:
# if msg['f'] == 0 and msg['t'] == 0:
atomic_print("Timeout sending safe")
send_state(odv, AXIS_STATE_IDLE)
else:
send_state(odv, AXIS_STATE_CLOSED_LOOP_CONTROL)
# axis 0 (right) is always same sign
# axis 1 (left) is always opposite sign
odv.axis0.controller.vel_setpoint = d[0]*( \
msg['f'] + msg['t'])
odv.axis1.controller.vel_setpoint = d[1]*( \
msg['f'] - msg['t'])
odv.axis0.watchdog_feed()
odv.axis1.watchdog_feed()
tele[name] = get_data(odv)
except (USBError, ChannelBrokenException) as e:
atomic_print("Lost contact with "+name+" odrive!")
odv = odrive.find_any(serial_number=serial_number)
atomic_print("refound " + name + " odrive")
finally:
atomic_print("Exiting and Sending safe command")
send_state(odv, AXIS_STATE_IDLE)
odv.axis0.controller.vel_setpoint = 0
odv.axis1.controller.vel_setpoint = 0
def clicked():
print("connecting")
global odrv0
global odrv1
odrv0 = odrive.find_any(serial_number = "208037743548") #Connect ot Odrive
print("odrv0 connected")
odrv1 = odrive.find_any(serial_number = "3762364A3137") #Connect ot Odrive
print("odrv1 connected")
# 208037743548 first odrive
# 3762364A3137 new odrive
con = "Connected to Odrive"
lbl.configure(text= con)
CurrentState.configure(text = con)
comboCtrlMode.current(odrv0.axis0.controller.config.control_mode + 1)
def connect(self):
"""
Connect to the ODrive.
"""
#TODO: Search for specific ODrive
if rospy.has_param("{}/serial".format(self.name)):
serial = rospy.get_param("{}/serial".format(self.name))
rospy.loginfo("Waiting for ODrive (serial: {})".format(serial))
self.ODrive = odrive.find_any(serial_number=serial)
else:
rospy.loginfo("Waiting for any ODrive")
self.ODrive = odrive.find_any()
rospy.loginfo("Found ODrive: {}".format(hex(self.ODrive.serial_number).upper()[2:]))
return
def axisSave(self):
print("save button")
self.ui.drive_label.setText("Save & reboot odrive")
self.ui.drive_label.repaint()
print(float(self.ui.Current_Lim.text()),
float(self.ui.Calibrate_current.text()),
float(self.ui.Vel_limit.text()),
float(self.ui.Brake_resistance.text()),
int(self.ui.Pole_pairs.text()),
int(self.ui.Encoder_cnt_rev.text()),
self.ui.Motor_type.currentIndex())
self.axis.motor.config.current_lim = float(self.ui.Current_Lim.text())
self.axis.motor.config.calibration_current = \
float(self.ui.Calibrate_current.text())
self.axis.controller.config.vel_limit = float(self.ui.Vel_limit.text())
self.drive.config.brake_resistance = \
float(self.ui.Brake_resistance.text())
self.axis.motor.config.pole_pairs = int(self.ui.Pole_pairs.text())
self.axis.encoder.config.cpr = int(self.ui.Encoder_cnt_rev.text())
self.axis.motor.config.motor_type = self.ui.Motor_type.currentIndex()
self.drive.save_configuration()
try:
self.drive.reboot()
except:
pass
if self.port:
self.drive = odrive.find_any(self.port)
else:
self.drive = odrive.find_any()
self.ui.drive_label.setText("Odrive {} found".format(
self.drive.serial_number))
self.ui.drive_label.repaint()
if self.ui.configframelabel.text() == "Axis 1":
self.axis = self.drive.axis1
else:
self.axis = self.drive.axis0
def full_reset_and_calibrate(targetSerial):
"""Completely resets the Odrive, calibrates axis1 and configures axis1 to only encoder index search on startup and be ready in AXIS_STATE_CLOSED_LOOP_CONTROL"""
odrv0 = odrive.find_any(serial_number=targetSerial)
print("found odrive with serial target")
odrv0.erase_configuration()
print("Erased [1/7]")
try: # Reboot causes loss of connection, use try to supress errors
odrv0.reboot()
except:
pass
odrv0 = odrive.find_any(serial_number=targetSerial)
print("Rebooted [2/7]")
print("Found odrive with serial " + str(odrv0.serial_number))
print("Connected [3/7]")
odrv0.axis0.motor.config.pre_calibrated = True # Set all the flags required for pre calibration
odrv0.axis0.encoder.config.pre_calibrated = True
odrv0.axis0.encoder.config.use_index = True
odrv0.axis0.config.startup_encoder_index_search = True # Change startup sequence
odrv0.axis0.config.startup_closed_loop_control = True
odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE # Calibrate
print("Started calibration 1 [4/7]", end="")
while odrv0.axis0.current_state != AXIS_STATE_IDLE: # Wait for calibration to be done
time.sleep(0.5)
print(".", end="")
print("\nCalibration 1 complete [5/7]")
odrv0.axis1.motor.config.pre_calibrated = True # Set all the flags required for pre calibration
odrv0.axis1.encoder.config.pre_calibrated = True
odrv0.axis1.encoder.config.use_index = True
odrv0.axis1.config.startup_encoder_index_search = True # Change startup sequence
odrv0.axis1.config.startup_closed_loop_control = True
odrv0.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE # Calibrate
print("Started calibration 2 [6/7]", end="")
while odrv0.axis1.current_state != AXIS_STATE_IDLE: # Wait for calibration to be done
time.sleep(0.5)
print(".", end="")
print("\nCalibration 2 complete [7/7]")
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
odrv0.save_configuration()
return odrv0
def init():
global drive
print("Looking for Odrive...")
drive = od.find_any()
print("Start calibration...")
drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
while drive.axis0.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
# configure the odrive
end_stops_config(5, 6) # pins better not be 1,2 may conflict with UART
traj_param_config(10, 1.3, 1.3)
home_config(0.5, 3)
#home()
# set input mode to trajectory control
drive.axis0.controller.config.input_mode = INPUT_MODE_TRAP_TRAJ
enable_end_stops(False, False)
rospy.loginfo("Calibration done")
rospy.init_node("odrive", anonymous=True)
rospy.Subscriber("boxbot_odrive", Odrive, set_goal)
s = rospy.Service("boxbot_descend", Descend, handle_descend)
rospy.spin()
def odrive_connect():
global odrive_is_connected
global odrv
try:
print('Odrive start connecting')
odrv = odrive.find_any()
except Exception as error:
print("Odrive Error")
print(error)
else:
odrv.axis0.encoder.config.use_index = True
odrv.axis0.controller.config.vel_limit = 2 * 10000
odrv.axis0.trap_traj.config.vel_limit = 1 * 10000
odrv.axis0.trap_traj.config.accel_limit = base_accel * 10000
odrv.axis0.trap_traj.config.decel_limit = base_accel * 10000
odrv.axis0.encoder.config.use_index = True
odrv.axis0.controller.config.vel_limit = 5 * 10000
odrv.axis0.trap_traj.config.vel_limit = 3 * 10000
#another motor
odrv.axis1.encoder.config.use_index = True
odrv.axis1.controller.config.vel_limit = 2 * 10000
odrv.axis1.trap_traj.config.vel_limit = 1 * 10000
odrv.axis1.trap_traj.config.accel_limit = base_accel * 10000
odrv.axis1.trap_traj.config.decel_limit = base_accel * 10000
odrv.axis1.encoder.config.use_index = True
odrv.axis1.controller.config.vel_limit = 10 * 10000
odrv.axis1.trap_traj.config.vel_limit = 9 * 10000
#done
odrive_is_connected = 1
print("Odrive Connected")
def main():
odrv0 = odrive.find_any(timeout=5)
if str(odrv0) == "None":
print("Didn't find an odrive :(")
quit()
else:
print("Found an odrive!")
print("Bus voltage is: ", str(odrv0.vbus_voltage))
if not odrv0.axis0.motor.is_calibrated:
print("Motor not calibrated!")
quit()
if not odrv0.axis0.encoder.is_ready:
print("Encoder not ready!")
odrv0.axis0.requested_state = 6 # Find index pulse
time.sleep(3)
try:
print("Measuring friction")
time.sleep(1)
odrv0.axis0.motor.config.current_lim = 0.1
odrv0.axis0.controller.config.control_mode = 3 # position control
odrv0.axis0.requested_state = 8
margin = 0.05 # 0.05 turns
sum_current = 0
iterations = 10
for i in range(iterations):
break_current = 0
pos_start = odrv0.axis0.encoder.pos_estimate
odrv0.axis0.controller.input_pos = pos_start + 0.25
pos_curr = pos_start
odrv0.axis0.motor.config.current_lim = 0.1
while (pos_curr > pos_start - margin) and (pos_curr <
pos_start + margin):
odrv0.axis0.motor.config.current_lim += 0.05
pos_curr = odrv0.axis0.encoder.pos_estimate
print("start pos: ", pos_start, ". curr pos: ", pos_curr)
print("current limit: ", odrv0.axis0.motor.config.current_lim)
time.sleep(0.05)
break_current = odrv0.axis0.motor.config.current_lim
print("Breaking current ", i, " : ", break_current)
sum_current += break_current
average_break_current = sum_current / iterations
print("\n Average break_current is: ", average_break_current)
except:
print("Exception raised")
finally:
odrv0.axis0.requested_state = 1
print("Ending Program")
quit()
def pos_publisher():
my_odrive = odrive.find_any()
if my_odrive == None:
print("Could not find ODrive, exiting...")
return -1
pub = rospy.Publisher('/avatar/gearShaft2_position_controller/command',
Float64,
queue_size=10)
pub2 = rospy.Publisher('/avatar/mainAxle_position_controller/command',
Float64,
queue_size=10)
rospy.init_node('pos_publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pos_estimate = my_odrive.axis0.encoder.pos_estimate
angle_estimate_rad = (pos_estimate * -6.28) - 0.4
rospy.loginfo(my_odrive.axis0.encoder.pos_estimate)
pos_estimate2 = my_odrive.axis1.encoder.pos_estimate
angle_estimate_rad2 = (pos_estimate2 * -6.28) + 0.7
pub2.publish(angle_estimate_rad2)
# rospy.loginfo(my_odrive.axis1.encoder.pos_estimate)
pub.publish(angle_estimate_rad)
rate.sleep()
def setup():
print("finding Odrives")
odrives = odrive.find_any(find_multiple=3)
print("found Odrives")
# Multiple Odrive setup:
axis_dict = json.loads(open('axis_config.json', "r").read())
output_dict = {}
for od in odrives:
c = axis_dict[str(od.serial_number)]
output_dict[c['axis1']['name']] = Joint(od.axis1, c['axis1']['ratio'])
output_dict[c['axis0']['name']] = Joint(od.axis0, c['axis0']['ratio'])
arm_variables = {'D1': 3.319, 'D2': 3.125, 'A2': 7.913, 'A3': 9.0}
arm_dict = {}
arm_dict['right_arm'] = Arm(output_dict['1 lower'], output_dict['1 upper'],
output_dict['1 shoulder'], arm_variables)
#arm_dict['left_arm'] = Arm(output_dict['2 lower'], output_dict['2 upper'], output_dict['2 shoulder'], arm_variables)
for name, arm in arm_dict.items():
print(f"calibrating {name}")
arm.calibrate_arm()
print(f"homing {name}")
arm.home_arm()
print(f"enabling {name}")
arm.enable_arm()
print("Setup Complete!")
return arm_dict
def main():
odrv0 = odrive.find_any(timeout=5)
if str(odrv0) == "None":
print("Didn't find an odrive :(")
quit()
else:
print("Found an odrive!")
print("Bus voltage is: ", str(odrv0.vbus_voltage))
if not odrv0.axis0.motor.is_calibrated:
print("Motor not calibrated!")
quit()
if not odrv0.axis0.encoder.is_ready:
print("Encoder not ready!")
quit()
print("start pos: ", odrv0.axis0.encoder.pos_estimate)
odrv0.axis0.controller.config.control_mode = 2 # velocity control
odrv0.axis0.requested_state = 8
odrv0.axis0.controller.input_vel = 1.5
odrv0.axis0.motor.config.current_lim = 10
t = time.time()
while (time.time() - t < 5) and abs(
odrv0.axis0.motor.current_control.Iq_measured) < 5:
print(odrv0.axis0.motor.current_control.Iq_measured)
odrv0.axis0.controller.input_vel = 0
pos_right = odrv0.axis0.encoder.pos_estimate
odrv0.axis0.requested_state = 1
print("Exited program.")
quit()
def __init__(self):
self.actual_pos_sim = 0
self.actual_vel_sim = 0
self.cmd_pos_sim = 0
self.sim_cmd_position_pub = rospy.Publisher(
"/avatar/mainAxle_position_controller/command",
Float64,
queue_size=10,
)
rospy.Subscriber(
"/avatar/joint_states",
JointState,
self.update_actual_pos_sim,
)
rospy.Subscriber(
"/avatar/mainAxle_position_controller/state",
JointControllerState,
self.update_cmd_pos_sim,
)
rospy.init_node("ape_haptics", anonymous=True)
self.odrive = odrive.find_any()
if self.odrive == None:
raise RuntimeError("Could not find ODrive")
def full_reset_and_calibrate(odrv0):
"""Completely resets the Odrive, calibrates axis0 and configures axis0 to only encoder index search on startup
and be ready in AXIS_STATE_CLOSED_LOOP_CONTROL"""
odrv0.erase_configuration()
print("Erased.")
try: # Reboot causes loss of connection, use try to supress errors
odrv0.reboot()
except:
pass
print("Rebooted.")
odrv0 = odrive.find_any() # Reconnect to the Odrive
print("Connected.")
odrv0.axis0.motor.config.pre_calibrated = True # Set all the flags required for pre calibration
odrv0.axis0.encoder.config.pre_calibrated = True
odrv0.axis0.encoder.config.use_index = True
odrv0.axis0.config.startup_encoder_index_search = True # Change startup sequence
odrv0.axis0.config.startup_closed_loop_control = True
odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE # Calibrate
print("Started calibration.")
while odrv0.axis0.current_state != AXIS_STATE_IDLE: # Wait for calibration to be done
time.sleep(0.1)
print("Calibration complete.")
odrv0.save_configuration()
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
return odrv0
def launch_dfu(args, logger, cancellation_token):
"""
Waits for a device that matches args.path and args.serial_number
and then upgrades the device's firmware.
"""
serial_number = args.serial_number
find_odrive_cancellation_token = Event(cancellation_token)
logger.info("Waiting for ODrive...")
devices = [None, None]
# Start background thread to scan for ODrives in DFU mode
def find_device_in_dfu_mode_thread():
devices[0] = find_device_in_dfu_mode(serial_number, find_odrive_cancellation_token)
find_odrive_cancellation_token.set()
t = threading.Thread(target=find_device_in_dfu_mode_thread)
t.daemon = True
t.start()
# Scan for ODrives not in DFU mode
# We only scan on USB because DFU is only implemented over USB
devices[1] = odrive.find_any("usb", serial_number,
find_odrive_cancellation_token, cancellation_token)
find_odrive_cancellation_token.set()
device = devices[0] or devices[1]
firmware = FirmwareFromFile(args.file) if args.file else None
update_device(device, firmware, logger, cancellation_token)
def __init__(self):
print("finding an odrive...")
self.odrv0 = odrive.find_any()
self.axis0 = self.odrv0.axis0
self.axis1 = self.odrv0.axis1
print('Odrive found ! ')
async def setup(self, settings):
if self.enabled:
self.odrive = odrive.find_any(
serial_number=str(self.odrive_serial_number))
self.set_settings(settings)
# put all axes in idle state
self.disable()
def connect(self, port=None, right_axis=0, timeout=30):
if self.driver:
self.logger.info(
"Already connected. Disconnecting and reconnecting.")
try:
self.driver = odrive.find_any(serial_number="208A358E524B",
timeout=timeout,
logger=self.logger)
self.axes = (self.driver.axis0, self.driver.axis1)
except:
self.logger.error("No ODrive found. Is device powered?")
return False
# save some parameters for easy access
self.right_axis = self.driver.axis0 if right_axis == 0 else self.driver.axis1
self.left_axis = self.driver.axis1 if right_axis == 0 else self.driver.axis0
# check for no errors
for axis in [self.right_axis, self.left_axis]:
if axis.error != 0:
error_str = "Had error on startup, rebooting. Axis error 0x%x, motor error 0x%x, encoder error 0x%x. Rebooting." % (
axis.error, axis.motor.error, axis.encoder.error)
self.logger.error(error_str)
self.reboot()
return False
self.encoder_cpr = self.driver.axis0.encoder.config.cpr
self.connected = True
self.logger.info("Connected to ODrive. " + self.get_version_string())
self._preroll_started = True
self._preroll_completed = True
return True
def connect(self, port=None, right_axis=0):
if self.driver:
self.logger.info(
"Already connected. Disconnecting and reconnecting.")
try:
self.driver = odrive.find_any(path="usb",
serial_number="207B37943548",
search_cancellation_token=None,
channel_termination_token=None,
timeout=30,
logger=self.logger)
#207B37943548
self.axes = (self.driver.axis1)
except:
self.logger.error(
"No ODrive found. Is device powered? Is the Serial Number Correct?"
)
return False
# save some parameters for easy access
self.right_axis = self.driver.axis1
self.encoder_cpr = self.driver.axis1.encoder.config.cpr
self.connected = True
return True
def init():
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
global my_drive
my_drive = odrive.find_any()
# To read a value, simply read the property
print("Odrive found, Shit is running at " + str(my_drive.vbus_voltage) + "V")
my_drive.axis0.controller.config.pos_gain = 1
print("axis 0 pos_gain is " + str(my_drive.axis0.controller.config.pos_gain))
my_drive.axis1.controller.config.pos_gain = 1
print("axis 1 pos_gain is " + str(my_drive.axis1.controller.config.pos_gain))
my_drive.axis0.controller.config.vel_gain = 0.25 #0.02
print("axis 0 vel_gain is " + str(my_drive.axis0.controller.config.vel_gain))
my_drive.axis1.controller.config.vel_gain = 0.25 #0.02
print("axis 1 vel_gain is " + str(my_drive.axis1.controller.config.vel_gain))
my_drive.axis0.controller.config.vel_integrator_gain = 1
print("axis 0 vel_integrator_gain is " + str(my_drive.axis0.controller.config.vel_integrator_gain))
my_drive.axis1.controller.config.vel_integrator_gain = 1
print("axis 1 vel_integrator_gain is " + str(my_drive.axis1.controller.config.vel_integrator_gain))
my_drive.axis0.controller.config.vel_limit = 500
my_drive.axis1.controller.config.vel_limit = 500
def odriveMotorConfiguration():
# configure ODrive for hoverboard motor / hall effect
# feedback. Returns a boolean for config success/failure.
# Reference: https://docs.odriverobotics.com/hoverboard.
# Raises ValueError for odrive motor config failure
odrv0 = odrive.find_any()
odrv0.axis0.motor.config.resistance_calib_max_voltage = 4
odrv0.axis0.motor.config.requested_current_range = 25 #Requires config save and reboot
odrv0.axis0.motor.config.current_control_bandwidth = 100
odrv0.axis0.encoder.config.mode = ENCODER_MODE_HALL
odrv0.axis0.encoder.config.cpr = 90
odrv0.axis0.encoder.config.bandwidth = 100
odrv0.axis0.controller.config.pos_gain = 1
odrv0.axis0.controller.config.vel_gain = 0.02
odrv0.axis0.controller.config.vel_integrator_gain = 0.1
odrv0.axis0.controller.config.vel_limit = 1000
odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
odrv0.save_configuration()
odrv0.reboot()
odrv0.axis0.requested_state = AXIS_STATE_MOTOR_CALIBRATION
try:
if error != 0 : raise Exception('unable to configure ODrive')
if phase_inductance != 0.00033594953129068017 : raise Exception('unable to configure ODrive')
if phase_resistance != 0.1793474406003952 : raise Exception('unable to configure ODrive')
except Exception:
return False
odrv0.axis0.motor.config.pre_calibrated = True
odrv0.save_configuration()
odrv0.reboot()
odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION
time.sleep(5)
return True
def connect(self, port=None, right_axis=0):
if self.driver:
self.logger.info("Already connected. Disconnecting and reconnecting.")
try:
self.driver = odrive.find_any(timeout=30, logger=self.logger)
self.axes = (self.driver.axis0, self.driver.axis1)
except:
self.logger.error("No ODrive found. Is device powered?")
return False
# save some parameters for easy access
if self.calibrate_axis == 1:
self.right_axis = self.driver.axis0 if right_axis == 0 else self.driver.axis1
self.right_axis.encoder.config.cpr = self.user_cpr
self.logger.info("Right Motor Connected with CPR = %d" % self.right_axis.encoder.config.cpr)
else:
self.left_axis = self.driver.axis1 if right_axis == 0 else self.driver.axis0
self.left_axis.encoder.config.cpr = self.user_cpr
self.logger.info("Left Motor Connected with CPR = %d" % self.left_axis.encoder.config.cpr)
self.encoder_cpr = self.user_cpr
self.connected = True
return True
def find_drive():
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
found_drive = odrive.find_any()
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#my_drive = odrive.find_any("serial:/dev/ttyUSB0")
return found_drive
def listener():
global my_drive, vels
# print("looking for odrive")
my_drive = odrive.find_any()
my_drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
my_drive.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
while my_drive.axis0.current_state != AXIS_STATE_IDLE:
rospy.sleep(0.1)
my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
my_drive.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
my_drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
my_drive.axis1.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=False)
rospy.Subscriber("/cmd_vel", Twist, callback)
rospy.Timer(rospy.Duration(POLL_TIME), poll, oneshot=False)
rospy.Subscriber("/turn", Float64, turn)
rospy.Subscriber("/turnslow", Float64, turnslow)
rospy.Subscriber("/drive", Float64, drive)
hi = rospy.Publisher('motor_listener_listening', String, queue_size=10)
hi.publish(String('Ready'))
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def rediscover(self):
"""
Reconnects to the ODrive
"""
self.handle = odrive.find_any(
path="usb", serial_number=self.yaml['serial-number'], timeout=15)#, printer=print)
for axis_idx, axis_ctx in enumerate(self.axes):
axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)]
def connect(self, port=None, right_axis=0):
if self.driver:
self.logger.info("Already connected. Disconnecting and reconnecting.")
try:
self.driver = odrive.find_any(path="usb", serial_number="206430804648", search_cancellation_token=None, channel_termination_token=None, timeout=30, logger=self.logger)
self.axes = (self.driver.axis0, self.driver.axis1)
except:
self.logger.error("No ODrive found. Is device powered?")
return False
# save some parameters for easy access
self.right_axis = self.driver.axis0 if right_axis == 0 else self.driver.axis1
self.left_axis = self.driver.axis1 if right_axis == 0 else self.driver.axis0
self.encoder_cpr = self.driver.axis0.encoder.config.cpr
self.connected = True
return True
# based on https://docs.odriverobotics.com/hoverboard.html
import odrive
import time
motor_calibration_time = 6
encoder_calibration_time = 12
input("Make sure that the wheels are free to turn. Don't get hurt!\nPress ENTER to proceed.")
print("Searching for ODrive...")
odrv0 = odrive.find_any()
print("Odrive detected!")
print("--------------Motor Calibration--------------------------------")
print("Calibrating motors...")
odrv0.axis1.requested_state = 4 # AXIS_STATE_MOTOR_CALIBRATION
time.sleep(motor_calibration_time)
if odrv0.axis1.motor.is_calibrated == 1:
print("Motor 1 successfully calibrated! Proceeding...")
else:
print("Could not calibrate motor 1. Something is wrong.")
print("Have you ran the first script to set all parameters?")
print("If yes and the wiring looks good, reset the ODrive and try again.\nExiting.")
quit()
print("Saving motor calibration")
odrv0.axis1.motor.config.pre_calibrated = True
print("--------------Encoder calibration------------------------------")
print("Calibrating encoder offset...")
odrv0.axis1.requested_state = 7 # AXIS_STATE_ENCODER_OFFSET_CALIBRATION
time.sleep(encoder_calibration_time)
if odrv0.axis1.encoder.is_ready == 1:
def update_device(device, firmware, logger, cancellation_token):
"""
Updates the specified device with the specified firmware.
The device passed to this function can either be in
normal mode or in DFU mode.
The firmware should be an instance of Firmware or None.
If firmware is None, the newest firmware for the device is
downloaded from GitHub releases.
"""
if isinstance(device, usb.core.Device):
serial_number = device.serial_number
dfudev = DfuDevice(device)
if (logger._verbose):
logger.debug("OTP:")
dump_otp(dfudev)
# Read hardware version from one-time-programmable memory
otp_sector = [s for s in dfudev.sectors if s['name'] == 'OTP Memory' and s['addr'] == 0x1fff7800][0]
otp_data = dfudev.read_sector(otp_sector)
if otp_data[0] == 0:
otp_data = otp_data[16:]
if otp_data[0] == 0xfe:
hw_version = (otp_data[3], otp_data[4], otp_data[5])
else:
hw_version = (0, 0, 0)
else:
serial_number = device.__channel__.usb_device.serial_number
dfudev = None
# Read hardware version as reported from firmware
hw_version_major = device.hw_version_major if hasattr(device, 'hw_version_major') else 0
hw_version_minor = device.hw_version_minor if hasattr(device, 'hw_version_minor') else 0
hw_version_variant = device.hw_version_variant if hasattr(device, 'hw_version_variant') else 0
hw_version = (hw_version_major, hw_version_minor, hw_version_variant)
if hw_version < (3, 5, 0):
print(" DFU mode is not supported on board version 3.4 or earlier.")
print(" This is because entering DFU mode on such a device would")
print(" break the brake resistor FETs under some circumstances.")
print("Warning: DFU mode is not supported on ODrives earlier than v3.5 unless you perform a hardware mod.")
if not odrive.utils.yes_no_prompt("Do you still want to continue?", False):
raise OperationAbortedException()
fw_version_major = device.fw_version_major if hasattr(device, 'fw_version_major') else 0
fw_version_minor = device.fw_version_minor if hasattr(device, 'fw_version_minor') else 0
fw_version_revision = device.fw_version_revision if hasattr(device, 'fw_version_revision') else 0
fw_version_prerelease = device.fw_version_prerelease if hasattr(device, 'fw_version_prerelease') else True
fw_version = (fw_version_major, fw_version_minor, fw_version_revision, fw_version_prerelease)
print("Found ODrive {} ({}) with firmware {}{}".format(
serial_number,
get_hw_version_string(hw_version),
get_fw_version_string(fw_version),
" in DFU mode" if dfudev is not None else ""))
if firmware is None:
if hw_version == (0, 0, 0):
if dfudev is None:
suggestion = 'You have to manually flash an up-to-date firmware to make automatic checks work. Run `odrivetool dfu --help` for more info.'
else:
suggestion = 'Run "make write_otp" to program the board version.'
raise Exception('Cannot check online for new firmware because the board version is unknown. ' + suggestion)
print("Checking online for newest firmware...", end='')
firmware = get_newest_firmware(hw_version)
if firmware is None:
raise Exception("could not find any firmware release for this board version")
print(" found {}".format(get_fw_version_string(firmware.fw_version)))
if firmware.fw_version <= fw_version:
print()
if firmware.fw_version < fw_version:
print("Warning: you are about to flash firmware {} which is older than the firmware on the device ({}).".format(
get_fw_version_string(firmware.fw_version),
get_fw_version_string(fw_version)))
else:
print("You are about to flash firmware {} which is the same version as the firmware on the device ({}).".format(
get_fw_version_string(firmware.fw_version),
get_fw_version_string(fw_version)))
if not odrive.utils.yes_no_prompt("Do you want to flash this firmware anyway?", False):
raise OperationAbortedException()
# load hex file
# TODO: Either use the elf format or pack a custom format with a manifest.
# This way we can for instance verify the target board version and only
# have to publish one file for every board (instead of elf AND hex files).
hexfile = IntelHex(firmware.get_as_hex())
logger.debug("Contiguous segments in hex file:")
for start, end in hexfile.segments():
logger.debug(" {:08X} to {:08X}".format(start, end - 1))
# Back up configuration
if dfudev is None:
do_backup_config = device.user_config_loaded if hasattr(device, 'user_config_loaded') else False
if do_backup_config:
odrive.configuration.backup_config(device, None, logger)
elif not odrive.utils.yes_no_prompt("The configuration cannot be backed up because the device is already in DFU mode. The configuration may be lost after updating. Do you want to continue anyway?", True):
raise OperationAbortedException()
# Put the device into DFU mode if it's not already in DFU mode
if dfudev is None:
find_odrive_cancellation_token = Event(cancellation_token)
put_into_dfu_mode(device, find_odrive_cancellation_token)
stm_device = find_device_in_dfu_mode(serial_number, cancellation_token)
find_odrive_cancellation_token.set()
dfudev = DfuDevice(stm_device)
logger.debug("Sectors on device: ")
for sector in dfudev.sectors:
logger.debug(" {:08X} to {:08X} ({})".format(
sector['addr'],
sector['addr'] + sector['len'] - 1,
sector['name']))
# fill sectors with data
touched_sectors = list(populate_sectors(dfudev.sectors, hexfile))
logger.debug("The following sectors will be flashed: ")
for sector,_ in touched_sectors:
logger.debug(" {:08X} to {:08X}".format(sector['addr'], sector['addr'] + sector['len'] - 1))
# Erase
try:
for i, (sector, data) in enumerate(touched_sectors):
print("Erasing... (sector {}/{}) \r".format(i, len(touched_sectors)), end='', flush=True)
dfudev.erase_sector(sector)
print('Erasing... done \r', end='', flush=True)
finally:
print('', flush=True)
# Flash
try:
for i, (sector, data) in enumerate(touched_sectors):
print("Flashing... (sector {}/{}) \r".format(i, len(touched_sectors)), end='', flush=True)
dfudev.write_sector(sector, data)
print('Flashing... done \r', end='', flush=True)
finally:
print('', flush=True)
# Verify
try:
for i, (sector, expected_data) in enumerate(touched_sectors):
print("Verifying... (sector {}/{}) \r".format(i, len(touched_sectors)), end='', flush=True)
observed_data = dfudev.read_sector(sector)
mismatch_pos = get_first_mismatch_index(observed_data, expected_data)
if not mismatch_pos is None:
mismatch_pos -= mismatch_pos % 16
observed_snippet = ' '.join('{:02X}'.format(x) for x in observed_data[mismatch_pos:mismatch_pos+16])
expected_snippet = ' '.join('{:02X}'.format(x) for x in expected_data[mismatch_pos:mismatch_pos+16])
raise RuntimeError("Verification failed around address 0x{:08X}:\n".format(sector['addr'] + mismatch_pos) +
" expected: " + expected_snippet + "\n"
" observed: " + observed_snippet)
print('Verifying... done \r', end='', flush=True)
finally:
print('', flush=True)
# If the flash operation failed for some reason, your device is bricked now.
# You can unbrick it as long as the device remains powered on.
# (or always with an STLink)
# So for debugging you should comment this last part out.
# Jump to application
dfudev.jump_to_application(0x08000000)
logger.info("Waiting for the device to reappear...")
device = odrive.find_any("usb", serial_number,
cancellation_token, cancellation_token, timeout=30)
if do_backup_config:
odrive.configuration.restore_config(device, None, logger)
os.remove(odrive.configuration.get_temp_config_filename(device))
logger.success("Device firmware update successful.")
import odrive
from odrive.utils import dump_errors
from odrive.enums import *
import time
print("Finding an odrive...")
odrv = odrive.find_any()
# axes = [odrv.axis0, odrv.axis1];
axes = [odrv.axis0];
flip_index_search_direction = False
save_and_reboot = True
print("Setting config...")
# Settings to protect battery
odrv.config.dc_bus_overvoltage_trip_level = 14.8
odrv.config.dc_bus_undervoltage_trip_level = 8.0
odrv.config.brake_resistance = 0
for ax in axes:
ax.motor.config.requested_current_range = 25
ax.motor.config.calibration_current = 10
ax.motor.config.current_lim = 10
ax.motor.config.resistance_calib_max_voltage = 4
ax.motor.config.pole_pairs = 10
ax.encoder.config.cpr = 4096
ax.encoder.config.use_index = True
ax.encoder.config.find_idx_on_lockin_only = True
ax.encoder.config.idx_search_unidirectional = True
#!/usr/bin/env python3
"""
Example usage of the ODrive python library to monitor and control ODrive devices
"""
from __future__ import print_function
import odrive
from odrive.enums import *
import time
import math
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
my_drive = odrive.find_any()
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#my_drive = odrive.find_any("serial:/dev/ttyUSB0")
# Calibrate motor and wait for it to finish
print("starting calibration...")
my_drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
while my_drive.axis0.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
# To read a value, simply read the property
print("Bus voltage is " + str(my_drive.vbus_voltage) + "V")
# Or to change a value, just assign to the property
