def checkSides():
global TRIG_R, ECHO_R, TRIG_L, ECHO_L, maxX, maxY, posX, posY, direction, roomList, \
threshold_L, threshold_R, hallwayWidth, seperationWidth, rightDist, leftDist
rightDist = ts.takeSamples(TRIG_R, ECHO_R)
leftDist = ts.takeSamples(TRIG_L, ECHO_L)
time.sleep(0.5)
hallwayWidth = leftDist + rightDist + seperationWidth
print("Right Distance: ", rightDist, "cm")
print("Left Distance: ", leftDist, "cm")
if (direction is "North") or (direction is "South"):
if (posX + 1 > maxX):
bumpMaxX()
if (posX == 0):
bumpMinX()
if (leftDist < threshold_L):
if direction is "North":
markObstacle("West")
else:
markObstacle("East")
if (rightDist < threshold_R):
if direction is "North":
markObstacle("East")
else:
markObstacle("West")
if (direction is "East") or (direction is "West"):
if (posY + 1 > maxY):
bumpMaxY()
if (posY == 0):
bumpMinY()
if (leftDist < threshold_L):
if direction is "East":
markObstacle("North")
else:
markObstacle("South")
if (rightDist < threshold_R):
if direction is "East":
markObstacle("South")
else:
markObstacle("north")
def centerTest():
global TRIG_L, TRIG_R, ECHO_L, ECHO_R, robot, moveDist
rightDist = ts.takeSamples(TRIG_R, ECHO_R)
leftDist = ts.takeSamples(TRIG_L, ECHO_L)
print("TESTING CENTERING PROCESS")
print("LEFT is ", leftDist, "RIGHT is ", rightDist)
if ((leftDist > 1500) or
(rightDist > 1500)): # Greater than 1500 being considered infinite
print("RETURNING LEFT OR RIGHT DIST > 1500 cm")
return
if (rightDist < (rightDist + leftDist) / 2):
robot.go(0, 25)
robot.waitAngle(90)
robot.stop()
robot.go(25)
robot.waitDistance(((rightDist + leftDist) / 2) - rightDist)
robot.stop()
robot.go(0, -25)
robot.waitAngle(-90)
robot.stop()
elif (leftDist < (rightDist + leftDist) / 2):
robot.go(0, -25)
robot.waitAngle(-90)
robot.stop()
robot.go(25)
robot.waitDistance(((rightDist + leftDist) / 2) - leftDist)
robot.stop()
robot.go(0, 25)
robot.waitAngle(90)
robot.stop()
import takeSample as ts
ECHO = 23
TRIG = 27
distance = ts.takeSamples(TRIG, ECHO)
print("Distance:", distance, "cm")
def sensor():
global TRIG, ECHO, threshold_Front
distance = ts.takeSamples(TRIG, ECHO)
print("Front Distance:", distance, "cm")
return 1 if (distance < threshold_Front) else 0
def angleTest():
global TRIG_L, TRIG_R, ECHO_L, ECHO_R, robot, moveDist
rightDist = ts.takeSamples(TRIG_R, ECHO_R)
leftDist = ts.takeSamples(TRIG_L, ECHO_L)
print("LEFT is ", leftDist, "RIGHT is ", rightDist)
if ((leftDist > 1500)
or (rightDist > 1500)): # Greater than 1500 being considered
print("RETURNING LEFT OR RIGHT DIST > 1500 cm")
return # infinite for these purposes
robot.go(18, 0)
robot.waitDistance(moveDist / 8)
robot.go(25, 0)
robot.waitDistance(moveDist / 8)
robot.go(50, 0)
robot.waitDistance(moveDist / 2)
robot.go(25, 0)
robot.waitDistance(moveDist / 8)
robot.go(10, 0)
robot.waitDistance(moveDist / 8)
robot.stop()
rightDist2 = ts.takeSamples(TRIG_R, ECHO_R)
leftDist2 = ts.takeSamples(TRIG_L, ECHO_L)
print("LEFT2 is ", leftDist2, "RIGHT2 is ", rightDist2)
if ((leftDist2 > 1500) or (rightDist2 > 1500)):
print("RETURNING LEFT2 OR RIGHT2 DIST > 1500 cm")
return
if ((rightDist2 < rightDist) and (rightDist2 <
(rightDist + leftDist) / 2)):
theta = math.degrees(math.asin((rightDist - rightDist2) / moveDist))
print("Right Angle is ", theta)
if (theta > 90):
print("RETURNING, RIGHT ANGLE OVER 90")
return
robot.go(0, 25)
robot.waitAngle(2 * theta)
robot.stop()
robot.go(18, 0)
robot.waitDistance(moveDist / 8)
robot.go(25, 0)
robot.waitDistance(moveDist / 8)
robot.go(50, 0)
robot.waitDistance(moveDist / 2)
robot.go(25, 0)
robot.waitDistance(moveDist / 8)
robot.go(10, 0)
robot.waitDistance(moveDist / 8)
robot.stop()
robot.go(0, -25)
robot.waitAngle(-theta)
robot.stop()
elif ((leftDist2 < leftDist) and (leftDist2 < (rightDist + leftDist) / 2)):
theta = math.degrees(math.asin((leftDist - leftDist2) / moveDist))
if (theta > 90):
print("RETURNING LEFT ANGLE OVER 90")
return
print("Left Angle is ", theta)
robot.go(0, -25)
robot.waitAngle(-2 * theta)
robot.stop()
robot.go(18, 0)
robot.waitDistance(moveDist / 8)
robot.go(25, 0)
robot.waitDistance(moveDist / 8)
robot.go(50, 0)
robot.waitDistance(moveDist / 2)
robot.go(25, 0)
robot.waitDistance(moveDist / 8)
robot.go(10, 0)
robot.waitDistance(moveDist / 8)
robot.stop()
robot.go(0, 25)
robot.waitAngle(theta)
robot.stop()