def runNoWallR(left, right, front):
if front > 40:
if right > 100:
motor.robot_forward()
elif right < 20:
if right < 5:
motor.robot_leftforward(50)
else:
motor.robot_leftforward(30)
elif left < 30 or right > 30:
motor.robot_rightforward(30)
else:
motor.robot_forward()
else:
motor.robot_stop()
def runR(left, right, front):
if front>40:
if right<25:
if right < 5:
motor.robot_leftforward(50)
else:
motor.robot_leftforward(30)
elif right >= 100:
motor.robot_forward()
elif right > 30 or left < 30:
if right < 40:
motor.robot_rightforward(20)
else:
motor.robot_rightforward(30)
else:
motor.robot_forward()
else:
motor.robot_stop()
def runL(left, right, front):
if front>40:
if left<25:
if left < 5:
motor.robot_rightforward(50)
else:
motor.robot_rightforward(30)
elif left >= 100:
motor.robot_forward()
elif left>30 or right<30:
if left < 40:
motor.robot_leftforward(20)
else:
motor.robot_leftforward(30)
else:
motor.robot_forward()
#print('robot_forward')
else:
motor.robot_stop()
def Control(q,src,dest):
global cur
global cornerFlag
global videoFlag
Sonar.ultraInit()
On = Sonar.SoNarUse()
On.Distance = 10
SR = threading.Thread(target = Sonar.SoNarRight,args = (On.R,On.b,On.On))
SL = threading.Thread(target = Sonar.SoNarLeft,args = (On.L,On.b,On.On))
SF = threading.Thread(target = Sonar.SoNarFront,args = (On.F,On.b,On.On))
SR.start()
SL.start()
SF.start()
motor.QuickStart()
isAscending = nodeAscendingCheck(src,dest)
startDecision(isAscending) #decision check before loop because turning does not happen all the time except for corners
videoFlag = True;
while q.empty():
On.b.wait()
#src != cur prevent cornerDecision colliding with startDecision
if cornerCheck(cur) and src != cur:
#to prevent doing cornerDecision many times in one spot
if cornerFlag == False:
cornerDecision(isAscending)
cornerFlag = True
elif cornerCheck(dest):
if cur == 7 or cur == 14:
runNoWallR(On.L.Distance, On.R.Distance, On.F.Distance))
elif cur == 1 or cur == 8:
runNoWallL(On.L.Distance, On.R.Distance, On.F.Distance))
else:
if cur == 8:
if On.F.Distance > 36:
else:
runR(On.L.Distance, On.R.Distance, On.F.Distance)
On.b.wait()
#Turn when destination reach, if corner is destination skip this step
if cornerCheck(dest) == False:
if isAscending():
motor.turn_left()
else:
motor.turn_right()
#Delivering to door
while True:
On.b.wait()
if On.F.Distance>36:
if cur == 7 or cur == 14:
runNoWallL()
elif cur == 1 or cur == 8:
runNoWallR()
else:
motor.robot_forward()
else:
break
On.b.wait()
On.b.wait()
On.On.Distance = 0;
On.b.wait()
motor.robot_stop()
motor.knock()
print('***PUSH***')
while GPIO.input(motor.SW) != True:
pass
turn_back()
robot_stop()
cornerFlag = False