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 spin(self):
#Update movement to spin
move = Movement()
move.modType = 'Add'
move.theta = -15
move.x = 0
move.y = 0
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 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 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 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 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))
