def mainLoop():
global globalDistance, globalStop, state, finished
global slowspeed, fastspeed
while finished == False:
if globalStop==1 or globalDistance<5:
pi2go.stop()
else:
if state==1: # Standard Line Follower
if pi2go.irLeftLine() and pi2go.irRightLine():
pi2go.forward(40)
elif pi2go.irRightLine()==False:
pi2go.spinRight(fastspeed)
elif pi2go.irLeftLine()==False:
pi2go.spinLeft(fastspeed)
elif state==2: # Obstacle avoider (reverses then spins when near object)
if globalDistance>15:
pi2go.forward(50)
else:
pi2go.reverse(30)
time.sleep(0.5)
pi2go.turnReverse(30,50)
time.sleep(3)
elif state==3: # Obstacle avoider (spins when near object)
if globalDistance>15:
pi2go.forward(50)
else:
pi2go.spinLeft(50)
elif state==4: # Avoids objects using IR sensors only
if pi2go.irAll()==False:
pi2go.forward(50)
else:
ir=pi2go.irRight()
pi2go.reverse(30)
time.sleep(0.5)
if ir:
pi2go.turnReverse(50,30)
else:
pi2go.turnReverse(30,50)
time.sleep(3)
import pi2go as p2 from time import sleep p2.init() a = p2.getDistance() b = p2.irAll() while True: if p2.getSwitch() == True: while a > 30 and b != True: p2.forward(75) p2.LsetLED(1,1) sleep(0.5) p2.LsetLED(1,0) a = p2.getDistance() b = p2.irAll() else: p2.spinRight(75) p2.LsetLED(1,1) sleep(0.7) p2.LsetLED(1,0) a = p2.getDistance() b = p2.irAll()
#!/usr/bin/python # irNav.py # navigate using ir obstacle detectors with pi2go library # Author : Zachary Igielman import time import pi2go pi2go.init() fast=50 slow=30 while True: if pi2go.irAll()==False: pi2go.forward(fast) pi2go.setAllLEDs(4095, 4095, 4095) else: ir=pi2go.irRight() pi2go.setAllLEDs(4095, 0, 0) pi2go.reverse(slow) time.sleep(0.5) if ir: pi2go.setAllLEDs(0, 0, 4095) pi2go.turnReverse(fast,slow) time.sleep(3) else: pi2go.setAllLEDs(0, 4095, 0) pi2go.turnReverse(slow,fast) time.sleep(3) pi2go.cleanup()
def isObstaclePresent():
if pi2go.irAll():
return True
else:
return False
def find(id_, x_, y_, init_dist_, quadrant_6, distance_6):
centerx = 317
centery = 404
radius = 450
i_radius = 150
if (x_ == 0 and y_ == 0) or (x_ == -1 and y_ == -1): # GPS
print 'false' #if 0 or -1
else:
dx = abs(x_ - centerx)
dy = abs(y_ - centery)
if ((dx + dy) <= radius):
print 'true'
dist_ = math.sqrt((dx * dx) + (dy * dy))
quadrant_ = dir(x_, y_)
print 'quadrant_', quadrant_
print 'quadrant_6', quadrant_6
if ((dx + dy) <= i_radius):
if distance_6 < 380:
pi2go.stop()
print 'stop1'
time.sleep(2)
if quadrant_6 == -2:
if quadrant_ == -1:
print 'ID %d found on right quadrant 0' %id_
if init_dist_ > distance_:
print 'near the center'
if distance_6 < 380:
print 'stop2'
pi2go.stop()
#print 'stop'
time.sleep(2) # stop
init_dist_ = distance_
elif (distance_ + 5 <= init_dist_ or init_dist_ >= distance_ - 5):
print 'ID %d has stopped' %id_
if distance_6 < 380:
pi2go.stop()
print 'stop3'
time.sleep(2)
else:
init_dist_ = distance_
if quadrant_6 == -1:
if quadrant_ == 0:
print 'ID %d found on right quarant -1' % id_
if init_dist_ > distance_:
print 'arriving center'
if distance_6 < 380:
print 'stop4'
pi2go.stop() # stop
time.sleep(2)
init_dist_ = distance_
elif (distance_ + 5 <= init_dist_ or init_dist_ >= distance_ - 5):
print 'ID %d has stopped' %id_
if distance_6 < 380:
pi2go.stop()
time.sleep(2)
else:
# print 'moving away'
init_dist_ = distance_
if quadrant_6 == 0:
if quadrant_ == 1:
print 'ID %d found on right quadrant 1' % id_
if init_dist_ > distance_:
print 'arriving center'
if distance_6 < 380:
print 'stop5'
pi2go.stop() # stop
time.sleep(2)
prev_dist_ = dist_
elif (dist_ + 5 <= prev_dist_ or prev_dist_ >= dist_ - 5):
print 'ID %d has stopped moving' %id_
if dist_6 < 380:
pi2go.irAll()
pi2go.stop()#new
time.sleep(2)
else:
init_dist_ = distance_
if quad_6 == 1:
if quad_ == -2:
print 'ID %d found on right quadrant -2' % id_
if init_dist_ > distance_:
print 'arriving center'
if distance_6 < 380:
print 'stop6'
pi2go.stop() # stop
time.sleep(2)
init_dist_ = distance_
elif (distance_ + 5 <= init_dist_ or init_dist_ >= distance_ - 5):
print 'ID %d has stopped ' %id_
if distance_6 < 380:
pi2go.irAll()
pi2go.stop()
time.sleep(2)
else: