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
))
