def gyroCallback(self, gyro):
currentAngle = deep(gyro.angle)
#Initialize the target angle to the angle of the
#hovercraft when it first turns on (prevents
#spinning when the craft is launched)
if self.first:
self.movement.theta = currentAngle
self.initialHeading = currentAngle
self.previousAngle = currentAngle
self.first = False
#Check to see if the magnitude is low. If so,
#set the target angle to the current angle (so if
#the joy isn't depressed anymore, it stops moving).
#Should be hard coded to 1 for any input that didn't
#originate from the left joystick.
if self.movement.mag < .5:
self.movement.theta = self.previousAngle
else:
self.previousAngle = self.movement.theta
targetAngle = self.movement.theta
#Determine how the target angle should be affected given theta
#(see Movement.msg for details)
if self.movement.modType is 'Add':
targetAngle = currentAngle + self.movement.theta
elif self.movement.modType is 'Set':
targetAngle = self.movement.theta
elif self.movement.modType is 'Bound': #NOTE: CHECK THIS LOGIC
targetAngle = self.movement.theta
#Proportional and Derivative computations
r = self.P*(targetAngle - currentAngle)
r = r + self.D*((targetAngle - currentAngle)-self.previousError)
self.previousError = targetAngle - currentAngle
#Deadband
if math.fabs(targetAngle - currentAngle) < 3:
r = 0
self.debugPrint("PosPID: Theta:{:6.2f} TargetAngle:{:6.2f} GyroAngle:{:6.2f} "
"Diff: {:6.2f} ModType: {:10s} GyroRate: {:6.2f}".format(self.movement.theta, targetAngle,
currentAngle, self.previousError,self.movement.modType,gyro.rate ))
#Ship message off to VelocityPID
move = Movement()
pub = rospy.Publisher('/angularPositionOut',Movement)
move.theta = r
move.lift = self.movement.lift
move.x = self.movement.x
move.y = self.movement.y
pub.publish(move)
def gyroCallback(self, gyro):
targetRate = self.movePass.theta
#Proportional and Derivative computations
r = self.P*(targetRate - gyro.rate)
r = r + self.D*((targetRate - gyro.rate)-self.previousError)
self.previousError = targetRate - gyro.rate
#Only reset r if the hovercraft wants to go faster in the offending direction
move = Movement()
move.theta = r
if gyro.rate < -280 and r <= 0:
self.debugPrint("Gyro Limiter Engaged -\n")
move.theta = 0
elif gyro.rate > 600 and r >= 0:
self.debugPrint("Gyro Limiter Engaged +\n")
move.theta = 0
#Ship message off to thrusterMapping
pub = rospy.Publisher('/angularVelocityOut',Movement)
move.x = self.movePass.x
move.y = self.movePass.y
move.lift = self.movePass.lift
pub.publish(move)
self.debugPrint('VelPID: TargetRate:{:5.3f} Gyro Rate:{:5.3f} '
'RateDiff:{:5.3f} r:{:5.3f}'.format(targetRate, gyro.rate,self.previousError,r))
def processData(self):
minDist = 500
minIndex= 0
for i in xrange(0,len(self.leftDistance)):
if self.leftDistance[i] is None:
self.leftDistance[i] = self.rightDistance[i]
if self.leftDistance[i] < minDist and self.leftDistance[i] is not None:
minDist = self.leftDistance[i]
minIndex=i
postProcessMove = Movement()
postProcessMove.theta = i
postProcessMove.modType='Set'
postProcessMove.x = 0
postProcessMove.y = 0
self.switcher.state = 2
self.printDebug("\nInitialAngle: %6.2f MinDist: %6.2f at angle %6.2f\n"%(self.initialAngle,minDist,minIndex))
publisher = rospy.Publisher('/mappingOut',Movement)
publisher.publish(postProcessMove)
rospy.sleep(10.0)
def interpretJoystick(self,preMove):
move = deep(preMove)
publisher = rospy.Publisher('/angleIntegratorOut',Movement)
xAxisL = move.xL
yAxisL = move.yL
xAxisR = move.xR
yAxisR = move.yR
xButton = move.xButton
bButton = move.bButton
leftBumperMag = move.bumperL
rightBumperMag= move.bumperR
#X/B button toggle logic
if bButton == 1 and not self.bButtonDepressed:
self.bButtonDepressed = True
self.buttonTargetAngle += 90
elif xButton == 1 and not self.xButtonDepressed:
self.xButtonDepressed = True
self.buttonTargetAngle += -90
if bButton == 0 and self.bButtonDepressed:
self.bButtonDepressed = False
if xButton == 0 and self.xButtonDepressed:
self.xButtonDepressed = False
#Bumper logic (rotational spin using shoulders)
#Right overrides left
bumperMag = 0
if rightBumperMag != 1:
bumperMag = (1 - rightBumperMag)
elif leftBumperMag != 1:
bumperMag = (1 - leftBumperMag)
#Get the arctangent of xAxis/yAxis to get the angle in radians.
#Convert it to degrees and make it so that it goes from 0-360 starting
#at the positive y axis (to match with the front of the hovercraft).
#Uses extreme deadzone to makesure accidental rotations don't happen.
magnitudeThreshold = 1
magnitude = math.sqrt(xAxisL**2 + yAxisL**2)
rotationalAngle = 0
if magnitude >= magnitudeThreshold:
rotationalAngle = round(math.atan2(xAxisL,yAxisL)*(180.0/3.141593),4)
if (rotationalAngle > 0):
rotationalAngle = rotationalAngle - 360
rotationalAngle = math.fabs(rotationalAngle)
#Ships off the message to the arbitrator
#Joystick overrides button target commands
moveOut = Movement()
moveOut.modType = 'Bound'
#Joystick Logic
if magnitude >= magnitudeThreshold:
#Reset button upon hitting the joystick
self.buttonTargetAngle = 0
moveOut.theta = rotationalAngle
moveOut.modType = 'Bound'
#Trigger absolute rotation if trigger/bumper is held down
if rightBumperMag != 1:
self.rightBumperAngle = self.rightBumperAngle + (rightBumperMag * 100)
self.buttonTargetAngle = 0
moveOut.theta = self.rightBumperAngle
moveOut.modType = 'Add'
magnitude = 1
else:
self.rightBumperAngle = 0
if leftBumperMag != 1:
self.buttonTargetAngle = 0
self.leftBumperAngle = self.leftBumperAngle + (leftBumperMag * 100)
moveOut.theta = -1 * self.leftBumperAngle
moveOut.modType = 'Add'
magnitude = 1
else:
self.leftBumperAngle = 0
#For 90 degree button rotations
if math.fabs(self.buttonTargetAngle) > 0:
magnitude = 1
moveOut.theta = self.buttonTargetAngle
moveOut.modType = 'Add'
moveOut.x = xAxisR
moveOut.y = yAxisR
moveOut.mag = magnitude
publisher.publish(moveOut)
#Prints all information related to the integrator if need be
if (self.debug == 1):
print("xL: %6.2f yL: %6.2f Angle: %6.2f Magnitude:%6.2f "
"xR: %6.2f yR: %6.2f Theta: %6.2f Button Target: %6.2f"
"rB: %6.2f lB: %6.2f rM: %6.2f lM: %6.2f " % (xAxisL,yAxisL,rotationalAngle,magnitude,xAxisR,yAxisR,moveOut.theta, self.buttonTargetAngle, self.rightBumperAngle, self.leftBumperAngle, rightBumperMag, leftBumperMag))
def gyroCallback(gyro):
global statemachine
global first
global counter
global targetAngle
global initial
pub = rospy.Publisher('/triangleOut',Movement)
move=Movement()
#go right
if statemachine == 1:
printDebug("statemachine 1")
counter=counter+1
if counter<=80: #the number need to be decided from experiment, may be a large number, to get triangle link length
if first == True:
targetAngle=gyro.angle
first=False
move.theta=targetAngle
move.x=-0.5
move.y=0
#pub.publish(move)
else:
counter=0
first=True
move.theta=targetAngle
move.x=0
move.y=0
statemachine=12
#intermediate state
if statemachine == 12:
counter=counter+1
move.theta=targetAngle
move.x=0.2
move.y=0
if counter>10:
counter=0
statemachine=2
#rotate anticlockwise for 120 degree
if statemachine == 2:
printDebug("statemachine 2")
if first:
targetAngle = gyro.angle
first = False
targetAngle += 120
move.theta=targetAngle
move.x=0
move.y=0
#pub.publish(move)
if math.fabs(targetAngle-gyro.angle)<=10:
counter=counter+1
if counter>10: #the number need to be decided from experiment, may be a large number, to make sure the angle has been achived
counter=0
first=True
move.theta=targetAngle
move.x=0
move.y=0
statemachine=3
#second edge
if statemachine == 3:
printDebug("statemachine 3")
counter=counter+1
if counter<=80: #number same case
if first == True:
targetAngle=gyro.angle
first=False
move.theta=targetAngle
move.x=-0.5
move.y=0
#pub.publish(move)
else:
counter=0
first=True
move.theta=targetAngle
move.x=0
move.y=0
statemachine=34
#intermediate state
if statemachine == 34:
counter=counter+1
move.theta=targetAngle
move.x=0.2
move.y=0
if counter>10:
counter=0
statemachine=4
#second rotate anticlockwise for 120 degree
if statemachine ==4:
printDebug("statemachine 4")
if first:
targetAngle = gyro.angle
first = False
targetAngle += 120
move.theta=targetAngle
move.x=0
move.y=0
#pub.publish(move)
if math.fabs(targetAngle-gyro.angle)<=10:
counter=counter+1
if counter>10: #number same case
counter=0
first=True
move.theta=targetAngle
move.x=0
move.y=0
statemachine=5
#third edge
if statemachine == 5:
printDebug("statemachine 5")
counter=counter+1
if counter<=80: #number same case
if first == True:
targetAngle=gyro.angle
first=False
move.theta=targetAngle
move.x=-0.5
move.y=0
#pub.publish(move)
else:
counter=0
first=True
statemachine=56
move.theta=targetAngle
move.x=0
move.y=0
#intermediate state
if statemachine == 56:
printDebug("statemachine 56")
counter=counter+1
move.theta=targetAngle
move.x=0.2
move.y=0
if counter>10:
counter=0
statemachine=60
if statemachine == 60:
printDebug("statemachine 60")
counter=counter+1
move.theta=gyro.angle
move.x=0
move.y=0
if counter>1000:
counter=0
statemachine=0
initial=True
pub.publish(move)