def moveAround():
while True:
while not (pi2go.irLeft() or pi2go.irRight() or pi2go.irCentre()):
if pi2go.getDistance() <= 2.0:
pi2go.spinLeft(speed)
time.sleep(1.5)
else:
pi2go.forward(speed)
pi2go.stop()
if pi2go.irLeft():
while pi2go.irLeft():
pi2go.spinRight(speed)
time.sleep(0.5)
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
pi2go.spinLeft(speed)
time.sleep(0.5)
pi2go.stop()
if pi2go.irCentre():
while pi2go.irCentre():
pi2go.reverse(speed)
time.sleep(2)
pi2go.spinLeft(speed)
time.sleep(1)
pi2go.stop()
def displayStatus(self):
left=pi2go.irLeft()
right=pi2go.irRight()
leftLine=pi2go.irLeftLine()
rightLine=pi2go.irRightLine()
distance=pi2go.getDistance()
myscreen.addstr(2,30,"sonar "+str(distance))
myscreen.addstr(3,30,"left sensor "+str(left))
myscreen.addstr(4,30,"right sensor "+str(right))
myscreen.addstr(5,30,"leftIR sensor "+str(leftLine))
myscreen.addstr(6,30,"rightIR sensor "+str(rightLine))
myscreen.refresh()
def displayStatus(self):
left = pi2go.irLeft()
right = pi2go.irRight()
leftLine = pi2go.irLeftLine()
rightLine = pi2go.irRightLine()
distance = pi2go.getDistance()
myscreen.addstr(2, 30, "sonar " + str(distance))
myscreen.addstr(3, 30, "left sensor " + str(left))
myscreen.addstr(4, 30, "right sensor " + str(right))
myscreen.addstr(5, 30, "leftIR sensor " + str(leftLine))
myscreen.addstr(6, 30, "rightIR sensor " + str(rightLine))
myscreen.refresh()
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)
pi2go.init()
# Here we set the speed to 40 out of 100 - feel free to change!
speed = 40
# Here is the main body of the program - a lot of while loops and ifs!
# In order to get your head around it go through the logical steps slowly!
try:
while True:
if pi2go.irLeft():
while pi2go.irLeft():
# While the left sensor detects something - spin right
pi2go.spinRight(speed)
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
# While the right sensor detects something - spin left
pi2go.spinLeft(speed)
pi2go.stop()
while not (pi2go.irLeft() or pi2go.irRight()):
if pi2go.getDistance() <= 0.3: # If the distance is less than 0.3, spin right for 1 second
pi2go.spinRight(speed)
time.sleep(1)
else:
pi2go.forward(speed)
pi2go.stop()
finally: # Even if there was an error, cleanup
pi2go.cleanup()
#!/usr/bin/env python import pi2go import time pi2go.init() speed = 50 pi2go.stepForward(speed, 15) time.sleep(1) pi2go.stepSpinL(speed, 12) time.sleep(1) linkerSensor = pi2go.irLeft() rechterSensor = pi2go.irRight() distanz = pi2go.getDistance() knopf = pi2go.getSwitch() pi2go.cleanup() print "Linker Sensor: ", linkerSensor print "Rechter Sensor: ", rechterSensor print "Distanz: ", distanz print "Knopf: ", knopf
# 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()
pi2go.init()
# Here we set the speed to 40 out of 100 - feel free to change!
speed = 40
# Here is the main body of the program - a lot of while loops and ifs!
# In order to get your head around it go through the logical steps slowly!
try:
while True:
if pi2go.irLeft():
while pi2go.irLeft():
# While the left sensor is true - spin right
pi2go.spinRight(speed)
# This stops the motors
pi2go.stop()
if pi2go.irRight():
while pi2go.irRight():
# While the right sensor is true - spin left
pi2go.spinLeft(speed)
pi2go.stop()
while not (pi2go.irLeft() or pi2go.irRight()):
if pi2go.getDistance() <= 3: # If the distance is less than 3cm, spin right for 1 second
pi2go.spinRight(speed)
time.sleep(1)
else:
pi2go.forward(speed)
pi2go.stop()
finally: # Even if there was an error, cleanup
pi2go.cleanup()
#!/usr/bin/env python import pi2go import time pi2go.init() speed = 50 pi2go.stepForward(speed,15) time.sleep(1) pi2go.stepSpinL(speed, 12) time.sleep(1) linkerSensor = pi2go.irLeft() rechterSensor = pi2go.irRight() distanz = pi2go.getDistance() knopf = pi2go.getSwitch() pi2go.cleanup() print "Linker Sensor: ", linkerSensor print "Rechter Sensor: ", rechterSensor print "Distanz: ", distanz print "Knopf: ", knopf
#!/usr/bin/env python
#Simply prints the state of the obstacle sensors
# Must be run as root - sudo python .py
import time, pi2go
pi2go.init()
vsn = pi2go.version()
if vsn == 1:
print "Running on Pi2Go"
else:
print "Running on Pi2Go-Lite"
try:
while True:
print 'Left:', pi2go.irLeft()
if vsn == 1:
print 'Centre:', pi2go.irCentre()
print 'Right:', pi2go.irRight()
print
time.sleep(1)
except KeyboardInterrupt:
print
finally:
pi2go.cleanup()
pi2go.init()
vsn = pi2go.version()
if vsn == 1:
print "Running on Pi2Go"
else:
print "Running on Pi2Go-Lite"
try:
while True:
print 'obstacle'
print 'irLeft', GPIO.input(11)
print pi2go.irLeft()
print 'irRight', GPIO.input(7)
print pi2go.irRight()
print 'irCentre', GPIO.input(13)
print pi2go.irCentre()
print 'line'
print 'LeftLine', GPIO.input(12)
print pi2go.irLeftLine()
print 'RightLine', GPIO.input(15)
print pi2go.irRightLine()
#print 'Left:', pi2go.irLeftLine()
#print 'Right:', pi2go.irRightLine()
print
time.sleep(5)
except KeyboardInterrupt:
print
#!/usr/bin/env python
#Simply prints the state of the obstacle sensors
# Must be run as root - sudo python .py
import time, pi2go
pi2go.init()
vsn = pi2go.version()
if vsn == 1:
print "Running on Pi2Go"
else:
print "Running on Pi2Go-Lite"
try:
while True:
print 'Left:', pi2go.irLeft()
if vsn == 1:
print 'Centre:', pi2go.irCentre()
print 'Right:', pi2go.irRight()
print
time.sleep(1)
except KeyboardInterrupt:
print
finally:
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))