Exemplo n.º 1
0
def thrusterCallback(thrusterData):

	ftime =  time.localtime(time.time())
	ttime = ftime[3]*3600+ftime[4]*60+ftime[5]

	global DEBUG
	global gyroRate
	global gyroAngle
	global gyroTime

	thrust = Thruster()
	thrust.lift = thrusterData.lift
	thrust.thruster1 = thrusterData.thruster1
	thrust.thruster2 = thrusterData.thruster2
	thrust.thruster3 = thrusterData.thruster3
	thrust.thruster4 = thrusterData.thruster4  #Counter Clockwise
	thrust.thruster5 = thrusterData.thruster5  #Clockwise

	if gyroRate < -355 :
		printD("Gyro Limiter Engaged -")
		gyroTime = ttime
		thrust.thruster5 = 0
	elif gyroRate > 620:
		printD("Gyro Limiter Engaged +")
		gyroTime = ttime
		thrust.thruster4 = 0

	if DEBUG:
		print "Gyro Rate:",gyroRate,"\tGyro Angle:",gyroAngle
		print "Gyro Time:",gyroTime,"\tTtime:",ttime,"\n"

	if (ttime - gyroTime) < 1:
		printD("Inhibited")
		thrust.thruster4=0
		thrust.thruster5=0

	pub = rospy.Publisher('/hovercraft/Thruster',Thruster)
	pub.publish(thrust)
Exemplo n.º 2
0
def callback(joyData):
    #I'm justifying using globals by saying that I'm not sure of the behavior
    #of rospy.Subscriber when passing additional arguments to callback
    global DEBUG
    global xbox
    
    const = config()

    #It would be unwise to change these
    start=joyData.buttons[7]
    yAxisR=joyData.axes[4] # 'R' for right
    xAxisR=joyData.axes[3]
    rotateAxis=joyData.axes[0]

    
    liftPower = const.lift
    thrustModifier = const.tAmod #Max thruster power of .5
    thrust = Thruster()
    rotatePower = math.fabs(rotateAxis)
    
    #Hypotenuse of x and y axis on right joystick
    #Dampened to 1 if necessary
    translatePower = math.sqrt(xAxisR*xAxisR+yAxisR*yAxisR)
    translatePower = 1 if translatePower > 1 else translatePower
    
    #Get the arctangent of yAxis/xAxis to get the angle in radians.
    #Convert it to degrees and make it so that it goes from 0-360 starting
    #at the positive x-axis
    '''angle = round(math.atan2(yAxisR,xAxisR)*(180.0/3.141593)+180,4)
    
    #Angle normalization for when we calculate the Thruster power
    t1angle = angle+360. if angle < 330. else angle
    t2angle = angle+360. if angle < 210. else angle
    t3angle = angle
    
    t1crit = 450.0
    t2crit = 330.0
    t3crit = 210.0
    increment = 1.0/120.0'''

    if start == 1 and not xbox.startButtonDepressed:
        xbox.startButtonDepressed = True
        printD("Start Button Pressed")
        thrust.thruster1 = 0
        thrust.thruster2 = 0
        thrust.thruster3 = 0
        thrust.thruster4 = 0
        thrust.thruster5 = 0
        
        if xbox.liftOn:
            printD("Turning Thruster Off")
            xbox.liftOn = False
            thrust.lift = 0
        else:
            printD("Turning Thruster On")
            thrust.lift = liftPower
            xbox.liftOn = True

        xbox.thrusterPub.publish(thrust)

    elif start == 0 and xbox.startButtonDepressed:
        xbox.startButtonDepressed = False

    if xbox.liftOn:
        thrust.lift = liftPower
        thrust.thruster1 = 0
        thrust.thruster2 = 0
        thrust.thruster3 = 0
        thrust.thruster4 = 0
        thrust.thruster5 = 0

        #Rotational Controls
        if rotateAxis < -.1:
            thrust.thruster4 = 0
            thrust.thruster5 = rotatePower * thrustModifier * const.t5mod
        elif rotateAxis > .1:
            thrust.thruster4 = rotatePower * thrustModifier * const.t4mod
            thrust.thruster5 =0
    
        #Translation Controls
        if yAxisR > .1 :
            thrust.thruster2 = yAxisR
            thrust.thruster3 = yAxisR
        elif yAxisR < -.1:
            thrust.thruster1 = -yAxisR
        else:
            thrust.thruster1 = 0
            thrust.thruster2 = 0
            thrust.thruster3 = 0
            

        '''
        The theory behind this:
        Orientation:  Right is 0 degrees.  Increasing in clockwise direction. 360 is 0
        There are three critical points:  90, 210, and 330 degrees.  These are the points where only
        one thruster is needed.

        So the thruster1 range should increase from -30 degrees to 90 degrees, and decrease from
        90 to 210 degrees. I'm assuming it should be continuous and equally distributed.
        If the power for thruster1 is 1 at 90 degrees, then it should be 1-(1/120) at 89 degrees,
        or 91 degrees. Etc...

        The angles are normalized so I don't have to worry about negative numbers.
        ''''''

        #Thruster1
        if t1angle < t1crit:
            thrust.thruster1 = 1.0 - ((t1crit - t1angle)*(increment))
        elif t1angle == t1crit:
            thrust.thruster1 = 1.0
        elif t1angle > t1crit:
            thrust.thruster1 = 1.0 - ((t1angle - t1crit)*(increment))

        #Thruster2
        if t2angle < t2crit:
            thrust.thruster2 = 1.0 - ((t2crit - t2angle)*(increment))
        elif t2angle == t2crit:
            thrust.thruster2 = 1.0
        elif t2angle > t2crit:
            thrust.thruster2 = 1.0 - ((t2angle - t2crit)*(increment))
        
        #Thruster3
        if t3angle < t3crit:
            thrust.thruster3 = 1.0 - ((t3crit - t3angle)*(increment))
        elif t3angle == t3crit:
            thrust.thruster3 = 1.0
        elif t3angle > t3crit:
            thrust.thruster3 = 1.0 - ((t3angle - t3crit)*(increment))'''

        #Power Dampening and Correction
        thrust.thruster1 = 0 if thrust.thruster1 < 0 else thrust.thruster1
        thrust.thruster2 = 0 if thrust.thruster2 < 0 else thrust.thruster2
        thrust.thruster3 = 0 if thrust.thruster3 < 0 else thrust.thruster3

        thrust.thruster1 = thrust.thruster1 * thrustModifier * translatePower *const.t1mod
        thrust.thruster2 = thrust.thruster2 * thrustModifier * translatePower *const.t2mod
        thrust.thruster3 = thrust.thruster3 * thrustModifier * translatePower *const.t3mod

        #Debug
        if DEBUG and xbox.liftOn:
            print "xAxisR:",xAxisR,"\tyAxisR",yAxisR,"\tRotate Axis:",rotateAxis
            print "LiftStatus:",xbox.liftOn,"\tStartButtonDepressed",xbox.startButtonDepressed
            #print "Angle",angle,"\tt1angle:",t1angle,"\tt2angle:",t2angle,"\tt3angle:",t3angle
            print "Thruster1:",thrust.thruster1,"\tThruster2:",thrust.thruster2
            print "Thruster3:",thrust.thruster3,"\tThruster4:",thrust.thruster4
            print "Thruster5:",thrust.thruster5,"\tLift:",thrust.lift,"\n"

        xbox.thrusterPub.publish(thrust)