def debug_robot(): rc.Open('/dev/ttyUSB0', 115200) while True: status = rc.ReadError(MTR_ADDRESS)[1] encoder = rc.ReadEncM1(MTR_ADDRESS) print encoder
def config(): rc.Open('/dev/ttyUSB1',115200) print rc.ReadMinMaxMainVoltages(MTR_ADDRESS) rc.SetPinFunctions(MTR_ADDRESS,2,2,2) aa = rc.ReadPinFunctions(MTR_ADDRESS) print aa rc.WriteNVM(MTR_ADDRESS)
def debug_robot(): rc.Open('/dev/ttyUSB0',115200) while True: status = rc.ReadError(MTR_ADDRESS)[1] dummy,c1,c2 = rc.ReadCurrents(MTR_ADDRESS) bvoltage =rc.ReadMainBatteryVoltage(MTR_ADDRESS)[1] / 10.0 diagstr = "BattVoltage %f, Current[%f,%f], Status 0x%x" % (bvoltage, c1/100.0, c2/100.0, status) print diagstr
def move_robot(): global pub global last_time rc.Open('/dev/ttyUSB0', 115200) print rc.ReadMinMaxMainVoltages(MTR_ADDRESS) pub = rospy.Publisher('motor_diagnostics', String, queue_size=10) rospy.init_node('roboclaw_ifc') last_time = rospy.get_rostime() rospy.Timer(rospy.Duration(1), timer_callback) rospy.Subscriber("/cmd_vel", Twist, twist_callback, queue_size=1) rospy.spin() rc.DutyM1M2(MTR_ADDRESS, 0, 0)
def move_robot(): global pub global last_time rc.Open('/dev/ttyUSB0',115200) print rc.ReadMinMaxMainVoltages(MTR_ADDRESS) rospy.init_node('roboclaw_ifc') vel_list = [.2, .4 , .2 , .1 , .0] for vel in vel_list: speed = int(16484 * vel) print speed rc.SpeedM1M2(MTR_ADDRESS, speed, speed) rospy.sleep(5) rc.DutyM1M2(MTR_ADDRESS, 0, 0)
def shutdown(self): """Handle shutting down the node""" rospy.loginfo("Shutting down") if hasattr(self, "sub"): self.sub.unregister() # so it doesn't get called after we're dead try: roboclaw.Open(self.dev_name, self.baud_rate) roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.Close() rospy.loginfo("Closed Roboclaw serial connection") #roboclaw.ForwardM1(self.address, 0) #roboclaw.ForwardM2(self.address, 0) except OSError: rospy.logerr("Shutdown did not work trying again") try: roboclaw.ForwardM1(self.address, 0) roboclaw.ForwardM2(self.address, 0) except OSError as e: rospy.logerr("Could not shutdown motors!!!!") rospy.logdebug(e) quit()
def __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("roboclaw_node") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name = rospy.get_param("~dev", "/dev/ttyACM0") baud_rate = int(rospy.get_param("~baud", "115200")) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct try: mutex.acquire() roboclaw.Open(dev_name, baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") finally: mutex.release() self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) try: mutex.acquire() version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass finally: mutex.release() if not version[0]: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0")) self.TICKS_PER_METER = float( rospy.get_param("~tick_per_meter", "4342.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) rospy.sleep(1) rospy.logdebug("dev %s", dev_name) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_speed %f", self.MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def __init__(self): self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")} rospy.init_node("roboclaw_node_height") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") self.dev_name = rospy.get_param("~dev", "/dev/ttyACM2") #may need to change the usb port self.baud_rate = int(rospy.get_param("~baud", "38400")) #may need to change the baud rate. see roboclaw usermanual self.address = int(rospy.get_param("~address", "129")) #roboclaw is current setup to use address 128 which is setting 7 #Error Handle if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct try: roboclaw.Open(self.dev_name, self.baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) try: version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.MAX_SPEED = float(rospy.get_param("~max_speed", "127")) self.MAX_DUTY = float(rospy.get_param("~max_duty", "32767")) self.last_set_speed_time = rospy.get_rostime() self.height_vel_sub = rospy.Subscriber("roboclaw/height_vel", Twist, self.cmd_vel_callback) self.height_pub = rospy.Publisher("roboclaw/height", JointState, queue_size=10) self.m1_duty = 0 self.m2_duty = 0 rospy.sleep(1) rospy.loginfo("dev %s", self.dev_name) rospy.loginfo("baud %d", self.baud_rate) rospy.loginfo("address %d", self.address) rospy.loginfo("max_speed %f", self.MAX_SPEED)
''' # brief : This file is speed script for roboclaw driver. # @author : koji kawabata # @date : 2019/10/23 ''' import time import signal import sys import roboclaw_driver.roboclaw_driver as rc rc.Open('/dev/ttyACM0', 115200) address = 0x80 rc.ForwardMixed(address, 0) rc.TurnRightMixed(address, 0) print("run speed command") try: rc.SpeedM1(address, 300) time.sleep(2) rc.SpeedM2(address, 300) time.sleep(2) rc.SpeedM1M2(address, 150, 150) time.sleep(2) rc.SpeedM1(address, -300) time.sleep(2) rc.SpeedM2(address, -300) time.sleep(2) rc.SpeedM1M2(address, -150, -150) time.sleep(2) rc.SpeedM1M2(address, 0, 0)
import time import roboclaw_driver.roboclaw_driver as rc #Windows comport name #rc = Roboclaw("COM9",115200) #Linux comport name #rc = Roboclaw("/dev/ttyACM0",115200) rc.Open("/dev/ttyACM0", 115200) address = 0x80 while(1): rc.ForwardM1(address,32) #1/4 power forward rc.BackwardM2(address,32) #1/4 power backward time.sleep(2) rc.BackwardM1(address,32) #1/4 power backward rc.ForwardM2(address,32) #1/4 power forward time.sleep(2) rc.BackwardM1(address,0) #Stopped rc.ForwardM2(address,0) #Stopped time.sleep(5) m1duty = 16 m2duty = -16 rc.ForwardBackwardM1(address,64+m1duty) #1/4 power forward rc.ForwardBackwardM2(address,64+m2duty) #1/4 power backward time.sleep(2) m1duty = -16
def __init__(self): self.lock = threading.Lock() self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")} rospy.init_node("roboclaw_node",log_level=rospy.DEBUG) rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name = rospy.get_param("~dev", "/dev/roboclaw") baud_rate = int(rospy.get_param("~baud", "460800")) self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1) self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct try: roboclaw.Open(dev_name, baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add(diagnostic_updater. FunctionDiagnosticTask("Vitals", self.check_vitals)) try: version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0")) self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0")) self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315")) self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1) rospy.sleep(1) rospy.logdebug("dev %s", dev_name) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED) rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("drive_base_node", anonymous=False) rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaws") dev_name = rospy.get_param("~dev", "/dev/ttyAMA0") baud_rate = int(rospy.get_param("~baud_rate", "115200")) # rear roboclaw self.rear_address = int(rospy.get_param("~rear_address", "129")) if self.rear_address > 0x87 or self.rear_address < 0x80: rospy.logfatal("Rear address out of range") rospy.signal_shutdown("Rear address out of range") # front roboclaw self.front_address = int(rospy.get_param("~front_address", "128")) if self.front_address > 0x87 or self.front_address < 0x80: rospy.logfatal("Front address out of range") rospy.signal_shutdown("Front address out of range") # TODO need someway to check if address is correct try: roboclaw.Open(dev_name, baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Rear Vitals", self.check_rear_vitals)) self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Front Vitals", self.check_front_vitals)) # collect and report rear roboclaw version try: version = roboclaw.ReadVersion(self.rear_address) except Exception as e: rospy.logwarn("Problem getting rear roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from rear roboclaw") else: rospy.loginfo("Rear roboclaw version: %s", repr(version[1])) # collect and report front roboclaw version try: version = roboclaw.ReadVersion(self.front_address) except Exception as e: rospy.logwarn("Problem getting front roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from front roboclaw") else: rospy.loginfo("Front roboclaw version: %s", repr(version[1])) # stop all motors roboclaw.SpeedM1M2(self.rear_address, 0, 0) roboclaw.ResetEncoders(self.rear_address) roboclaw.SpeedM1M2(self.front_address, 0, 0) roboclaw.ResetEncoders(self.front_address) # collect parameters self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0")) self.TICKS_PER_METER = float( rospy.get_param("~tick_per_meter", "1634.2")) self.BASE_WIDTH = float(rospy.get_param("~axel_width", "0.233")) self.BASE_LENGTH = float(rospy.get_param("~axel_length", "0.142")) self.DIAGNOSTIC = bool(rospy.get_param("~diagnostic", "true")) # instantiate encoder self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH, self.BASE_LENGTH) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) rospy.sleep(1) # report configuration rospy.loginfo("device : %s", dev_name) rospy.loginfo("baud_rate : %d", baud_rate) rospy.loginfo("rear_address : %d", self.rear_address) rospy.loginfo("front_address : %d", self.front_address) rospy.loginfo("max_speed : %f", self.MAX_SPEED) rospy.loginfo("ticks_per_meter : %f", self.TICKS_PER_METER) rospy.loginfo("base_width : %f", self.BASE_WIDTH) rospy.loginfo("base_length : %f", self.BASE_LENGTH) rospy.loginfo("diagnostic : %s", self.DIAGNOSTIC)
def __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("roboclaw_node") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name = rospy.get_param("~dev", "/dev/ttyACM0") baud_rate = int(rospy.get_param("~baud", "115200")) self.topic_timeout = int(rospy.get_param("~topic_timeout", "1")) self.TICKS_PER_RADIAN = float( rospy.get_param("~ticks_per_rotation", "663")) / (2 * pi) self.MAX_SPEED = float(rospy.get_param("~max_speed", "0")) addresses = str(rospy.get_param("~addresses", "128")).split(",") self.addresses = [] for addr in addresses: addr = int(addr) if addr > 0x87 or addr < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") self.addresses.append(addr) rospy.loginfo("Addresses: %s" % str(self.addresses)) # TODO need some way to check if address is correct try: roboclaw.Open(dev_name, baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw serial device") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") return self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) for addr in self.addresses: version = None try: version = roboclaw.ReadVersion(addr) except Exception as e: rospy.logwarn( "Problem getting roboclaw version from address %d" % addr) rospy.logdebug(e) if version is None or not version[0]: rospy.logwarn( "Could not get version from roboclaw address %d" % addr) rospy.signal_shutdown( "Could not get version from roboclaw address %d" % addr) return rospy.logdebug("Controller %d version is %s" % (addr, repr(version[1]))) roboclaw.SpeedM1M2(addr, 0, 0) roboclaw.ResetEncoders(addr) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("/cmd_motors", MotorSpeeds, self.cmd_motors_callback, queue_size=1) rospy.logdebug("dev %s", dev_name) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("addresses %s", str(self.addresses)) rospy.logdebug("max_speed %f", self.MAX_SPEED) rospy.logdebug("ticks_per_radian: %f", self.TICKS_PER_RADIAN)
def __init__(self): self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")} rospy.init_node("roboclaw_node", log_level=rospy.DEBUG) #TODO: remove 2nd param when done debugging rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") self.dev_name = rospy.get_param("~dev", "/dev/ttyACM0") self.baud_rate = int(rospy.get_param("~baud", "115200")) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO (Matt): figure out why roboclaw sometimes fails to Open try: roboclaw.Open(self.dev_name, self.baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add(diagnostic_updater. FunctionDiagnosticTask("Vitals", self.check_vitals)) try: version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass if version is None: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.LINEAR_MAX_SPEED = float(rospy.get_param("linear/x/max_velocity", "2.0")) self.ANGULAR_MAX_SPEED = float(rospy.get_param("angular/z/max_velocity", "2.0")) self.TICKS_PER_METER = float(rospy.get_param("tick_per_meter", "10")) self.BASE_WIDTH = float(rospy.get_param("base_width", "0.315")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() self.sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=5) self.TIMEOUT = 2 rospy.sleep(1) rospy.logdebug("dev %s", self.dev_name) rospy.logdebug("baud %d", self.baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("roboclaw_node") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name = rospy.get_param("~dev", "/dev/ttyACM0") baud_rate = int(rospy.get_param("~baud", "115200")) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct try: roboclaw.Open(dev_name, baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") # We have a single 'roboclaw' object handling serial communications. # We're about to launch different threads that each want to talk. # 1 - Diagnostics thread calling into our self.check_vitals # 2 - '/cmd_vel' thread calling our self.cmd_vel_callback # 3 - our self.run tht publishes to '/odom' # To prevent thread collision in the middle of serial communication # (which causes sync errors) all access to roboclaw from now # must be synchronized using this mutually exclusive lock object. self.mutex = threading.Lock() self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) try: # version = roboclaw.ReadVersion(self.address) with self.mutex: version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) # roboclaw.SpeedM1M2(self.address, 0, 0) # roboclaw.ResetEncoders(self.address) with self.mutex: roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0")) self.TICKS_PER_METER = float( rospy.get_param("~ticks_per_meter", "14964.27409")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.456")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() # if not sure what to use, use this! rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) # if using the joystick on the ROS Control app: # rospy.Subscriber("joy_teleop/cmd_vel", Twist, self.cmd_vel_callback) # if using the key_teleop package: # rospy.Subscriber("key_vel", Twist, self.cmd_vel_callback) # if using the mouse_teleop package: # rospy.Subscriber("mouse_vel", Twist, self.cmd_vel_callback) # if using the WebApp # rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self.cmd_vel_callback) rospy.sleep(1) rospy.logdebug("dev %s", dev_name) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_speed %f", self.MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } rospy.init_node("roboclaw_node_b") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name_front = rospy.get_param("~dev_front", "/dev/ttyACM0") dev_name_back = rospy.get_param("~dev_back", "/dev/ttyACM1") baud_rate = int(rospy.get_param("~baud", "38400")) rospy.logwarn("after dev name") self.address_front = int(rospy.get_param("~address_front", "128")) self.address_back = int(rospy.get_param("~address_back", "129")) #check wheather the addresses are in range if self.address_front > 0x87 or self.address_front < 0x80 or self.address_back > 0x87 or self.address_back < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct # try: # roboclaw.Open(dev_name_front, baud_rate) # except Exception as e: # rospy.logfatal("Could not connect to front Roboclaw") # rospy.logdebug(e) # rospy.signal_shutdown("Could not connect to front Roboclaw") try: roboclaw.Open(dev_name_back, baud_rate) except Exception as e: rospy.logfatal("Could not connect to back Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to back Roboclaw") #run diagnostics self.updater = diagnostic_updater.Updater( ) ###check later may be do it for both the motors self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) #check if you can get the version of the roboclaw...mosly you dont try: version = roboclaw.ReadVersion(self.address_front) except Exception as e: rospy.logwarn("Problem getting front roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from front roboclaw") else: rospy.logdebug(repr(version[1])) try: version = roboclaw.ReadVersion(self.address_back) except Exception as e: rospy.logwarn("Problem getting back roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from back roboclaw") else: rospy.logdebug(repr(version[1])) roboclaw.SpeedM1M2(self.address_front, 0, 0) roboclaw.ResetEncoders(self.address_front) roboclaw.SpeedM1M2(self.address_back, 0, 0) roboclaw.ResetEncoders(self.address_back) self.MAX_SPEED = float(rospy.get_param("~max_speed", "1.7")) self.TICKS_PER_METER = float( rospy.get_param("~tick_per_meter", "89478.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.1762")) self.BASE_LENGTH = float(rospy.get_param("~base_length", "0.2485")) self.WHEEL_RADIUS = float(rospy.get_param("~wheel_radius", "0.0635")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH, self.BASE_LENGTH, self.WHEEL_RADIUS) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) # rospy.sleep(1) rospy.logdebug("dev_front %s dev_back %s", dev_name_front, dev_name_back) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address_front %d address_back %d", self.address_front, self.address_back) rospy.logdebug("max_speed %f", self.MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f base_length %f", self.BASE_WIDTH, self.BASE_LENGTH)
def __init__(self): self.ERRORS = { 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), 0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"), 0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"), 0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"), 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), 0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"), 0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"), 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home") } self.ref_WR = 0 self.ref_WL = 0 self.WR = 0 self.WL = 0 self.UiR_1 = 0 self.errorL_1 = 0 self.UiL_1 = 0 self.UiR_1 = 0 self.Kp = 7.0 #11.0 self.Ki = 2.0 #30.25*0.1 #=3.025 self.Kd = 3.0 #1.0/0.1 #=10 self.errorR_1 = 0 self.errorR_2 = 0 self.errorL_2 = 0 self.errorL_1 = 0 self.UR_2 = 0 self.UR_1 = 0 self.UL_2 = 0 self.UL_1 = 0 self.dt = 0.1 self.value = 0 self.last_UL = 0 self.last_UR = 0 rospy.init_node("roboclaw_node") rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") dev_name = rospy.get_param("~dev", "/dev/tty_roboclaw") baud_rate = int(rospy.get_param("~baud", "115200")) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") ######################################## TOPICS ############################################# self.encoder_pub = rospy.Publisher('/encoders', String, queue_size=10) #self.currents_pub = rospy.Publisher('/currents', String, queue_size=10) #self.voltage_pub = rospy.Publisher('/voltage', String, queue_size=10) self.speed_pub = rospy.Publisher('/speed', String, queue_size=10) self.WL_pub = rospy.Publisher('/WL', String, queue_size=10) self.WR_pub = rospy.Publisher('/WR', String, queue_size=10) self.RefL_pub = rospy.Publisher('/RefL', String, queue_size=10) self.RefR_pub = rospy.Publisher('/RefR', String, queue_size=10) self.pid_pub = rospy.Publisher('/pid', String, queue_size=10) # TODO need someway to check if address is correct try: roboclaw.Open(dev_name, baud_rate) except Exception as e: rospy.logfatal("Could not connect to Roboclaw") rospy.logdebug(e) rospy.signal_shutdown("Could not connect to Roboclaw") self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("Roboclaw") self.updater.add( diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals)) try: version = roboclaw.ReadVersion(self.address) except Exception as e: rospy.logwarn("Problem getting roboclaw version") rospy.logdebug(e) pass if not version[0]: rospy.logwarn("Could not get version from roboclaw") else: rospy.logdebug(repr(version[1])) roboclaw.SpeedM1M2(self.address, 0, 0) roboclaw.ResetEncoders(self.address) self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0")) self.TICKS_PER_METER = float( rospy.get_param("~tick_per_meter", "14853.7362")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.38")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() ################################ SUBSCRIBER #################################### #rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) rospy.Subscriber("/joy", Joy, self.joy_event) rospy.sleep(1) rospy.logdebug("dev %s", dev_name) rospy.logdebug("baud %d", baud_rate) rospy.logdebug("address %d", self.address) rospy.logdebug("max_speed %f", self.MAX_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)