예제 #1
0
 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
예제 #2
0
 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
예제 #3
0
 def spin(self):
     #Update movement to spin
     move = Movement()
     move.modType = 'Add'
     move.theta = -15
     move.x = 0
     move.y = 0
     return move
예제 #4
0
 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)
예제 #6
0
 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
예제 #7
0
 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))
예제 #9
0
 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
예제 #10
0
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)
예제 #11
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)

        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)
        '''
예제 #12
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))