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