def move(self, x=0, y=0, theta=0):
move = Movement()
move.x = x
move.y = y
move.theta = theta
move.modType = 'Add'
return move
def move(self, x=0, y=0, theta=0):
#Returns some kind of movement (Factory method)
move = Movement()
move.x = x
move.y = y
move.theta = theta
move.modType = 'Add'
return move
def spin(self):
#Update movement to spin
move = Movement()
move.modType = 'Add'
move.theta = -15
move.x = 0
move.y = 0
return move
def moveHovercraft(self, event):
publisher = rospy.Publisher('/visualServoOut', Movement)
if (self.ticksSinceLandmarkSeen < 25):
publisher.publish(self.move)
else:
noMove = Movement()
noMove.x = 0
noMove.y = 0
noMove.theta = 0
noMove.modType = 'Add'
publisher.publish(noMove)
self.ticksSinceLandmarkSeen += 1
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 == '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
#print ("%s: %f, Current: %f\tTarget: %f"%(self.movement.modType, self.movement.theta, currentAngle, targetAngle))
#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 kickBallOut(self):
#A sleep so we can pick the ball up. The backwards momentum wasn't fast enough to kick the ball out
rospy.sleep(5)
#Keep moving backwards until the ball isn't in the hook anymore
if self.isBallCaptured:
move = Movement()
move.y = -1
move.x = 0
move.theta = 0
move.modType = 'Add'
return move
else:
self.state = 4
return self.haltMove
def collectBall(self):
if not self.isBallInView:
self.state -= 1 #Might run into trouble here if it gets decremented twice in a row
return self.haltMove
elif self.isBallCaptured:
self.state += 1 #Ball is in the doohickey
return self.haltMove
else:
move = Movement()
move.y = .4
move.x = 0
#if self.ballLocation.x > 20:
move.theta = -self.ballLocation.x/2
move.modType = 'Add'
return 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 moveTowardsLandmark(self, landmarkNum):
#Incorporate landmark Number we're looking for
'''
if not self.isBallCaptured:
self.state -= 1
return self.haltMove
'''
if not self.isLandmarkInView or (self.currentVisibleLMCode != landmarkNum and self.currentVisibleLMCode != -1):
self.state -= 1
return self.haltMove
elif self.isLandmarkInView and not self.targetReached:
move = Movement()
move.y = 1
move.x = 0
move.theta = -(float(self.landmarkX)-180.)/10.
move.modType = 'Add'
return move
elif self.isLandmarkInView and self.targetReached:
self.state = 9 #Switch to ball kick out state
return self.haltMove
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)
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)
magnitudeThreshold = 1
magnitude = math.sqrt(xAxisL**2 + yAxisL**2)
rotationalAngle = xAxisL*10
#Ships off the message to the arbitrator
#Joystick overrides button target commands
moveOut = Movement()
moveOut.theta =0
moveOut.modType = 'Bound'
#Joystick Logic
if magnitude >= magnitudeThreshold:
#Reset button upon hitting the joystick
self.buttonTargetAngle = 0
moveOut.theta = rotationalAngle
moveOut.modType = 'Add'
#Trigger absolute rotation if trigger/bumper is held down
#print moveOut.theta
if rightBumperMag != 1:
#print "Right Trigger"
self.rightBumperAngle = rightBumperMag
self.buttonTargetAngle = 0
moveOut.theta = -.1 if self.rightBumperAngle < 0 else 0
moveOut.modType = 'Add'
magnitude = 1
else:
self.rightBumperAngle = 0
if leftBumperMag != 1:
self.buttonTargetAngle = 0
self.leftBumperAngle = leftBumperMag
moveOut.theta = .1 if self.leftBumperAngle < 0 else 0
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'
'''
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
moveOut = Movement()
moveOut.x = xAxisR
moveOut.y = yAxisR
moveOut.mag = math.fabs(xAxisL)
if moveOut.mag > .5:
if xAxisL < -.2:
theta = -30
elif xAxisL > .2:
theta = 30
else:
theta = 0
moveOut.theta = theta
moveOut.modType = 'Add'
else:
moveOut.theta = 0
moveOut.modType = 'Add'
publisher.publish(moveOut)
'''
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))
