# 

# Wandering behavior.

# now with more maptool

# This wanderer has the robot move backwards (i.e. the laptop faces the user as the

# user walks behind the robot). The servo should be pointed with its cord pointing

# towards the laptop. The camera is not used.

#

# This wandering behavior follows the left-hand rule.

#

#

#


#

# Required libraries

# socket is needed to connect to the sonar server and the ER1 server

# string is needed to send commands to the servers

#


import socket
import string
import msvcrt
import time

#

# Hard-coded variables

# sonar variables are definitions of distances on the sonar

# velocity variables are definitions of speeds for the robot to move at

# Servo variable are positions

# maxloop is the biggest number of times to do one behavior before returning to the main loop

# also the biggest number of times to run the main loop before asking if should do more

#


sonarTooClose = 30
sonarClose = 50
sonarHallWidth = 130

sonarMaxTrust = 300     # Max value the sonar will accept without double-checking

sonarTolerance = 10      # Error tolerance of the sonar

sonarNumTries  = 1

velocitySlow = 7
velocityNormal = 15

maxloop = 5

servoCenter = 125
servoLeft = 240
servoRight = 30

#

# Dynamic globals

#

posX = 0
posY = 0
posTh = 0
lastX = 0
lastY = 0
lastTh = 0
servo = 0
lastSonar = -1

#

# Initialize the TCP connections to the sonar and the robot

#


HOST = '127.0.0.1'            # localhost  

SONARPORT = 5010              # The port used by the sonar server

ER1PORT   = 5001              # The port used by the ER1 server

MAPPORT   = 5005              # the port used by the map server


# Here is the sonar connection

sonarSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # python is cool

sonarSocket.connect((HOST, SONARPORT))   # ditto


# Here is the ER1 connection

er1Socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # python is cool

er1Socket.connect((HOST, ER1PORT))   # ditto


# Here is the map connection

mapSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # python is cool

mapSocket.connect((HOST, MAPPORT))   # ditto



#

# Definitions of useful functions:

# Send a command to the robot, sonar, map

#


def send(socket, command):
    socket.send(command)            # send it off to the server

    if command == "q":                   # if we want to quit

        return ""
    data = socket.recv(128)         # the server always responds!!

    lineReceived = data                  # gratuitous name change

    return lineReceived     

def sendSonar(command):
    global sonarSocket
    command += "\r\n"
    return send(sonarSocket, command)

def sendMap(command):
    global mapSocket
    command += "\r\n"
    send(mapSocket, command)

def sendRobot(command):
    global er1Socket
    return send(er1Socket, command)


#

# Updating the position

#


def updatePos():
    print "updatePos"
    global posX
    global posY
    global posTh
    global lastX
    global lastY
    global lastTh

    lastX = posX;
    lastY = posY;
    lastTh = posTh;

    odo = sendRobot("p")
    odostuff = odo.split(' ')
    posX = float(odostuff[0])
    posY = float(odostuff[1])
    posTh = float(odostuff[2])   

#

# Sending stuff to map funcs

#


def sendPosToMap():
    print "sendPosToMap"
    global posX
    global posY
    global posTh
    global lastX
    global lastY
    global lastTh
    sendMap("d " + str(posX - lastX) + " " + str(posY - lastY) + " " + str(posTh - lastTh))

def sendSonarToMap(val):
    print "sendSonarToMap"
    global servo
    sendMap("s " + str(val) + " " + str(servo))

def updateParticles():
    print "updateParticles"
    sendMap("p")         # Turn particles off

    sendMap("m")
    time.sleep(0.1)
    sendMap("n")
    time.sleep(0.1)
    sendMap("b")
    sendMap("p")         # Turn particles on




#

# getSonar: Get a signal from the sonar, possibly averaging over several tries

#


def getOneSonarReading():
    global sonarMaxTrust
    val = int(sendSonar("1"))
    if val > sonarMaxTrust: 
        val = int(sendSonar("1"))
    lastSonar = val
    return val

def doMcl():
    global sonarMaxTrust
    val = int(sendSonar("1"))
    if val > sonarMaxTrust: 
        val = int(sendSonar("1"))
    print "got sonar"
    updatePos()
    sendPosToMap()
    time.sleep(0.1)
    sendSonarToMap(val)
    time.sleep(0.1)
    updateParticles()
    lastSonar = val
    return val


def getSonarAvg(maxTries):
    avg = 0
    tries = 0
    while tries < maxTries:
        val = getOneSonarReading()
        avg += val
        tries += 1
    avg /= maxTries
    print "Sonar: " + str(avg)
    return avg

def getSonar():
    global sonarNumTries
    return getSonarAvg(sonarNumTries)



#

# Here is a function to move the servo to a position.

# It takes an int as input.

#


def moveServo(amount):
    global servo
    global servoCenter
    servo = amount - servoCenter
    sonarString = "BD1SV2M" + str(amount)
    sendSonar(sonarString)
    sendSonar("1")              # Must throw out first sonar reading after servo move



#

# The emergency behavior

# Stop

# Play emergency sound (not done yet)

# Turn until you can move back (not done yet)

# Move back until you are not in range

#


def emergency():
    global maxloop
    global sonarTooClose
    global velocitySlow
    loopvar = 0
    print sendRobot("h");        # stop

    while not msvcrt.kbhit():
        sonarVal = getSonar()
        print "Sonar distance:", sonarVal
        if sonarVal < sonarTooClose:
            lineToSend = "v f " + str(velocitySlow) + " 10 0"
            print sendRobot(lineToSend)
        else:
            break
        loopvar += 1
        if loopvar > maxloop:
            break;
    sendRobot("h");

#

# Wall chase

# Move the robot forward until it is within a range of the wall

# Add a little safety margin

# This will also move the robot back if it is too close

#


def gotoWall(dist):
    global velocityNormal
    global velocitySlow
    global sonarTooClose
    global sonarTolerance
    global sonarHallWidth
    minDist = sonarTooClose
    maxDist = dist
    if maxDist < sonarTooClose:
        maxDist = sonarTooClose

    moveServo(servoCenter)
    lastwall = 1000
    sign = 1
    while not msvcrt.kbhit():
        wall = doMcl()           # getSonar()

        if wall < sonarTooClose:
            print "EMERGENCY!"
            return 0
        if wall > sonarHallWidth:
            wall = getSonar()
            if wall > sonarHallWidth:
                print "I lost the wall!"
                return 0
        diff = wall - lastwall
        print "Diff is", diff
        if sign * diff > sonarTolerance:
            print "I do not think I am following a real wall"
            sendRobot("h")
            return 0
        if wall > maxDist:
            sendRobot("v b " + str(velocitySlow) + " 10 0")
            sign = 1            # We are going forwards

        elif wall < minDist:
            sendRobot("v f " + str(velocitySlow) + " 10 0")
            sign = -1           # We are going backwards

        else:
            print "Got the wall at dist", wall
            break
        lastwall = wall
        time.sleep(0.5)
    sendRobot("h")
    return 1


#

# Forward motion until the sonar detects some obstacle to the forward

#

def longForward():
    global velocityNormal
    global sonarHallWidth
    global sonarClose
    global sonarTolerance

    moveServo(servoCenter)
    print "Going forward"
    sendRobot("v b " + str(velocityNormal) + " 10 0")
    while not msvcrt.kbhit():
        lastWall = getSonar()
        if lastWall < sonarTooClose:
            print "EMERGENCY"
            return 0
        time.sleep(0.5)
        nextWall = getSonar()
        if nextWall < sonarTooClose:
            print "EMERGENCY"
            return 0
        if nextWall < sonarHallWidth:
            sendRobot("h")
            print "2 FOUND A WALL at dist", nextWall
            gotoWall(sonarClose)
            return 1     

#

# Find a direction with no wall in it

#

def findOpening():
    global servoLeft
    global servoCenter
    global servoRight
    global sonarHallWidth

    moveServo(servoCenter)
    centerWall = doMcl()
    if centerWall < sonarTooClose:
        print "EMERGENCY"
        return 0
    if centerWall > sonarHallWidth:            # Can go straight, idiot

        print "I SHOULD GO STRAIGHT"
        return 1
    print "I CAN'T GO STRAIGHT"
    moveServo(servoLeft)
    currLeft = doMcl()
    if currLeft > sonarHallWidth:
        print "I CAN GO LEFT - sonar says", currLeft
        sendRobot("v tl 0.5 0.5 1.51")
        return 1
    moveServo(servoRight)
    if doMcl() > sonarHallWidth:
        print "I CAN GO RIGHT"
        sendRobot("v tr 0.5 0.5 1.51")
        return 1
    # Could not go left or right

    moveServo(servoCenter)
    print "I MUST MAKE A U TURN"
    sendRobot("v tr 0.5 0.5 3.14")
    return 1
#

# Left-wall-finding behavior

# Move the robot forward slowly until it finds a wall on the left

# or  can't go forward any more.

#


def wallFind():
    global servoLeft
    global servoRight
    global servoCenter
    global sonarClose
    global sonarHallWidth
    global maxloop
    global velocitySlow

    tries = 0

    while not msvcrt.kbhit():
        moveServo(servoCenter)
        currSonar = getSonar()
        if currSonar < sonarClose:
            print "TROUBLE: currSonar says", currSonar
            return 0
        loopVar = 0;
        while getSonar() > sonarClose and loopVar < 5:
            sendRobot("v b " + str(velocitySlow) + " 10 0");
            time.sleep(1)
            loopVar += 1
        sendRobot("h")
        moveServo(servoLeft)
        currSonar = getSonar()
        if currSonar < sonarHallWidth:
            print "Found left wall at", currSonar
            return 1
        moveServo(servoRight)
        currSonar = getSonar()
        if currSonar < sonarClose:      # Too close to right wall

            print "Too close to right wall at", currSonar
            sendRobot("v tl 0.5 0.5 0.2")
        tries += 1
        if tries > maxloop:
            print "TOOK TOO MANY TRIES TO FIND A WALL. SOMETHING IS WRONG."
            return 0

#

# Wall-aligning behavior

# Aligns the robot's wheels so that the robot is parallel to the left wall.

#


def wallAlign():
    global servoCenter
    global servoLeft
    global servoRight
    global velocityNormal
    global sonarHallWidth
    global sonarTolerance
    global sonarClose
    global maxloop

    loopVar = 0
    tryToReposition = 1
    while not msvcrt.kbhit() and loopVar < maxloop:
        moveServo(servoCenter)
        wall = getSonar()
        if wall < sonarClose:
            wall = getSonar()               # Confirm

            if wall < sonarClose:
                print "End of corridor reached while trying to align with wall."
                return 0
        moveServo(servoLeft)
        lastWall = getSonar()
        if lastWall > sonarHallWidth:
            print "I cannot find a wall: sonar said", lastWall
            return 0                 # No wall to align to

        elif lastWall < sonarTooClose and tryToReposition == 1: # I am too close to the wall

            print "I am too close to the wall. Sonar said ", lastWall
            sendRobot("v tl 0.5 0.5 0.9")
            moveServo(servoCenter)
            straightWall = getSonar()
            if straightWall > sonarClose:
                # Something is wrong

                tryToReposition = 0
                print "Wall is weird. trying to align without repositioning."
            if gotoWall(sonarClose) == 0:   # GotoWall failed - something is wrong

                tryToReposition = 0         # Try to realign without repositioning

            sendRobot("v tr 0.5 0.5 0.9")
            continue
        elif lastWall > sonarClose and tryToReposition == 1:  # I want to get closer to the wall

            print "I want to be closer to the wall. Sonar said ", lastWall
            sendRobot("v tl 0.5 0.5 0.9")
            moveServo(servoCenter)
            straightWall = getSonar()
            if straightWall < sonarClose:
                # Something is wrong

                tryToReposition = 0
                print "Wall is weird. trying to align without repositioning."
            if gotoWall(sonarClose) == 0:   # GotoWall failed - something is wrong

                tryToReposition = 0         # Try to realign without repositioning

            sendRobot("v tr 0.5 0.5 0.9")
            continue
        sendRobot("v d -8")          # Go forward

        currWall = getSonar()
        if currWall > sonarHallWidth:
            print "My wall disappeared! Sonar said", currWall
            return 0                 # Wall went away

        if currWall > lastWall + sonarTolerance:    # Facing too far right

            print "Too far right with diff", currWall - lastWall
            print "currWall:", currWall, "lastWall:", lastWall
            sendRobot("v tl 0.5 0.5 0.1")
        elif currWall < lastWall - sonarTolerance:  # Facing too far left

            print "Too far left with diff", currWall - lastWall
            print "currWall:", currWall, "lastWall:", lastWall
            sendRobot("v tr 0.5 0.5 0.1")
        else:
            print "I am aligned with the left wall at", currWall-lastWall
            print "Distance is", currWall
            return 1
        loopVar += 1
    print "Took too many tries to align. Exiting."
    return 0



#

# Higher-level behaviors

#


def decision():
    if findOpening() == 0:
        print "I CANNOT MAKE A DECISION. PLEASE HELP."
        return 0
    # If there is a way to go, I am now facing it.

    if wallFind() == 0:
        print "COULD NOT FIND LEFT WALL."
        return 0
    return 1    

def normal():
    if wallAlign() == 0:
        print "COULD NOT ALIGN WITH THE LEFT WALL. I WILL TRY TO FIND IT."
        if wallFind() == 0:
            print "HELP, I AM IN TROUBLE."
            return 0
        else:
            print "Found wall, restaring behavior."
            return 0         
    if longForward() == 0:
        print "I AM NOT ALIGNED WITH THE WALL. I WILL TRY AGAIN."
        return 0
    return 1

        

#

# Initialization

#

sendMap("p")
#

# Main loop

#


while not msvcrt.kbhit():
    moveServo(servoCenter)
    doMcl()
    sonarVal = getSonar()
    if sonarVal < sonarTooClose:
        print "Emergency", sonarVal
        emergency()
    elif sonarVal < sonarClose:
        print "Decision", sonarVal
        decision()
    else:
        print "Normal", sonarVal
        normal()

moveServo(servoCenter)
sendRobot("v b 0 10 0");

sendRobot("q")
sendSonar("q")
sendMap("q")

er1Socket.close()
sonarSocket.close()
mapSocket.close()

print "Done"

syntax highlighted by Code2HTML, v. 0.9.1