def StopRover(): AccessRoboclaw() addresses = [128, 129, 130] for a in addresses: roboclaw.SpeedM1M2(a, 0, 0) roboclaw.ResetEncoders(a) addresses = [131, 132, 133] for a in addresses: roboclaw.SpeedM1M2(a, 0, 0)
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("tome_roboclaws")
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.addressBackMotors = int(rospy.get_param("~addressBackMotors"))
self.addressFrontMotors = int(rospy.get_param("~addressFrontMotors"))
if self.addressBackMotors > 0x87 or self.addressBackMotors < 0x80:
rospy.logfatal("addressBackMotors out of range")
rospy.signal_shutdown("addressBackMotors out of range")
if self.addressFrontMotors > 0x87 or self.addressFrontMotors < 0x80:
rospy.logfatal("addressBackMotors out of range")
rospy.signal_shutdown("addressBackMotors out of range")
# TODO need someway to check if addressBackMotors 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_back_motors))
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals_front_motors))
try:
versionBackMotor = roboclaw.ReadVersion(self.addressBackMotors)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
# rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not versionBackMotor[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(versionBackMotor[1]))
try:
versionFrontMotor = roboclaw.ReadVersion(self.addressFrontMotors)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
# rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not versionFrontMotor[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(versionFrontMotor[1]))
roboclaw.SpeedM1M2(self.addressBackMotors, 0, 0)
roboclaw.SpeedM1M2(self.addressFrontMotors, 0, 0)
roboclaw.ResetEncoders(self.addressBackMotors)
roboclaw.ResetEncoders(self.addressFrontMotors)
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("addressBackMotors %d", self.addressBackMotors)
rospy.logdebug("addressFrontMotors %d", self.addressFrontMotors)
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 CalibrateAbsoluteEncoders(): #better use the roboclaw software for that AccessRoboclaw() roboclaw.ResetEncoders(131) cal_speed = 100 #drive to positive limit pos roboclaw.SpeedM1M2(131, cal_speed, cal_speed) time.sleep(2) roboclaw.SpeedM1M2(132, cal_speed, cal_speed) time.sleep(2) # wait till position is reached roboclaw.SpeedM1M2(131, 0, 0) time.sleep(2) roboclaw.SpeedM1M2(132, 0, 0) time.sleep(2) #get limit positions while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_LEFT) if enc[1] != 0: break print "upper limit w1: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_RIGHT) if enc[1] != 0: break print "upper limit w4: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_LEFT) if enc[1] != 0: break print "upper limit w3: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_RIGHT) if enc[1] != 0: break print "upper limit w6: " + str(enc); #drive to positive limit pos roboclaw.SpeedM1M2(131, -cal_speed, -cal_speed) time.sleep(2) roboclaw.SpeedM1M2(132, -cal_speed, -cal_speed) time.sleep(2) # wait till position is reached roboclaw.SpeedM1M2(131, 0, 0) time.sleep(2) roboclaw.SpeedM1M2(132, 0, 0) time.sleep(2) #get limit positions while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_LEFT) if enc[1] != 0: break print "lower limit w1: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_FRONT_RIGHT) if enc[1] != 0: break print "lower limit w4: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_LEFT) if enc[1] != 0: break print "lower limit w3: " + str(enc); while True: enc = GetEncRover(MotorSelect.STEERING_BACK_RIGHT) if enc[1] != 0: break print "lower limit w6: " + str(enc);
Twist,
queue_size=100) # wheel velocity in m/s
rospy.init_node('beetle_control', anonymous=True)
#rate = rospy.Rate(10)
StopRover()
print "setting wheels to initial pos..."
print "wheel 1"
#SetWheelAngleDirectly(MotorSelect.STEERING_FRONT_LEFT, 0, 200)
#SetWheelAngleDirectly(MotorSelect.STEERING_FRONT_RIGHT, 0, 300)
print "wheel 3"
#SetWheelAngleDirectly(MotorSelect.STEERING_BACK_LEFT, 0, 100)
print "wheel 6"
#SetWheelAngleDirectly(MotorSelect.STEERING_BACK_RIGHT, 0, 100)
roboclaw.ResetEncoders(128)
roboclaw.ResetEncoders(129)
roboclaw.ResetEncoders(130)
roboclaw.ResetEncoders(131)
roboclaw.ResetEncoders(132)
roboclaw.ResetEncoders(133)
print "starting measurement thread..."
meas_thread = threading.Thread(target=PublishEncValues,
args=(
pub1,
pub2,
pub3,
pub4,
pub5,
pub6,
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)