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)
# 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 on_message(self, message):
#Messages are of the form: "MessageType/Instruction" hence each message
#from scratch needs to be separated into is consistuent parts.
print message
msg= message.split("/")
#MOTOR FUNCTIONS
if msg[0]== 'stop':
pi2go.stop()
elif msg[0]== 'forward':
pi2go.forward(float(msg[1]))
elif msg[0]== 'reverse':
pi2go.reverse(float(msg[1]))
elif msg[0]== 'spinLeft':
pi2go.spinLeft(float(msg[1]))
elif msg[0]== 'spinRight':
pi2go.spinRight(float(msg[1]))
elif msg[0]== 'turnForward':
pi2go.turnForward(float(msg[1]), float(msg[2]))
elif msg[0]== 'turnReverse':
pi2go.turnReverse(float(msg[1]), float(msg[2]))
elif msg[0]== 'goM':
pi2go.go(float(msg[1]), float(msg[2]))
elif msg[0]== 'go':
pi2go.go(float(msg[1]))
# SERVO FUNCTIONS
#elif msg[0]== 'startServos':
#pi2go.startServos()
#elif msg[0]== 'stopServos':
#pi2go.stopServos()
#elif msg[0]== 'setServos':
#pi2go.setServo(msg[1],float(msg[2]))
# LED FUNCTIONS
#elif msg[0]== 'setLED':
#pi2go.setLED(msg[1], msg[2], msg[3], msg[4])
#elif msg[0]== 'setAllLEDs':
#pi2go.setAllLEDs(msg[1], msg[2], msg[3])
elif msg[0]== 'LsetLED':
pi2go.LsetLED(msg[1], msg[2])
# IR FUNCTIONS
elif msg[0]== 'irLeft':
val = pi2go.irLeft()
self.write_message(str(val))
elif msg[0]== 'irRight':
val = pi2go.irRight()
self.write_message(str(val))
elif msg[0]== 'irLeftLine':
val =pi2go.irLeftLine()
self.write_message(str(val))
elif msg[0]== 'irRightLine':
val= pi2go.irRightLine()
self.write_message(str(val))
# ULTRASONIC FUNCTION
elif msg[0]== 'ultraSonic':
val=pi2go.getDistance()
self.write_message(str(val))