# 

# Wandering behavior.

# 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

#

# 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 = 20
sonarClose = 50
sonarHallWidth = 130

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

sonarTolerance = 10      # Error tolerance of the sonar

sonarNumTries  = 3

velocitySlow = 10
velocityNormal = 20

maxloop = 50

servoCenter = 125
servoLeft = 240
servoRight = 30

#

# 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


# 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




#

# Definitions of useful functions:

# Send a command to the socket

# Send a command to the sonar

#


def sendSonar(command):
    global sonarSocket
    command += "\r\n"
    sonarSocket.send(command)            # send it off to the server

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

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

    lineReceived = data                  # gratuitous name change

    return lineReceived          

def getSonar():
    global sonarMaxTrust
    global sonarNumTries
    avg = 0
    tries = 0
    while tries < sonarNumTries:
        val = int(sendSonar("1"))
        if val > sonarMaxTrust:
            val = int(sendSonar("1"))
        avg += val
        tries += 1
    avg /= sonarNumTries
    return avg

def sendRobot(command):
    global er1Socket
    er1Socket.send(command)              # send it off to the server

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

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

    lineReceived = data                  # gratuitous name change

    return lineReceived   

#

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

# It takes an int as input.

#


def moveServo(amount):
    sonarString = "BD1SV2M" + str(amount)
    sendSonar(sonarString)
    sendSonar("1")                  # Must throw out first sonar reading after 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("v f 0 10 0");        # 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:
            print sendRobot("v f 0 10 0");
            break
        loopvar += 1
        if loopvar > maxloop:
            print sendRobot("v f 0 10 0")
            break;
    sendRobot("v f 0 10 0");

#

# 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

    minDist = sonarTooClose
    if minDist < dist - 2*sonarTolerance:
        minDist = dist - 2*sonarTolerance
    maxDist = dist - sonarTolerance
    if maxDist < sonarTooClose:
        maxDist = sonarTooClose

    moveServo(servoCenter)
    while not msvcrt.kbhit():
        wall = getSonar()
        if wall > maxDist:
            sendRobot("v b " + str(velocitySlow) + " 10 0")
        elif wall < minDist:
            sendRobot("v f " + str(velocitySlow) + " 10 0")
        else:
            print "Got the wall!"
            break
    sendRobot("v f 0 10 0")
    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 < sonarHallWidth:   # there HAS to be an obstacle

            print "1 FOUND A WALL at dist", lastWall
            gotoWall(sonarClose)
            sendRobot("h")     # stop the robot


            return 1  # Later this should return odometry

        nextWall = getSonar()
        if nextWall < lastWall - sonarTolerance or nextWall < sonarHallWidth:
            print "2 FOUND A WALL at dist", nextWall
            gotoWall(sonarClose)
            sendRobot("h")

            return 1     


def turnTo(dir):
    global servoCenter
    moveServo(servoCenter)
    currWall = getSonar()
    while not msvcrt.kbhit():
        sendRobot("v t" + dir + " 0.5 0.5 0.3")
        nextWall = getSonar()
        if nextWall > currWall + 50:
            break
        

#

# Initialization

#


#

# Main loop

#


while 1:
    moveServo(servoCenter)
    sonarVal = getSonar()
    if sonarVal < sonarTooClose:
        print "Emergency", sonarVal
        emergency()

    print "Commands: ";
    print " w - Forward";
    print " s - Back";
    print "Space bar - stop";
    print " a - Turn left";
    print " d - Turn right";
    print " q - Quit";  


    c = msvcrt.getch();   # get a keypress (no enter needed)

    if c == ' ':
        print "STOPPING"
        sendRobot("h");
    if c == 'w':
        print "GOING FORWARD"
        longForward()
        print "DONE"
    if c == 's':
        print "GOING BACK... HIT SPACE TO STOP"
        sendRobot("v f 10 10 1");
    if c == 'a':
        print "TURNING LEFT"
        turnTo("l")
        print "DONE"
    if c == 'd':
        print "TURNING RIGHT"
        turnTo("r")
        print "DONE"
    if c == 'q':
        break

moveServo(servoCenter)
sendRobot("h");

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


er1Socket.close()
sonarSocket.close()

print "Done"