# 

# The emergency behavior

# Back away slowly from anything too close to the sonar

#

# No sonar calibration is done - a value is hard coded as "too close"

#

# This assumes the sonar is already facing forward.

# If not, the command to move the servo is BD1SV2M200 (where 200 is whatever position we want)

#


#

# 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

#

# Hard-coded variables

# sonar variables are definitions of distances on the sonar

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

#


sonarTooClose = 15
velocitySlow = 3

#

# 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 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   



#

# Test both connections

#

print "Hit enter to start."
yesno = raw_input();

while 1:
    print "sending 1"
    sonarVal = sendSonar("1")
    print sonarVal
    if int(sonarVal) < sonarTooClose:
        lineToSend = "v b " + str(velocitySlow) + " 10 0"
        print sendRobot(lineToSend);
    else:
        print sendRobot("v b 0 10 0");
    print "Do another loop?"
    yesno = raw_input();
    if yesno == "n":
        break

sendRobot("v b 0 10 0");

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

er1Socket.close()
sonarSocket.close()

print "Done"