def check_vitals(self, stat): try: status_front = roboclaw.ReadError(self.address_front)[1] status_back = roboclaw.ReadError(self.address_back)[1] except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return state, message = self.ERRORS[status_front] stat.summary(state, message) state, message = self.ERRORS[status_back] stat.summary(state, message) try: stat.add("front Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address_front)[1] / 10)) stat.add("front Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address_front)[1] / 10)) stat.add("front Temp1 C:", float(roboclaw.ReadTemp(self.address_front)[1] / 10)) stat.add("front Temp2 C:", float(roboclaw.ReadTemp2(self.address_front)[1] / 10)) stat.add("back Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address_back)[1] / 10)) stat.add("back Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address_back)[1] / 10)) stat.add("back Temp1 C:", float(roboclaw.ReadTemp(self.address_back)[1] / 10)) stat.add("back Temp2 C:", float(roboclaw.ReadTemp2(self.address_back)[1] / 10)) except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return stat
def check_vitals(self, stat): try: mutex.acquire() status = roboclaw.ReadError(self.address)[1] except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) mutex.release() return mutex.release() state, message = self.ERRORS[status] stat.summary(state, message) try: mutex.acquire() stat.add( "Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10)) stat.add( "Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10)) stat.add("Temp1 C:", float(roboclaw.ReadTemp(self.address)[1] / 10)) stat.add("Temp2 C:", float(roboclaw.ReadTemp2(self.address)[1] / 10)) except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) finally: mutex.release() return stat
def check_vitals_front_motors(self, stat): try: status = roboclaw.ReadError(self.addressFrontMotors)[1] except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return state, message = self.ERRORS[status] stat.summary(state, message) try: stat.add( "Main Batt V:", float( roboclaw.ReadMainBatteryVoltage(self.addressFrontMotors)[1] / 10)) stat.add( "Logic Batt V:", float( roboclaw.ReadLogicBatteryVoltage( self.addressFrontMotors)[1] / 10)) stat.add("Temp1 C:", float(roboclaw.ReadTemp(self.addressFrontMotors)[1] / 10)) stat.add( "Temp2 C:", float(roboclaw.ReadTemp2(self.addressFrontMotors)[1] / 10)) except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return stat
def twist_callback(msg): global pub global last_time last_time = rospy.get_rostime() #rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z)) #rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z)) TR = MIX_ROTATION v_l = msg.linear.x - TR * msg.angular.z v_r = msg.linear.x + TR * msg.angular.z dutyleft = int(v_l * MAX_DUTY_CYCLE * 32768) dutyright = int(v_r * MAX_DUTY_CYCLE * 32768) dutyleft = limit(dutyleft, 32767) dutyright = limit(dutyright, 32767) rc.DutyM1M2(MTR_ADDRESS, dutyleft, dutyright) rospy.loginfo("[%d , %d]" % (dutyleft, dutyright)) dummy, c1, c2 = rc.ReadCurrents(MTR_ADDRESS) status = rc.ReadError(MTR_ADDRESS)[1] 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) pub.publish(diagstr)
def debug_robot(): rc.Open('/dev/ttyUSB0', 115200) while True: status = rc.ReadError(MTR_ADDRESS)[1] encoder = rc.ReadEncM1(MTR_ADDRESS) print encoder
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 check_vitals(self, stat): 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") } try: status = roboclaw.ReadError(address)[1] except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return state, message = self.ERRORS[status] stat.summary(state, message) try: stat.add("Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10)) stat.add("Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10)) stat.add("Temp1 C:", float(roboclaw.ReadTemp(self.address)[1] / 10)) stat.add("Temp2 C:", float(roboclaw.ReadTemp2(self.address)[1] / 10)) except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return stat
def check_vitals(self, stat): rospy.logdebug("checking vitals") for addr in self.addresses: try: status = roboclaw.ReadError(addr)[1] except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) continue state, message = self.ERRORS[status] stat.summary(state, message) try: stat.add("%d Main Batt V:" % addr, float(roboclaw.ReadMainBatteryVoltage(addr)[1] / 10)) stat.add("%d Logic Batt V:" % addr, float(roboclaw.ReadLogicBatteryVoltage(addr)[1] / 10)) stat.add("%d Temp1 C:" % addr, float(roboclaw.ReadTemp(addr)[1] / 10)) stat.add("%d Temp2 C:" % addr, float(roboclaw.ReadTemp2(addr)[1] / 10)) except OSError as e: rospy.logwarn("Diagnostics OSError: %d", e.errno) rospy.logdebug(e) return stat