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 zero_pwms(): cmd = ControlThrustConfig() cmd.controller = 'xbox_rc' cmd.pulse_width = ZERO_PWM cmd.id = ControlThrustConfig.PORT pwm_pub.publish(cmd) cmd.id = ControlThrustConfig.STARBOARD pwm_pub.publish(cmd)
def stopThrusters():
global starboard_setpoint
global port_setpoint
global starboard_current
global port_current
msg = ControlThrustConfig()
msg.controller = 'azi_drive'
msg.id = ControlThrustConfig.STARBOARD
msg.pulse_width = ZERO_PW / 1000000.0
pwm_pub.publish(msg)
msg.id = ControlThrustConfig.PORT
pwm_pub.publish(msg)
#Zero internal varibles
starboard_setpoint = 0.0;
port_setpoint = 0.0;
starboard_current = 0.0
port_current = 0.0
port_current = port_setpoint
elif port_current < port_setpoint:
port_current += RAMP_RATE
if port_current > port_setpoint:
port_current = port_setpoint
if starboard_current > starboard_setpoint:
starboard_current -= RAMP_RATE
if starboard_current < starboard_setpoint:
starboard_current = starboard_setpoint
elif starboard_current < starboard_setpoint:
starboard_current += RAMP_RATE
if starboard_current > starboard_setpoint:
starboard_current = starboard_setpoint
msg = ControlThrustConfig()
msg.controller = 'azi_drive'
msg.id = ControlThrustConfig.PORT
msg.pulse_width = convertNewtonsToPW(port_current) / 1000000.0
pwm_pub.publish(msg)
msg.id = ControlThrustConfig.STARBOARD
msg.pulse_width = convertNewtonsToPW(starboard_current) / 1000000.0
pwm_pub.publish(msg)
#Wait till next cycle
r.sleep()
#Clean up
stopThrusters() #Stop motors