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 routineLeft():
robot.sonarSpin(-angle)
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()
# robot.forward(42)
print "!! I AM AT POINT 2 !!"
time.sleep(1)
robot.forward(-42)
robot.turnLeftDeg(90)
robot.sonarSpin(90-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)) #turn back for wall following
robot.followWallLeft(distance, diswall)
print "!! I AM AT POINT 1 !!"
time.sleep(1)
robot.sonarSpin(2*angle)
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)) #turn back for wall following
robot.followWallBackwards(distance, diswall)
print "!! I AM AT POINT 3 !!"
def routineLeft():
robot.sonarSpin(-angle)
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()
# robot.forward(42)
print "!! I AM AT POINT 2 !!"
time.sleep(1)
robot.forward(-42)
robot.turnLeftDeg(90)
robot.sonarSpin(90 - 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)) #turn back for wall following
robot.followWallLeft(distance, diswall)
print "!! I AM AT POINT 1 !!"
time.sleep(1)
robot.sonarSpin(2 * angle)
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)) #turn back for wall following
robot.followWallBackwards(distance, diswall)
print "!! I AM AT POINT 3 !!"
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 !!"
from robot import robot robot = robot() for a in range(4): for b in range(4): robot.forward(10) robot.turnRightDeg(90) robot.terminate()
def runToWallAt21(forward=1): # -1 for backward
toWall = robot.getSonarSingle() + 5 - 21
if forward == -1:
toWall -= 2
print "still need = " + str(toWall)
robot.forward(toWall * forward)
distance = robot.distance()
turnSpeed = (x - 200) / 2
if (turnSpeed < 0):
turnSpeed = -turnSpeed
robot.setTurnSpeed(turnSpeed)
robot.setDefaultSpeed(200)
if (x == 0):
robot.stop_driving()
robot.stop_turning()
elif (distance > 100):
if (0 < x < 190):
robot.left()
robot.forward()
elif (210 < x):
robot.right()
robot.forward()
else:
robot.stop_turning()
robot.forward()
else:
if (0 < x < 170):
robot.left()
robot.stop_driving()
elif (230 < x):
robot.right()
robot.stop_driving()
else:
robot.stop_turning()
from robot import robot
robot = robot()
for a in range(4):
for b in range(4):
robot.forward(10)
robot.turnRightDeg(90)
robot.terminate()
def runToWallAt21(forward=1): # -1 for backward
toWall = robot.getSonarSingle() + 5 - 21
if forward == -1:
toWall -= 2
print "still need = " + str(toWall)
robot.forward(toWall*forward)
startTime = datetime.now()
measurements = robot.turnSonarTakingMeasurements()
# Change sonar to move FASTAAAAAAAAAAAAR!
robot.setMotorAngleControllerParameters(robot.sonar_motor, fastParams)
# place.get_Loc(measurements)
robot.sonarSpin(-90)
toturn = robot.getMeanAngle(measurements) -13
toturn = toturn if toturn <= 180 else toturn - 360
robot.turnLeftDeg(toturn)
robot.forward(disout)
#### ***Decide location
sonarRight = robot.getSonarMeasurements(1)[0]
robot.sonarSpin(-180)
sonarLeft = robot.getSonarMeasurements(1)[0]
if sonarRight > 30:
if sonarLeft > 30:
position = "middle"
startTime = datetime.now()
measurements = robot.turnSonarTakingMeasurements()
# Change sonar to move FASTAAAAAAAAAAAAR!
robot.setMotorAngleControllerParameters(robot.sonar_motor, fastParams)
# place.get_Loc(measurements)
robot.sonarSpin(-90)
toturn = robot.getMeanAngle(measurements) - 13
toturn = toturn if toturn <= 180 else toturn - 360
robot.turnLeftDeg(toturn)
robot.forward(disout)
#### ***Decide location
sonarRight = robot.getSonarMeasurements(1)[0]
robot.sonarSpin(-180)
sonarLeft = robot.getSonarMeasurements(1)[0]
if sonarRight > 30:
if sonarLeft > 30:
position = "middle"
else:
position = "left"
else:
position = "right"
