def publish_servo_info(): global id, num, battery_voltage_warn_flag, battery_voltage_fatal_flag, voltage, k multi_servo_info = Multi_servo_info() for i in range(num): voltage = list(voltage) multi_servo_info.encoder_count.append( Kondo_B3M.get_encoder_total_count(id[i])) # ommit to get motor velocity due to low control rate # multi_servo_info.motor_velocity.append( # Kondo_B3M.get_servo_Velocity(id[i])) multi_servo_info.input_voltage.append(voltage[i]) # we don't have to monitor voltage at every loop, so get sparsed at a time per 100 loops if k % 100 == 0: k = 0 for j in range(num): voltage[j] = Kondo_B3M.get_servo_voltage(id[j]) if voltage[j] < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0: print("") rospy.logwarn('battery voltage is low !') battery_voltage_warn_flag = 1 elif voltage[j] < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0: print("") rospy.logfatal('battery voltage is fatally low !') k = k + 1 multi_servo_info_pub.publish(multi_servo_info) del multi_servo_info
def publish_servo_info(): global id servo_info.encoder_count = Kondo_B3M.get_encoder_total_count(id) voltage = Kondo_B3M.get_servo_voltage(id) if voltage < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0: rospy.logwarn('battery voltage is low !') battery_voltage_warn_flag = 1 elif voltage < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0: rospy.logfatal('battery voltage is low !') battery_voltage_fatal_flag = 1 servo_info.input_voltage = voltage servo_info.motor_velocity = Kondo_B3M.get_servo_Velocity(id) servo_info_pub.publish(servo_info)
def publish_servo_info(): global id, num, battery_voltage_warn_flag, battery_voltage_fatal_flag, voltage multi_servo_info = Multi_servo_info() for i in range(num): multi_servo_info.encoder_count.append( Kondo_B3M.get_encoder_total_count(id[i])) multi_servo_info.input_voltage.append( Kondo_B3M.get_servo_voltage(id[i])) voltage = multi_servo_info.input_voltage if voltage[i] < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0: print("") rospy.logwarn('battery voltage is low !') battery_voltage_warn_flag = 1 elif voltage[i] < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0: print("") rospy.logfatal('battery voltage is fatally low !') multi_servo_info.motor_velocity.append( Kondo_B3M.get_servo_Velocity(id[i])) multi_servo_info_pub.publish(multi_servo_info) del multi_servo_info