Example #1
0
def BallFound():
    global phase
    global goalPosKnown

    # FIND A BALL #
    seen = False
    ball = visThread.findBall()
    if ball:
        seen = True
        # print 'RealPosition', ball
    else:
        ball = mHandler.getKalmanBallPos()

    if firstCall["BallFound"]:
        mot.setFME(True)
        firstCall["BallFound"] = False
        firstCall["BallNotFound"] = True
        # initialize mean mu for kalman filter
        if seen:
            mHandler.setBallLoc(ball)

    mHandler.setBallLoc(ball)
    (x, y) = ball
    memProxy.insertData("dntPhase", "BallFound")

    if seen and x < 0.17 and -0.016 < y < 0.016:
        print "Kick"
        # BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE CORRECTION
        mHandler.killWalk()
        phase = "Kick"
    else:
        if seen:
            print "Kalman position", x, y
            try:
                theta = math.atan(y / x)  # TypeError: unsupported operand type(s) for /: 'list' and 'list'??
            except:
                theta = float(1)
                y = y[0]
                x = x[0]
                print "error in theta calculation"
            # hacked influencing of perception, causing walking forward to have priority
            theta *= 0.4
            x = 3.0 * (x - 0.17)
            y *= 0.6
            mHandler.sWTV(x, y, theta, max(1 - x, 0.85))
            time.sleep(0.15)
            goalPosKnown = False, None
        else:
            print "Not seen ball", x, y
            try:
                theta = math.atan(y / x)  # TypeError: unsupported operand type(s) for /: 'list' and 'list'??
            except:
                y = y[0]
                theta = float(1)
                print "error in theta calculation"
            mHandler.postWalkTo(0, 0, math.copysign(theta, y) / 2.0)
            # TODO determine where ball is in headposition and look there instead of random places
            if not vis.scanCircle(visThread):
                phase = "BallNotFound"
Example #2
0
def BallNotFoundKeep():
    """ Keeper lost track of ball
    """
    global phase
    global ball_loc
    mot.setFME(False)

    visThread.startScan()
    # if a ball is found
    if vis.scanCircle(visThread):
        # reinitialize
        ball_loc = list()
        phase = "BallFoundKeep"
        firstCall["BallNotFoundKeep"] = True
    elif firstCall["BallNotFoundKeep"]:
        memProxy.insertData("dntBallDist", 0)
        mot.keepNormalPose()
        firstCall["BallNotFoundKeep"] = False
Example #3
0
def Kick():
    global phase
    global goalPosKnown
    mot.setFME(False)

    memProxy.insertData("dntPhase", "Kick")
    visThread.stopScan()

    # scan for a goal
    goal = None
    if goalPosKnown[0]:
        goal = goalPosKnown[1]
    else:
        goal = vis.scanCircleGoal()
    if goal:
        goalPosKnown = (True, goal)

    print "Kick phase: ", goal
    mot.setHead(0, 0.45)
    visThread.startScan()
    ball = vis.scanCircle(visThread)

    # Case 0 : Ball stolen/lost.
    # Check if the ball is still there, wait until ball is found has passed
    # Something of a mean, ...

    if not ball:
        print "Ball gone"
        phase = "BallNotFound"
    elif ball[0] > 0.25 or ball[1] > 0.1 or ball[1] < -0.1:
        print "Ball too far"
        phase = "BallFound"
    elif ball and not goal:
        mHandler.kick(0)
        phase = "BallNotFound"
    else:
        # else a goal is found, together with it's color
        (color, kickangles) = goal
        if type(kickangles[0]) == tuple:

            (first, second) = kickangles
            kickangle = (3 * first[0] + second[0]) / 4.0  # kick slightly more towards left pole
        else:
            kickangle = kickangles[0]

        if color == "Blue":
            goalColor = 0
        else:
            goalColor = 1

        # use kalman filter to position ball in 10 measurements
        c = 0
        while c < 10:
            c += 1
            ball = visThread.findBall()
            mHandler.setBallLoc(ball)
        ball = mHandler.getKalmanBallPos()

        # Cases 1-3, if you see your own goal, kick to the other side OR if the keeper saw the ball
        print "keep saw ball: " + str(memProxy.getData("dntKeepSawBall"))
        if memProxy.getData("dntKeepSawBall"):
            # Case 1, goal is left, kick to the right.
            if kickangle >= 0.7:
                mHandler.kick(-1.1)
            # Case 2, goal is right, kick to the left.
            if kickangle <= -0.7:
                mHandler.kick(1.1)
            else:
                # Case 3, goal is straight forward, HAK, but only in the first 2 minutes of each half.
                if gsc.getSecondsRemaining() > 8:
                    mHandler.kick(1.1)
                else:
                    mHandler.kick(1.1)

        else:
            # Case 4, other player's goal is found.
            # Kick towards it.

            # general kick interface distributes kick
            if abs(kickangle < 0.6):
                mHandler.walkTo(0, math.copysign(0.03, kickangle), 0)
            while c < 10:
                c += 1
                ball = visThread.findBall()
                mHandler.setBallLoc(ball)
            ball = mHandler.getKalmanBallPos()
            print "Goalcolor", goalColor, "KickAngle", kickangle
            print "TeamColor", teamColor, "Locatie", ball
            mHandler.kick(kickangle)
        phase = "BallNotFound"
        ledProxy.fadeRGB("LeftFaceLeds", 0x00000000, 0)  # no goal yet, left led turns black
Example #4
0
def BallFoundKeep():
    global phase
    global ball_loc
    global firstCall

    if firstCall["BallFoundKeep"]:
        mot.stance()
        firstCall["BallFoundKeep"] = False
        mot.setFME(False)

    maxlength = 6
    halfmaxlength = 3

    #  FIND A BALL  #
    ball = visThread.findBall()
    if ball:
        # if ball[0] < 0.3 and (ball[1] < 0.5 or ball[1] > -0.5):
        #    phase = 'InGoalArea'
        #    mot.move('normalPose')
        #    visThread.findBall()
        #    return True
        (x, y) = ball

        length = len(ball_loc)
        if length == maxlength:
            ball_loc = ball_loc[1:]
            ball_loc.append(ball)
        else:
            ball_loc.append(ball)
        if length == maxlength:
            xold = 0
            yold = 0
            xnew = 0
            ynew = 0

            # shift elements sidewards, newest ballloc becomes nr <maxlength>, oldest is thrown away
            for number in range(maxlength):
                (x, y) = ball_loc[number]

                # add to xold/new and yold/new variables, number 0 is oldest, number <maxlength> most recent ballloc
                if 0 <= number < halfmaxlength:
                    xold += x
                    yold += y
                else:
                    xnew += x
                    ynew += y
            # calculate the mean of measurements
            xold /= halfmaxlength
            yold /= halfmaxlength
            xnew /= halfmaxlength
            ynew /= halfmaxlength

            # calc diff in distance
            distold = math.sqrt(xold ** 2 + yold ** 2)
            distnew = math.sqrt(xnew ** 2 + ynew ** 2)
            speed = distold - distnew

            print "Ball moving from ", xold, yold, "to", xnew, ynew, "(mean). Speed", speed, "Distnew: ", distnew

            # calculate direction if speed is high enough
            if speed > 0.23 or (distnew < 0.8 and speed > 0.05):

                # This is all triangular magic using similarity between two triangles formed by balllocations and Nao.
                # Keep in mind that y-axis is inverted and that x is forward, y is sideways!
                #
                #                                              x
                #      (yold,xold)                             |
                #            .                                 |
                #            | \                               |
                #          B |   \                             |
                #            |     \                           |
                #            |_______.  (ynew,xnew )           |
                #                A   | \                       |
                #                    |   \                     |
                #                  C |     \                   |
                #                    |       \                 |
                #     y _____________|_________._______________.___________ -y
                #                            (dir,0)           Nao = (0,0)
                #
                #                    [--- D ---]

                # Mathematical proof:
                # A = yold - ynew
                # B = xold - xnew
                # C = xnew
                # D = ynew - dir

                # Similar triangles -> C / B = D / A
                #                      D     = A*C/B
                #                      D     = ynew - dir
                #                      A*C/B = ynew - dir
                #                      dir   = A*C/B - ynew

                dir = (yold - ynew) * xnew / (xold - xnew) - ynew
                print "Direction", dir
                # if a direction has been found, clear all variables
                mHandler.dive(dir)
                phase = "BallNotFoundKeep"

                ball_loc = list()
                visThread.clearCache()
                firstCall["BallFoundKeep"] = True
                print "Direction", dir
    else:
        phase = "BallNotFoundKeep"
        firstCall["BallFoundKeep"] = True