def routineRight():
robot.sonarSpin(angle)
robot.followWallBackwards(half/2, diswall)
robot.sonarSpin(-(90+angle))
distance = half_fromwall - robot.getSonarSingle()
robot.sonarSpin(90+angle)
robot.followWallBackwards(distance, diswall)
robot.turnRightDeg(90)
#turn sonar to wall and take measurement
robot.sonarSpin(-(90+angle)) # i am looking at small wall
runToWallAt21()
print "!! I AM AT POINT 2 !!"
time.sleep(1)
robot.forward(-42)
robot.turnLeftDeg(90)
robot.sonarSpin(90+angle)# turn back to look at left wall
robot.followWallBackwards(half-earlystop, diswall)
robot.sonarSpin(90-angle) # look at right wall
runToWallAt21(-1)
robot.turnLeftDeg(90)
distance = robot.getSonarSingle() - 21 + 2
robot.sonarSpin(-(90-angle)) #turn back for wall following
robot.followWallBackwards(distance, diswall)
print "!! I AM AT POINT 3 !!"
time.sleep(1)
robot.sonarSpin(-(angle*2))
robot.followWallLeft(small,diswall)
robot.turnRightDeg(90)
robot.followWallLeft(full-earlystop, diswall)
robot.sonarSpin(-(90-angle)) # look at right wall
runToWallAt21()
robot.turnRightDeg(90)
distance = robot.getSonarSingle() - 21
robot.sonarSpin(90-angle) #turn back for wall following
robot.followWallLeft(distance, diswall)
print "!! I AM AT POINT 1 !!"
def routineRight():
robot.sonarSpin(angle)
robot.followWallBackwards(half / 2, diswall)
robot.sonarSpin(-(90 + angle))
distance = half_fromwall - robot.getSonarSingle()
robot.sonarSpin(90 + angle)
robot.followWallBackwards(distance, diswall)
robot.turnRightDeg(90)
#turn sonar to wall and take measurement
robot.sonarSpin(-(90 + angle)) # i am looking at small wall
runToWallAt21()
print "!! I AM AT POINT 2 !!"
time.sleep(1)
robot.forward(-42)
robot.turnLeftDeg(90)
robot.sonarSpin(90 + angle) # turn back to look at left wall
robot.followWallBackwards(half - earlystop, diswall)
robot.sonarSpin(90 - angle) # look at right wall
runToWallAt21(-1)
robot.turnLeftDeg(90)
distance = robot.getSonarSingle() - 21 + 2
robot.sonarSpin(-(90 - angle)) #turn back for wall following
robot.followWallBackwards(distance, diswall)
print "!! I AM AT POINT 3 !!"
time.sleep(1)
robot.sonarSpin(-(angle * 2))
robot.followWallLeft(small, diswall)
robot.turnRightDeg(90)
robot.followWallLeft(full - earlystop, diswall)
robot.sonarSpin(-(90 - angle)) # look at right wall
runToWallAt21()
robot.turnRightDeg(90)
distance = robot.getSonarSingle() - 21
robot.sonarSpin(90 - angle) #turn back for wall following
robot.followWallLeft(distance, diswall)
print "!! I AM AT POINT 1 !!"
def routineMid():
robot.sonarSpin(-angle)
robot.followWallLeft(half-earlystop, diswall)
robot.sonarSpin(-(90-angle)) # look at right wall
runToWallAt21()
robot.turnRightDeg(90)
distance = robot.getSonarSingle() - 21
robot.sonarSpin((90-angle))
robot.followWallLeft(distance,diswall)
print "!! I AM AT POINT 1 !!"
time.sleep(1)
robot.sonarSpin(angle*2)
robot.followWallBackwards(small,diswall)
robot.turnLeftDeg(90)
robot.followWallBackwards(full-earlystop, diswall)
robot.sonarSpin((90-angle)) # look at right wall
runToWallAt21(-1)
robot.turnLeftDeg(90)
distance = robot.getSonarSingle() - 21 + 2
robot.sonarSpin(-(90-angle))
robot.followWallBackwards(distance, diswall)
print "!! I AM AT POINT 3 !!"
time.sleep(1)
robot.sonarSpin(-angle*2)
robot.followWallLeft(small,diswall)
robot.turnRightDeg(90)
robot.followWallLeft(half/2,diswall)
robot.sonarSpin(90+angle)
distance = half_fromwall - robot.getSonarSingle()
robot.sonarSpin(-(90+angle))
robot.followWallLeft(distance, diswall)
robot.turnRightDeg(90)
robot.sonarSpin(-(90-angle)) #
runToWallAt21()
print "!! I AM AT POINT 2 !!"
def routineMid():
robot.sonarSpin(-angle)
robot.followWallLeft(half - earlystop, diswall)
robot.sonarSpin(-(90 - angle)) # look at right wall
runToWallAt21()
robot.turnRightDeg(90)
distance = robot.getSonarSingle() - 21
robot.sonarSpin((90 - angle))
robot.followWallLeft(distance, diswall)
print "!! I AM AT POINT 1 !!"
time.sleep(1)
robot.sonarSpin(angle * 2)
robot.followWallBackwards(small, diswall)
robot.turnLeftDeg(90)
robot.followWallBackwards(full - earlystop, diswall)
robot.sonarSpin((90 - angle)) # look at right wall
runToWallAt21(-1)
robot.turnLeftDeg(90)
distance = robot.getSonarSingle() - 21 + 2
robot.sonarSpin(-(90 - angle))
robot.followWallBackwards(distance, diswall)
print "!! I AM AT POINT 3 !!"
time.sleep(1)
robot.sonarSpin(-angle * 2)
robot.followWallLeft(small, diswall)
robot.turnRightDeg(90)
robot.followWallLeft(half / 2, diswall)
robot.sonarSpin(90 + angle)
distance = half_fromwall - robot.getSonarSingle()
robot.sonarSpin(-(90 + angle))
robot.followWallLeft(distance, diswall)
robot.turnRightDeg(90)
robot.sonarSpin(-(90 - angle)) #
runToWallAt21()
print "!! I AM AT POINT 2 !!"
import sys
import time
sys.path.insert(0, '../')
from robot import robot
robot = robot(0, 0, 0, True, True)
robot.enableSonar()
# (273,21,-90), (525,21,-90), (21,21,-90)
#robot.findDistance(63)
while True:
robot.sonarSpin(110)
robot.followWallBackwards(400, 25)
time.sleep(2)
robot.sonarSpin(-40)
robot.followWallLeft(400, 25)
time.sleep(2)
robot.sonar_reset()
import sys
import time
sys.path.insert(0, '../')
from robot import robot
robot = robot(0, 0, 0, True, True)
robot.enableSonar()
# (273,21,-90), (525,21,-90), (21,21,-90)
#robot.findDistance(63)
while True:
robot.sonarSpin(110)
robot.followWallBackwards(400,25)
time.sleep(2)
robot.sonarSpin(-40)
robot.followWallLeft(400,25)
time.sleep(2)
robot.sonar_reset();
