def getout():
leftLine = pi2go.irLeftLine()
rightLine = pi2go.irRightLine()
while (not (leftLine and rightLine)):
pi2go.spinLeft(20)
leftLine = pi2go.irLeftLine()
rightLine = pi2go.irRightLine()
def calibrate(action):
"""Calibrate the robot by performing the action until the left line sensor has changed
to dark three times.
Returns time taken for action"""
count = 0
timestart = 0.0
timeend = 0.0
online = False
# start the motor action, e.g. spinleft(100)
action
# act until see dark line
while pi2go.irLeftLine():
time.sleep(0.01)
online = True
timestart = time.time()
while count < 2:
if not pi2go.irLeftLine():
if not online:
online = True
count += 1
elif online:
online = False
time.sleep(0.01)
timeend = time.time()
pi2go.stop()
logging.debug('Time={}-{}={}'.format(timeend, timestart, (timeend-timestart)/2))
return (timeend-timestart)/3
def gotoline(speed, over=True):
"""Proceed forward until cross the line."""
pi2go.goBoth(speed)
while not pi2go.irLeftLine():
time.sleep(0.1)
if over:
# get off the line
while pi2go.irLeftLine():
time.sleep(0.1)
def line(speed):
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
if left == False and right == True:
pi2go.forward(speed)
elif right == True and left == True:
pi2go.spinLeft(speed - 20) # speed-20 to smoothen the line follow
elif left == True and right == False:
pi2go.spinRight(speed - 20) # speed-20 to smoothen the line follow
elif left == False and right == False:
pi2go.forward(speed)
def linefollow(speed):
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
if left == False and right == True:
pi2go.forward(speed)
elif right == True and left == True:
pi2go.spinLeft(speed - 30)
elif left == True and right == False:
pi2go.spinRight(speed - 30)
elif left == False and right == False:
pi2go.forward(speed)
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)
def pi2goMove():
#print "pi2go Move"
if not piStop:
if pi2go.irLeftLine() == False:
#print "Left"
pi2go.turnForward(0.3*speed, speed)
elif pi2go.irRightLine() == False:
#print "Right"
pi2go.turnForward(speed, 0.3*speed)
else:
#print "Straight"
pi2go.forward(speed)
else:
pi2go.forward(0)
def distance():
stime = time.time()
while True:
# Defining the sensors
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
ntime = time.time()
# timecheck for distance
if ntime > (stime + 0.1):
#print "%.5f" %time.time()
dist = (int(pi2go.getDistance() * 10)) / 10.0
#print "%.5f" %time.time()
stime = ntime
# distance groups
if dist < 10:
pi2go.setAllLEDs(2000, 0, 0)
elif dist > 20:
pi2go.setAllLEDs(0, 0, 2000)
else:
pi2go.setAllLEDs(0, 2000, 0)
def distance():
stime = time.time()
while True:
# Defining the sensors
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
ntime = time.time()
# timecheck for distance
if ntime > (stime + 0.1):
#print "%.5f" %time.time()
dist = (int(pi2go.getDistance()*10))/10.0
#print "%.5f" %time.time()
stime = ntime
# distance groups
if dist < 10:
pi2go.setAllLEDs(2000, 0, 0)
elif dist > 20:
pi2go.setAllLEDs(0, 0, 2000)
else:
pi2go.setAllLEDs(0, 2000, 0)
def calibrateRight():
print "Calibrating right..."
pi2go.setAllLEDs(0, 400, 0)
pi2go.spinRight(20)
leftLine = not pi2go.irLeftLine()
rightLine = not pi2go.irRightLine()
while (not leftLine and not rightLine):
leftLine = not pi2go.irLeftLine()
rightLine = not pi2go.irRightLine()
if (rightLine):
while (rightLine):
leftLine = not pi2go.irLeftLine()
rightLine = not pi2go.irRightLine()
while (not leftLine):
leftLine = not pi2go.irLeftLine()
rightLine = not pi2go.irRightLine()
pi2go.spinLeft(20)
while (leftLine):
leftLine = not pi2go.irLeftLine()
else:
pi2go.spinLeft(20)
while (not rightLine):
leftLine = not pi2go.irLeftLine()
rightLine = not pi2go.irRightLine()
pi2go.spinRight(20)
while (rightLine):
leftLine = not pi2go.irLeftLine()
rightLine = not pi2go.irRightLine()
pi2go.stop()
pi2go.setAllLEDs(0, 0, 0)
#tollgate 2, task 1a
import pi2go, time
import sys
pi2go.init()
speed = 60
try:
while True:
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
if left == False and right == True:
pi2go.forward(speed)
elif left == True and right == True:
pi2go.spinLeft(speed - 20)
elif left == True and right == False:
pi2go.spinRight(speed - 20)
elif left == False and right == False:
pi2go.forward(speed)
finally:
pi2go.cleanup()
sys.exit(0)
import time, pi2go
pi2go.init()
vsn = pi2go.version()
if vsn == 1:
print "Running on Pi2Go"
else:
print "Running on Pi2Go-Lite"
count = 1
try:
while True:
print 'Count: ', count
print 'Left:', pi2go.irLeft()
if vsn == 1:
print 'Centre:', pi2go.irCentre()
print 'Right:', pi2go.irRight()
print 'Line left', pi2go.irLeftLine()
print 'Line right', pi2go.irRightLine()
print
count += 1
time.sleep(1)
except KeyboardInterrupt:
print
finally:
pi2go.cleanup()
getout()
calibrateRight()
pi2go.setAllLEDs(0, 0, 0)
pi2go.stop()
baseSpeed = 30
reassureTime = 1500
lastTouchLeft = millis()
lastTouchRight = millis()
mode = "NORMAL"
while True:
leftLine = not pi2go.irLeftLine()
rightLine = not pi2go.irRightLine()
distance = pi2go.getDistance()
if (distance < 1):
print "Distance too low! Turning! ", distance
pi2go.stop()
for i in range(3):
pi2go.setAllLEDs(400, 0, 0)
time_.sleep(0.3)
pi2go.setAllLEDs(0, 0, 0)
time_.sleep(0.3)
pi2go.spinLeft(20)
while (not rightLine):
#!/usr/bin/env python import pi2go import time pi2go.init() speed = 50 print "vorwaerts!" pi2go.forward(speed) print "fuer 2 Sekunden..." time.sleep(2) print "drehe links!" pi2go.spinLeft(speed) print "fuer 3 Sekunden..." time.sleep(3) print "stop" pi2go.stop() linkerLinienSensor = pi2go.irLeftLine() rechterLinienSensor = pi2go.irRightLine() print "Linker Linien-Sensor: ", linkerLinienSensor print "Rechter Linien-Sensor: ", rechterLinienSensor pi2go.cleanup()
#!/usr/bin/env python
#Simply prints the state of the line follower 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.irLeftLine()
print 'Right:', pi2go.irRightLine()
print
time.sleep(1)
except KeyboardInterrupt:
print
finally:
pi2go.cleanup()
def line_follower():
"""
State:
L R
1 1 - Both White - Depends on P
1 0 - Left White - Turn Left
0 1 - Right White - Turn Right
0 0 - Both Black - Go Forward
P - previous State
------------------
0 - Left
1 - Right
:return:
"""
dist = 0
speed = 70
change = 20
start = 0
STATE = 00
prev_STATE = 11
STOP = False
while True:
#print "line follower %f" %time.time()
# we don't need to start a thread if theres only one.....
# print 'get dist: %f' % time.time()
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
# print 'get ir: %f' % time.time()
if not left and not right: # If both sensors are the on --> forward
# pi2go.forward(speed)
STATE = 00
elif left and not right: # If the left sensor is Off --> move right
# pi2go.turnForward(speed+change, speed-change)
STATE = 10
elif right and not left: # If the right sensor is off --> move left
# pi2go.turnForward(speed - change, speed + change)
STATE = 01
else: # stop
# pi2go.stop()
STATE = 11
if time.time() - start > 0.2:
dist = (int(pi2go.getDistance() * 10)) / 10.0
#out_q.put(dist)
start = time.time()
if dist > 10:
STOP = False
STATE = 69
else:
STOP = True
if STOP:
pi2go.stop()
if STATE == prev_STATE:
pass
elif STATE == 00:
pi2go.forward(speed)
# pi2go.stop()
elif STATE == 10:
pi2go.turnForward(speed + change, speed - change)
elif STATE == 01:
pi2go.turnForward(speed - change, speed + change)
elif STATE == 11:
# pi2go.forward(speed)
pi2go.stop()
prev_STATE = STATE
# lf.py
# slow line follower using pi2go library
# Author : Zachary Igielman
import sys, time
import pi2go
pi2go.init()
pwmMax = 4095
fast = 40
try:
while True:
if pi2go.irLeftLine() and pi2go.irRightLine():
pi2go.forward(fast)
pi2go.setAllLEDs(pwmMax, pwmMax, pwmMax) # Turn LEDs White for Forwards
print('straight')
elif pi2go.irRightLine()==False:
pi2go.spinRight(fast)
pi2go.setAllLEDs(pwmMax, 0, 0) # Turn LEDs Red for Right
print('right')
elif pi2go.irLeftLine()==False:
pi2go.spinLeft(fast)
pi2go.setAllLEDs(0, 0, pwmMax) # Turn LEDs Blue to Left
print('left')
except KeyboardInterrupt:
pi2go.setAllLEDs (0, 0, 0)
pi2go.cleanup()
sys.exit()
import time, pi2go
pi2go.init()
vsn = pi2go.version()
if vsn == 1:
print "Running on Pi2Go"
else:
print "Running on Pi2Go-Lite"
count = 1
try:
while True:
print 'Count: ', count
print 'Left:', pi2go.irLeft()
if vsn == 1:
print 'Centre:', pi2go.irCentre()
print 'Right:', pi2go.irRight()
print 'Line left', pi2go.irLeftLine()
print 'Line right', pi2go.irRightLine()
print
count += 1
time.sleep(1)
except KeyboardInterrupt:
print
finally:
pi2go.cleanup()
#!/usr/bin/env python
#Simply prints the state of the line follower 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.irLeftLine()
print 'Right:', pi2go.irRightLine()
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))
import pi2go, time
# Reading single character by forcing stdin to raw mode
import sys
import tty
import termios
import threading
# main loop
speed = 30
pi2go.init()
try:
while True:
if (pi2go.irLeftLine() == True and pi2go.irRightLine() == True):
pi2go.go(speed * 0.94, speed)
elif (pi2go.irLeftLine() == False):
pi2go.spinLeft(speed)
elif (pi2go.irRightLine() == False):
pi2go.spinRight(speed)
elif (pi2go.irLeftLine() == False and pi2go.irRightLine() == False):
pi2go.stop()
#print(pi2go.irLeftLine())
#print(pi2go.irRightLine())
#print('#######')
#time.sleep(3)
finally:
pi2go.cleanup()
pi2go.go(movingSpeed, movingSpeed)
# Turn right
if (turnRight):
# Turn right until right sensor doesn't recognize the line anymore
pi2go.go(30, -30)
if (pi2go.irRightLine()):
goForward = True
turnLeft = False
turnRight = False
turnAround = False
# Turn left
if (turnLeft):
# Turn left until left sensor doesn't recognize the line anymore
pi2go.go(-30, 30)
if (pi2go.irLeftLine()):
goForward = True
turnLeft = False
turnRight = False
turnAround = False
# Turn around
while turnAround:
# Turn right fast until the both sensor detects a line
pi2go.go(70, -70)
if (not pi2go.irRightLine()):
rightSensorFoundTurnLine = True;
if (not pi2go.irLeftLine() and rightSensorFoundTurnLine):
# Both sensor detected a line, which means that we are over turned.
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
finally:
pi2go.cleanup()
def line_follower():
"""
State:
L R
1 1 - Both White - Depends on P
1 0 - Left White - Turn Left
0 1 - Right White - Turn Right
0 0 - Both Black - Go Forward
P - previous State
------------------
0 - Left
1 - Right
:return:
"""
dist = 0
speed = 70
change = 20
start = 0
STATE = 00
prev_STATE = 11
STOP = False
stop = False
prev_stop = True
while True:
#print "line follower %f" %time.time()
# print 'get dist: %f' % time.time()
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
# print 'get ir: %f' % time.time()
if not left and not right: # If both sensors are the on --> forward
STATE = 00
elif left and not right: # If the left sensor is Off --> move right
STATE = 10
elif right and not left: # If the right sensor is off --> move left
STATE = 01
else:
STATE = 11
if time.time() - start > 0.15:
dist = (int(pi2go.getDistance()*10))/10.0
print dist
while dist < 25:
dist = (int(pi2go.getDistance()*10))/10.0
pi2go.stop()
STATE = 69
time.sleep(0.15)
if stop == False:
pi2go.setAllLEDs(4095,0,0)
stop = True
start = time.time()
stop = False
if stop == prev_stop:
pass
elif stop == False:
pi2go.setAllLEDs(0,4095,0)
if STATE == prev_STATE:
pass
elif STATE == 00:
pi2go.forward(speed)
elif STATE == 10:
pi2go.turnForward(speed + change, speed - change)
elif STATE == 01:
pi2go.turnForward(speed - change, speed + change)
elif STATE == 11:
pi2go.stop()
prev_stop = stop
prev_STATE = STATE
import pi2go
import time
pi2go.init()
# Here we set the speed to 60 out of 100 - feel free to change!
speed = 40
change = 25
try:
stime = time.time()
while True:
# Defining the sensors
left = pi2go.irLeftLine()
right = pi2go.irRightLine()
ntime = time.time()
# timecheck for distance
if ntime > (stime + 0.1):
#print "%.5f" %time.time()
dist = (int(pi2go.getDistance()*10))/10.0
#print "%.5f" %time.time()
stime = ntime
# distance groups
if dist < 10:
pi2go.setAllLEDs(2000, 0, 0)
elif dist > 20:
pi2go.setAllLEDs(0, 0, 2000)
def follow_line():
"""Follow a black line on a white background"""
#SIM_PULSE = 0.035
TURN_RATE = 50
STEP_RATE = 1 #0.5
SPEED_LINE = 35
logging.debug('Turn-rate={} Step-rate={}'.format(TURN_RATE, STEP_RATE))
# start speed
speed = SPEED_LINE
logging.debug('speed={}'.format(speed))
# start with no turn-rate
turn = 0.0
# count steps on this turn-rate; use to accelerate
step = 1
logging.debug('turn={} step={}'.format(turn, step))
while not pi2go.getSwitch(): #action
#logging.debug('time0={}'.format(time.time()))
# turn left if left sensor detects dark line
if not pi2go.irLeftLine():
#logging.debug('Left')
if turn > 0:
step = 0
#turn = -TURN_RATE
#turn = max(turn - TURN_RATE, -100)
speed = TURN_RATE-20
turn = -TURN_RATE
elif not pi2go.irRightLine():
#logging.debug(' Right')
if turn < 0:
step = 0
#turn = TURN_RATE
#turn = min(turn + TURN_RATE, 100)
speed = TURN_RATE-20
turn = TURN_RATE
else:
# no change for now as line is between sensors
#logging.debug(' None ')
speed = SPEED_LINE
turn = 0
#pass
# accelerate
step += 1
#step = min(step + STEP_RATE, 100)
#logging.debug("turn={} step={}".format(turn, step))
leftspeed = max(min(speed + turn + step * STEP_RATE * my_sign(turn), 100), -100)
rightspeed = max(min(speed - turn + step * STEP_RATE * my_sign(-turn), 100), -100)
#logging.debug("turn={} step={} lspeed={} rspeed={}".format(turn, step, leftspeed, rightspeed))
#logging.debug("lspeed={} rspeed={}".format(leftspeed, rightspeed))
#pi2go.stop()
#time.sleep(0.5)
pi2go.go(leftspeed, rightspeed)
# Ignore the time delay to sample as fast as possible.
#logging.debug('time1={}'.format(time.time()))
#time.sleep(SIM_PULSE)
# IGNORE button code for now.
#if getButton[0] = 1:
# # button is pressed to wait for release to flip action state
# while getButton[0] =1:
# time (0.1)
# action = start-stop(action)
pi2go.stop()
