def apply_command(force): global message_received message_received = True #print 'speed: ',str(int(force*200/max_force)),' motor driver: ',thruster_id if (force > 0): thrust = map_thruster_curve("forward",force) print "forward force = ",force,"forward thrust = ",thrust motordriver.set_forward_speed(thrust) # motordriver.set_forward_speed(str(int(force*200/max_force))) elif (force < 0): thrust = map_thruster_curve("reverse",-force) print "reverse force = ",force,"reverse thrust = ",thrust motordriver.set_reverse_speed(thrust) # motordriver.set_reverse_speed(str(int(-force*200/min_force))) else: motordriver.stop() motordriverstat_msg = motor_driver_statistics( header=Header( stamp=rospy.Time.now(), frame_id="/base_link", ), id=thruster_id, current = "0",#motordriver.get_current(), out_voltage = "0",#motordriver.get_out_voltage(), batt_voltage = "0",#motordriver.get_batt_voltage(), )
def apply_command(force): global message_received message_received = True #print 'speed: ',str(int(force*200/max_force)),' motor driver: ',thruster_id output = interpolate.sample_curve((force_list, output_list), force) if output > 0: motordriver.set_forward_speed(output) elif output < 0: motordriver.set_reverse_speed(-output) else: motordriver.stop() motordriverstat_msg = motor_driver_statistics( header=Header( stamp=rospy.Time.now(), frame_id="/base_link", ), id=thruster_id, current = "0",#motordriver.get_current(), out_voltage = "0",#motordriver.get_out_voltage(), batt_voltage = "0",#motordriver.get_batt_voltage(), )
def apply_command(force): global message_received message_received = True #print 'speed: ',str(int(force*200/max_force)),' motor driver: ',thruster_id output = interpolate.sample_curve((force_list, output_list), force) if output > 0: motordriver.set_forward_speed(output) elif output < 0: motordriver.set_reverse_speed(-output) else: motordriver.stop() motordriverstat_msg = motor_driver_statistics( header=Header( stamp=rospy.Time.now(), frame_id="/base_link", ), id=thruster_id, current="0", #motordriver.get_current(), out_voltage="0", #motordriver.get_out_voltage(), batt_voltage="0", #motordriver.get_batt_voltage(), )
def apply_command(thruster_id, force): motordriver = motordrivers[thruster_id] global message_received message_received = True if (force > 0): motordriver.set_forward_speed(str(int(force*200/max_force))) elif (force < 0): motordriver.set_reverse_speed(str(int(-force*200/min_force))) else: motordriver.stop() motordriverstat_msg = motor_driver_statistics( header=Header( stamp=rospy.Time.now(), frame_id="/base_link", ), id=thruster_id, current = "0",#motordriver.get_current(), out_voltage = "0",#motordriver.get_out_voltage(), batt_voltage = "0",#motordriver.get_batt_voltage(), ) motor_driver_statistics_publisher.publish(motordriverstat_msg)