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 check_errors(axis):
if axis.error != 0:
print("ERROR:")
dump_errors(odrv_YZ, True)
dump_errors(odrv_X, True)
print("Quiting due to error...")
sys.exit()
def clear_errors(self, event):
# Non-critical code for clearing error states. To be run with ros.timer
if self.driver.axis0.error or self.driver.axis1.error:
rospy.logerr("Axis Error %s %s" %
(self.driver.axis0.error, self.driver.axis1.error))
rospy.logerr("Controller Error %s %s" %
(self.driver.axis0.controller.error,
self.driver.axis1.controller.error))
o_utils.dump_errors(self.driver, clear=True)
def clearE(self):
# Edit by GGC on June 28
# An attempt to clear odrive errors
if not self.driver:
self.logger.error("Not connected.")
return False
else:
print(dump_errors(self.driver))
dump_errors(self.driver, True)
return True
def on_event(self, event):
global modrive
if (event == "arm cmd"):
modrive.arm()
return ArmedState()
if (modrive.check_errors()):
print("clearing calibration errors")
dump_errors(modrive.odrive, True)
return self
def wait_and_exit_on_error(ax):
while ax.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
for odrv in odrvs:
odrv.axis0.watchdog_feed()
odrv.axis1.watchdog_feed()
if ax.error != errors.axis.ERROR_NONE:
for odrv in odrvs:
if (ax == odrv.axis0 or ax == odrv.axis1):
dump_errors(odrv, True)
exit()
def check_errors(self):
"""
Check to see if the odrive encountered any errors, will just eject if errors are found.
"""
self._check_connected()
for axis in self.axes.values():
if axis.error != 0:
print("ERROR:")
dump_errors(self.odrv_YZ, True)
dump_errors(self.odrv_X, True)
print("Quiting due to error...")
sys.exit()
def on_event(self, event):
"""
Handle events that are delegated to the Error State.
"""
global modrive
dump_errors(modrive.odrive, True)
if (event == "odrive error"):
try:
modrive.reboot() # only runs after initial pairing
except:
print('channel error caught')
return DisconnectedState()
return self
def calibrate(self):
dump_errors(self.odrive, True) # clears all odrive encoder errors
self._requested_state(AXIS_STATE_FULL_CALIBRATION_SEQUENCE)
front_state, back_state = self.get_current_state()
# if both axes are idle it means its done calibrating
while (front_state != AXIS_STATE_IDLE
or back_state != AXIS_STATE_IDLE):
front_state, back_state = self.get_current_state()
pass
self._pre_calibrate(self.front_axis)
self._pre_calibrate(self.back_axis)
self.odrive.save_configuration()
def __init__(self):
# CONSTANTS
self.MIN_MOTOR_RADS = 104 # So min wheel speed is ~50 RPM
self.MAX_MOTOR_RADS = 471 # So max wheel speed is ~200 RPM
self.MAX_EFFORT = 20
# Encoder counts per radian the MOTOR has turned
self.ENCODER_COUNTS_PER_RAD = 4000 / (2 * 3.1415926)
# Encoder counts from the wheels
self._encoder_counts = [0, 0]
# Find the Odrive - block until received
print("\033[1;31mWaiting for Odrive...\033[0m")
self._odrv = odrive.find_any()
print("\033[1;32m...Found Odrive\033[0m")
# Do initial callibration
self._odrv.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
self._odrv.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
while (self._odrv.axis0.current_state != AXIS_STATE_IDLE
and self._odrv.axis1.current_state != AXIS_STATE_IDLE):
time.sleep(0.1)
if self._odrv.axis0.error != 0x00 or self._odrv.axis1.error != 0x00:
dump_errors(self._odrv, True)
raise RuntimeError("Failed to calibrate axis")
# Set ourselves to the reset state
self.reset()
# Ensure that we're in velocity control
self._odrv.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
self._odrv.axis1.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
self._ctrl_modes = [
CTRL_MODE_VELOCITY_CONTROL, CTRL_MODE_VELOCITY_CONTROL
]
def start(odrv):
print("--------------------------------------")
dump_errors(odrv, True)
if (odrv.axis0.encoder.config.pre_calibrated
and odrv.axis1.encoder.config.pre_calibrated) != 1:
print(
"System not calibrated - proceeding to calibration based on index search"
)
calibrate.first_time_calibration(odrv)
configure.currents(odrv)
configure.velocity_limit(odrv)
configure.gains(odrv)
configure.trap_traj(odrv, vel_lim=1, accel_lim=8)
calibrate.set_encoder_zero(odrv)
time.sleep(.2)
update_time_errors(odrv)
print("DONE start robo")
print("--------------------------------------")
print()
def start(odrv):
dump_errors(odrv, True)
if (odrv.axis0.encoder.config.pre_calibrated
and odrv.axis1.encoder.config.pre_calibrated) != 1:
print(
"System not calibrated - proceeding to calibration based on index search"
)
calibrate.first_time_calibration(odrv)
configure.currents(odrv)
configure.velocity_limit(odrv)
configure.gains(odrv,
gan_pos=25,
gan_vel=250 / 1000.0,
gan_int_vel=400 / 1000.0)
configure.trap_traj(odrv, vel_lim=6, accel_lim=48)
calibrate.set_encoder_zero(odrv)
time.sleep(.2)
update_time_errors(odrv)
return "DONE start robo"
def on_event(self, event):
"""
Handle events that are delegated to the Error State.
"""
global modrive
print(dump_errors(modrive.odrive, True))
if (event == OdriveEvent.ODRIVE_ERROR):
try:
modrive.reboot() # only runs after initial pairing
except Exception:
pass
return DisconnectedState()
return self
def set_params(ax):
dump_errors(ax, True)
odrv.axis0.watchdog_feed()
odrv.axis1.watchdog_feed()
# --- Time BEGIN here
readwrite_time_start = rospy.Time.now().to_sec()
for i in range(0, 10001):
# Either read or write 10000 times, NOT both!
# ----- MOTOR (READ) -----
# print("Calibrating ODrive # {}".format(to_calib.index(odrv)))
# print("motor config.resistance_calib_max_voltage is " + str(ax.motor.config.resistance_calib_max_voltage))
# # ----- ENCODER (WRITE) -----
# print('assigning new bandwidth...')
ax.encoder.config.bandwidth = 100
# print("Loop Counter: " + str(i))
# -- Time STOP: Calculate time taken to reset ODrive
rospy.logdebug(
"Reading/Writing took {} seconds".format(rospy.Time.now().to_sec() -
readwrite_time_start))
import odrive
from odrive.enums import *
from odrive.utils import start_liveplotter, dump_errors
import time
import math
import numpy
import time
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
my_drive = odrive.find_any()
dump_errors(my_drive,True)
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#my_drive = odrive.find_any("serial:/dev/ttyUSB0")
#use the plotter to monitor the current and position
# start_liveplotter(lambda: [(my_drive.axis0.motor.current_control.Iq_measured*100),my_drive.axis0.encoder.count_in_cpr])
start_liveplotter(lambda: [(my_drive.axis0.controller.input_pos),my_drive.axis0.encoder.pos_estimate])
# 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
time.sleep(5)
# EXAMPLE
# Read and print the voltage property
def clear_errors(odrv):
dump_errors(odrv, True)
def wait_and_exit_on_error(ax):
while ax.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
if ax.error != errors.axis.ERROR_NONE:
dump_errors(odrv, True)
exit()
def check_error(odrv_axis, message):
if (odrv_axis.error != 0):
print(dump_errors(odrv_axis))
sys.exit(message)
def clear_errors(self, event):
# Non-critical code for clearing error states. To be run with ros.timer
if port == "207C37863548" and self.driver.axis0.error:
rospy.logerr(self.driver.axis0.error)
o_utils.dump_errors(self.driver, clear=True)
def __init__(self, timeout):
# specify left, middle, and right ODrives
rospy.loginfo("Looking for ODrives...")
self.SERIAL_NUMS = [
35593293288011, # Left, 0
35623406809166, # Middle, 1
35563278839886
] # Right, 2
rospy.loginfo("Getting odrives first time")
self.get_odrives()
rospy.loginfo("Erasing and restarting odrives")
for odrv in self.odrvs:
odrv.erase_configuration()
try:
odrv.reboot()
except ChannelBrokenException:
pass
rospy.loginfo("Reconnecting to odrives")
self.get_odrives()
rospy.loginfo("Reconnected!")
# Set left and right axis
self.leftAxes = [
self.odrvs[0].axis0, self.odrvs[0].axis1, self.odrvs[1].axis1
]
self.rightAxes = [
self.odrvs[1].axis0, self.odrvs[2].axis0, self.odrvs[2].axis1
]
self.axes = self.leftAxes + self.rightAxes
# Set axis state
for ax in (self.leftAxes + self.rightAxes):
ax.watchdog_feed()
# Clear errors
for odrv in self.odrvs:
dump_errors(odrv, True)
odrv.config.brake_resistance = 0.5
for ax in (self.leftAxes + self.rightAxes):
ax.controller.config.vel_gain = 0.01
ax.controller.config.vel_integrator_gain = 0.05
ax.controller.config.control_mode = 2
ax.controller.vel_setpoint = 400
ax.motor.config.direction = 1
ax.sensorless_estimator.config.pm_flux_linkage = 5.51328895422 / (
7 * 140) # pole pairs = 7, motor kv = 140KV
# increase current_lim_tolerance
ax.motor.config.current_lim_tolerance = 20
# set to ignore illegal hall state and save all changes
ax.encoder.config.ignore_illegal_hall_state = True
# calibrate motor
ax.requested_state = AXIS_STATE_MOTOR_CALIBRATION
self.wait_and_exit_on_error(ax)
ax.requested_state = AXIS_STATE_SENSORLESS_CONTROL
ax.controller.vel_setpoint = 0
# Sub to topic
rospy.Subscriber('joy', Joy, self.vel_callback)
# Set first watchdog
self.timeout = timeout # log error if this many seconds occur between received messages
self.timer = rospy.Timer(rospy.Duration(self.timeout),
self.watchdog_callback,
oneshot=True)
self.watchdog_fired = False
# Init other variables
self.last_msg_time = 0
self.last_recv_time = 0
self.next_wd_feed_time = 0
rospy.loginfo("Ready for topic")
rospy.spin()
def __init__(self, timeout):
# specify left, middle, and right ODrives
rospy.loginfo("Looking for ODrives...")
self.SERIAL_NUMS = [
35593293288011, # Left, 0
35623406809166, # Middle, 1
35563278839886] # Right, 2
self.odrvs = [
None,
None,
None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in self.SERIAL_NUMS:
self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(self.SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in self.odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None, Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# self.odrv0 = odrive.find_any()
# # odrv1 = odrive.find_any()
# # odrv2 = odrive.find_any()
rospy.loginfo("Found ODrives")
# Set left and right axis
self.leftAxes = [self.odrvs[0].axis0, self.odrvs[0].axis1, self.odrvs[1].axis1]
self.rightAxes = [self.odrvs[1].axis0, self.odrvs[2].axis0, self.odrvs[2].axis1]
self.axes = self.leftAxes + self.rightAxes
# Set axis state
rospy.logdebug("Enabling Watchdog")
for ax in (self.leftAxes + self.rightAxes):
ax.watchdog_feed()
ax.config.watchdog_timeout = 2
ax.encoder.config.ignore_illegal_hall_state = True
# Clear errors
for odrv in self.odrvs:
dump_errors(odrv, True)
for ax in (self.leftAxes + self.rightAxes):
ax.controller.vel_ramp_enable = True
ax.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
ax.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
# Sub to topic
rospy.Subscriber('joy', Joy, self.vel_callback)
# Set first watchdog
self.timeout = timeout # log error if this many seconds occur between received messages
self.timer = rospy.Timer(rospy.Duration(self.timeout), self.watchdog_callback, oneshot=True)
self.watchdog_fired = False
# Init other variables
self.last_msg_time = 0
self.last_recv_time = 0
self.next_wd_feed_time = 0
rospy.loginfo("Ready for topic")
rospy.spin()
from odrive.enums import *
from odrive.utils import start_liveplotter, dump_errors
import time
print("connecting")
# odrv1 = odrive.find_any(serial_number = "3762364A3137") #Connect ot Odrive1
print("odrv1 connected")
odrv0 = odrive.find_any(serial_number = "208037743548") #Connect ot Odrive0
print("odrv0 connected")
# Find a connected ODrive (this will block until you connect one)
# print("finding an odrive...")
# odrv0 = odrive.find_any()
dump_errors(odrv0,False)
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#odrv0 = odrive.find_any("serial:/dev/ttyUSB0")
#use the plotter to monitor the current and position
# start_liveplotter(lambda: [(odrv0.axis0.motor.current_control.Iq_measured*100),odrv0.axis0.encoder.count_in_cpr])
# start_liveplotter(lambda: [(odrv0.axis0.controller.input_pos),odrv0.axis0.encoder.pos_estimate])
time.sleep(2)
# EXAMPLE
# Read and print the voltage property
print("Bus voltage is " + str(odrv0.vbus_voltage) + "V")
def idle_wait():
while odrv0.axis1.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
print(dump_errors(odrv0))
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
odrv0 = OdriveManager(path='/dev/ttySC0',
serial_number='336B31643536').find_odrive()
# odrv0 = odrive.find_any()
print(dump_errors(odrv0, True))
print(dump_errors(odrv0, True))
odrv0.axis1.requested_state = AXIS_STATE_IDLE
odrv0.axis1.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL
odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#odrv0 = odrive.find_any("serial:/dev/ttyUSB0")
odrv0.axis1.motor.config.current_control_bandwidth = 20
odrv0.axis1.encoder.config.bandwidth = 100
odrv0.axis1.motor.current_control.p_gain = 0.3
odrv0.axis1.motor.current_control.i_gain = 0
odrv0.axis1.motor.current_control.final_v_beta = 0.1 # Voltage Ramp Rate
def wait_and_exit_on_error(ax):
while ax.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
if ax.error != errors.axis.ERROR_NONE:
dump_errors(odrv, True)
exit()
# for odrv in odrives:
# print(odrv.serial_number)
# if odrv.serial_number == "208037743548":
# odrv_YZ = odrv
# elif odrv.serial_number == "3762364A3137":
# odrv_X = odrv
# else:
# print("Found unkown odrive")
print("finding YZ odrive...")
odrv_YZ = odrive.find_any(serial_number="208037743548")
print("finding X odrive...")
odrv_X = odrive.find_any(serial_number="3762364A3137")
print("Found, prexisting errors:")
dump_errors(odrv_YZ, True)
dump_errors(odrv_X, True)
# Generic config
# odrv_YZ.config.enable_brake_resistor = True
odrv_YZ.config.brake_resistance = 0.4690000116825104
# odrv_X.config.enable_brake_resistor = True
odrv_X.config.brake_resistance = 0.4690000116825104
# Individual setup
# Z SETUP
odrv_YZ.axis0.motor.config.resistance_calib_max_voltage = 5.0
odrv_YZ.axis0.controller.config.pos_gain = 20.0
odrv_YZ.axis0.controller.config.vel_gain = 0.16
odrv_YZ.axis0.controller.config.vel_integrator_gain = 0.32
def loco_dump_errors(self):
dump_errors(odrv1, True)
def clear_errors(self):
dump_errors(self._odrv, True)
def idle_wait():
while odrv0.axis1.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
print(dump_errors(odrv0))
if __name__ == '__main__':
try:
odrive.find_any().reboot()
sleep(2)
pass
except:
pass
drv = Odrive()
drv.axis0.requested_state = 3 #CALIBRATION
sleep(15)
ut.dump_errors(drv.od)
# print(drv.enc0.config.cpr)
# print(drv.od.axis0.motor.config.pole_pairs)
drv.axis0.requested_state = 8
# sleep(1)
# for i in range(1):
# drv.pos0 = 0
# sleep(3)
# drv.pos0 = 5
# sleep(3)
# #ut.dump_errors(drv.od)
drv.axis1.requested_state = 3 #CALIBRATION
sleep(15)
ut.dump_errors(drv.od)
from __future__ import print_function
import odrive
from odrive.tests import *
from odrive.utils import dump_errors
from odrive.enums import *
from odrive.utils import start_liveplotter
import time
print("finding an odrive...")
odrv = odrive.find_any()
#axis = odrv.axis0
dump_errors(odrv, True)
exit
def dig_dump_errors(self):
dump_errors(odrv0, True)