Ejemplo n.º 1
0
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
			)))
Ejemplo n.º 2
0
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)
Ejemplo n.º 3
0
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
Ejemplo n.º 4
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