def deploy_hydrophone(self): #INITIALLY WHEN THE BOAT STARTS THE CABLE SHOULD BE FEED OVER THE TOP OF THE SERVO deploy_msg = DynamixelFullConfig() deploy_msg.id = 4 #id 4 is the stern servo for the hydrophones deploy_msg.led = 0 deploy_msg.goal_position = -2 * math.pi deploy_msg.moving_speed = 1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit = 173 # 173/1023 is about 17% torque deploy_msg.goal_acceleration = 20 deploy_msg.control_mode = DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity = 1.4 for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(8.5 / 100)
def retract_hydrophone(self): #WHEN THE HYDROPHONES RETRACT CABLE SHOULD FEED BACK OVER THE TOP OF THE SERVO deploy_msg = DynamixelFullConfig() deploy_msg.id = 4 #id 4 is the stern servo for the hydrophones deploy_msg.led = 0 deploy_msg.goal_position = 4.3 # 2.4 rad/s~22 rpm NOTE: we explicitly retract to pi to try and avoid being at the 0/2*PI boundary on a powerup deploy_msg.moving_speed = 1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit = 143 # 143/1023 is about 14% torque (so we don't break the rope if someone didn't feed them correctly to start) deploy_msg.goal_acceleration = 20 deploy_msg.control_mode = DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity = 1.4 self.servo_full_config_pub.publish(deploy_msg) for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(20 / 100)
def xbox_cb(joy_msg): global killed global back_btn_pressed global current_controller # Toggle RC state if joy_msg.buttons[BTNS['BACK']]: if not back_btn_pressed: back_btn_pressed = True if current_controller != 'xbox_rc': request_controller_proxy('xbox_rc') else: request_controller_proxy('azi_drive') else: back_btn_pressed = False # If kill_broadcaster.send(killed) is put outside these if statments then # rc will echo any other kill with its own kill msg which prevents the RC node from ever coming out of a kill # without pressing the start button if (joy_msg.buttons[BTNS['KILL']] == 1): #Press KILL Switch # Kill the boat killed = True kill_broadcaster.send(True) if (joy_msg.buttons[BTNS['START']] == 1): #Press START Switch # Do not update killed here as there may be other kills that affect the weather the boat should or shouldn't be killed kill_broadcaster.send(False) if killed: zero_pwms() else: cmd = ControlThrustConfig() cmd.controller = 'xbox_rc' cmd.id = ControlThrustConfig.PORT cmd.pulse_width = 0.0005*(joy_msg.axes[AXIS['LEFT_STICK_Y']]) + ZERO_PWM #LEFT_STICK # (0.0001*logit((joy_msg.axes[AXIS['LEFT_STICK_Y']])/2+0.5))+0.0015 pwm_pub.publish(cmd) cmd.id = ControlThrustConfig.STARBOARD cmd.pulse_width = 0.0005*(joy_msg.axes[AXIS['RIGHT_STICK_Y']]) + ZERO_PWM #RIGHT_STICK # (0.0001*logit((joy_msg.axes[AXIS['RIGHT_STICK_Y']])/2+0.5))+0.0015 pwm_pub.publish(cmd) # Zero servos for x in range(2,4): servo_pub.publish(ControlDynamixelFullConfig( controller = 'xbox_rc', config = DynamixelFullConfig( id= x, goal_position= math.pi, moving_speed= 12, # near maximum, not actually achievable ... torque_limit= 1023, goal_acceleration= 38, control_mode= DynamixelFullConfig.JOINT, goal_velocity= 10 )))
def deploy_hydrophone(self): #INITIALLY WHEN THE BOAT STARTS THE CABLE SHOULD BE FEED OVER THE TOP OF THE SERVO deploy_msg=DynamixelFullConfig() deploy_msg.id=4 #id 4 is the stern servo for the hydrophones deploy_msg.led=0 deploy_msg.goal_position=-2*math.pi deploy_msg.moving_speed=1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit=173 # 173/1023 is about 17% torque deploy_msg.goal_acceleration=20 deploy_msg.control_mode=DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity=1.4 for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(8.5/100)
def retract_hydrophone(self): #WHEN THE HYDROPHONES RETRACT CABLE SHOULD FEED BACK OVER THE TOP OF THE SERVO deploy_msg=DynamixelFullConfig() deploy_msg.id=4 #id 4 is the stern servo for the hydrophones deploy_msg.led=0 deploy_msg.goal_position=4.3 # 2.4 rad/s~22 rpm NOTE: we explicitly retract to pi to try and avoid being at the 0/2*PI boundary on a powerup deploy_msg.moving_speed=1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit=143 # 143/1023 is about 14% torque (so we don't break the rope if someone didn't feed them correctly to start) deploy_msg.goal_acceleration=20 deploy_msg.control_mode=DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity=1.4 self.servo_full_config_pub.publish(deploy_msg) for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(20/100)
def send_angle(self, angle, servo): self.servo_pub.publish( DynamixelFullConfig( id=servo, goal_position=clamp_angles(angle), moving_speed=self.servo_max_rotation * 16, torque_limit=1023, goal_acceleration=38, control_mode=DynamixelFullConfig.JOINT, goal_velocity=0.0 # This is for wheel mode, we don't use it ))
def reset_servo_angles(): for servo in [2, 3]: servo_pub.publish( DynamixelFullConfig( id=servo, goal_position=clamp_angles(0.0), moving_speed=1.0, torque_limit=1023, goal_acceleration=38, control_mode=DynamixelFullConfig.JOINT, goal_velocity=1.0, ))
def dynam_config(_id, _pos, _speed, _torque, _acc, _mode, _vel): time.sleep(.3) pub.publish( DynamixelFullConfig( id=_id, goal_position=_pos, moving_speed=_speed, torque_limit=_torque, goal_acceleration=_acc, control_mode=_mode, goal_velocity=_vel # This is for wheel mode, we don't use it ))
class Node(object): # Constructor def __init__(self): rospy.init_node('tank_steer_pd', anonymous=False) # Grab params if rospy.has_param('~simulate'): self.simulate = rospy.get_param('~simulate') else: self.simulate = 0 self.max_thrust = rospy.get_param('/max_thrust', 100.0) self.min_thrust = rospy.get_param('/min_thrust', -100.0) self.L = rospy.get_param('distance_between_thrusters', 0.6096) #meters # Set up publishers for servo position and prop speed self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size = 10) self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config', DynamixelFullConfig, queue_size = 10) # Initilize kill self.killed = False self.kill_listener = KillListener(self.set_kill, self.clear_kill) self.kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Tank steer PD shutdown') rospy.on_shutdown(self.on_shutdown) # Clear in case it was previously killed try: self.kill_broadcaster.clear() except rospy.service.ServiceException, e: rospy.logwarn(str(e)) # Get subscriber to wrench topic (from trajectory generator) self.wrench_sub = rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size = 10) # Setup some constants # Define thrustors and there position # TODO: should be retrieved and not hard coded """ self.thrusters = [ dict( # port id=3, pwm_pub= rospy.Publisher('stm32f3discovery_imu_driver/pwm1', Float64, queue_size = 10), angle_pub= rospy.Publisher('port_angle', Float64, queue_size = 10), ), dict( # starboard id=2, position= np.array([-.5239, -.3048, -.5]), angle= 0, #thrust_range= (min(calib_data[0]), max(calib_data[0])), dangle_range= (-3.2e-0, +3.2e-0), angle_range= (-math.pi*.75, +math.pi*.75), # at most (-6*math.pi, +6*math.pi) effort= 0, dangle= 0, pwm_pub= rospy.Publisher('stm32f3discovery_imu_driver/pwm2', Float64, queue_size = 10), angle_pub= rospy.Publisher('starboard_angle', Float64, queue_size = 10), ), ] """ # See Wrench Callback for deffinition of wrench_tf self.wrench_tf = np.matrix([[0.5, -1 / self.L], [0.5, 1 / self.L]]) # Wait for the dynamixel node to initilze if not self.simulate: rospy.loginfo('Checking/waiting for dynamixel server') while(self.servo_pub.get_num_connections() <= 0 and not rospy.is_shutdown): pass rospy.loginfo('dynamixel server found') # Zero servos for x in range(2,4): self.servo_pub.publish(DynamixelFullConfig( id= x, goal_position= math.pi, moving_speed= 12, # near maximum, not actually achievable ... torque_limit= 1023, goal_acceleration= 38, control_mode= DynamixelFullConfig.JOINT, goal_velocity= 10, )) # Wait for wrench msgs rospy.spin()
def _wrench_cb(self, msg): # Since it is impossible to move in the y direction with this thruster config this direction is completly ignored # Torque is equal to the z component of the wrench.torque desired = np.matrix([[msg.wrench.force.x], [msg.wrench.torque.z]]) # Multiple by precalculated wrench thrust = self.wrench_tf * desired # Check limits # TODO: get actual limits # Preserve torque but adjust force if outside of limits diff = abs(thrust[0, 0] - thrust[1,0]) max_diff = self.max_thrust - self.min_thrust if diff > max_diff: # Can't satisfy torque rospy.logwarn("Torque request can not be satisfied!") if thrust[0,0] > self.max_thrust: thrust[0,0] = self.max_thrust thrust[1,0] = self.min_thrust elif thrust[0,0] < self.min_thrust: thrust[0,0] = self.min_thrust thrust[1,0] = self.max_thrust elif thrust[1,0] > self.max_thrust: thrust[1,0] = self.max_thrust thrust[0,0] = self.min_thrust elif thrust[1,0] < self.min_thrust: thrust[1,0] = self.min_thrust thrust[0,0] = self.max_thrust else: if thrust[0,0] > self.max_thrust: rospy.logwarn("Port thrust above max, preserving torque and reducing force") offset = thrust[0,0] - self.max_thrust thrust[0,0] = self.max_thrust thrust[1,0] -= offset elif thrust[0,0] < self.min_thrust: rospy.logwarn("Port thrust below minimum, preserving torque and reducing force") offset = self.min_thrust - thrust[0,0] thrust[0,0] = self.min_thrust thrust[1,0] += offset elif thrust[1,0] > self.max_thrust: rospy.logwarn("Starboard thrust above max, preserving torque and reducing force") offset = thrust[1,0] - self.max_thrust thrust[1,0] = self.max_thrust thrust[0,0] -= offset elif thrust[1,0] < self.min_thrust: rospy.logwarn("Starboard thrust below minimum, preserving torque and reducing force") offset = self.min_thrust - thrust[1,0] thrust[1,0] = self.min_thrust thrust[0,0] += offset # Zero servos for x in range(2,4): self.servo_pub.publish(DynamixelFullConfig( id= x, goal_position= math.pi, moving_speed= 12, # near maximum, not actually achievable ... torque_limit= 1023, goal_acceleration= 38, control_mode= DynamixelFullConfig.JOINT, goal_velocity= 10, )) if not self.killed: # Output thrust self.thrust_pub.publish(thrusterNewtons( id = 3, thrust = thrust[0,0] )) self.thrust_pub.publish(thrusterNewtons( id = 2, thrust = thrust[1,0] )) else: # Output thrust self.thrust_pub.publish(thrusterNewtons( id = 3, thrust = 0 )) self.thrust_pub.publish(thrusterNewtons( id = 2, thrust = 0 ))