#
# 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
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 = 3
velocitySlow = 7
velocityNormal = 15
maxloop = 5
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("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
minDist = sonarTooClose
maxDist = dist
if maxDist < sonarTooClose:
maxDist = sonarTooClose
moveServo(servoCenter)
lastwall = 1000
sign = 1
while not msvcrt.kbhit():
wall = getSonar()
if wall < sonarTooClose:
print "EMERGENCY!"
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 < lastWall - 2*sonarTolerance or 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 = getSonar()
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 = getSonar()
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 getSonar() > 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
#
#
# Main loop
#
while not msvcrt.kbhit():
moveServo(servoCenter)
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")
er1Socket.close()
sonarSocket.close()
print "Done"
syntax highlighted by Code2HTML, v. 0.9.1