def __init__(self):
pi2go.init()
# Main body of code - this detects your key press and changes direction depending on it.
try:
while True:
keyp = self.readkey
if keyp == 'w' or keyp == UP:
pi2go.forward(SPEED)
print 'Forward', SPEED
elif keyp == 's' or keyp == DOWN:
pi2go.reverse(SPEED)
print 'Backward', SPEED
elif keyp == 'd' or keyp == RIGHT:
pi2go.spinRight(SPEED)
print 'Spin Right', SPEED
elif keyp == 'a' or keyp == LEFT:
pi2go.spinLeft(SPEED)
print 'Spin Left', SPEED
elif keyp == '.' or keyp == '>':
SPEED = min(100, SPEED+10)
print 'SPEED+', SPEED
elif keyp == ',' or keyp == '<':
SPEED = max (0, SPEED-10)
print 'SPEED-', SPEED
elif keyp == ' ':
pi2go.stop()
print 'Stop'
elif ord(keyp) == 3:
break
# When you want to exit - press ctrl+c and it will generate a keyboard interrupt - this is handled nicely here!
except KeyboardInterrupt:
pi2go.cleanup()
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 manual():
"""Manual control.
Use for straight_line if autonomous mode not working.
Use for obstacle course if autonomous mode not working.
Use for joust and skittles."""
BTN_LEFT = 80
BTN_RIGHT = 79
BTN_DOWN = 81
BTN_UP = 82
BTN_FAST = 28 # Y
BTN_SLOW = 17 # N
BTN_STOP = 44 # Space
BTN_EXIT = 41 # ESC
#explorerhat.light.green.on()
global leftspeed, rightspeed
while not pi2go.getSwitch():
control = None
try:
control = dev.read(endpoint.bEndpointAddress, endpoint.wMaxPacketSize, USB_TIMEOUT)
#print(control)
except:
pass
if control != None:
logging.debug('control={}'.format(control))
if BTN_DOWN in control:
pi2go.reverse(max(leftspeed, rightspeed))
if BTN_UP in control:
pi2go.forward(max(leftspeed, rightspeed))
if BTN_LEFT in control:
pi2go.spinLeft(max(leftspeed, rightspeed))
if BTN_RIGHT in control:
pi2go.spinRight(max(leftspeed, rightspeed))
if BTN_FAST in control:
leftspeed = min(leftspeed + 10, 100)
rightspeed = min(rightspeed + 10, 100)
logging.debug('leftspeed={} rightspeed={}'.format(leftspeed, rightspeed))
if BTN_SLOW in control:
leftspeed = max(leftspeed - 10, 0)
rightspeed = max(rightspeed -10, 0)
logging.debug('leftspeed={} rightspeed={}'.format(leftspeed, rightspeed))
if BTN_STOP in control:
pi2go.stop()
if BTN_EXIT in control:
break
time.sleep(0.02)
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)
speed = 30
degree = 0
pi2go.init()
pi2go.startServos()
# Main body of code - this detects your key press and changes direction depending on it
try:
while True:
keyp = readkey()
if keyp == 'w' or keyp == UP:
pi2go.forward(speed)
print 'Forward', speed
elif keyp == 's' or keyp == DOWN:
pi2go.reverse(speed)
print 'Backward', speed
elif keyp == 'd' or keyp == RIGHT:
pi2go.spinRight(speed)
print 'Spin Right', speed
elif keyp == 'a' or keyp == LEFT:
pi2go.spinLeft(speed)
print 'Spin Left', speed
elif keyp == '.' or keyp == '>':
speed = min(100, speed+10)
print 'Speed+', speed
elif keyp == ',' or keyp == '<':
speed = max (0, speed-10)
print 'Speed-', speed
print "Use . or > to speed up"
print "Speed changes take effect when the next arrow key is pressed"
print "Press Ctrl-C to end"
print
pi2go.init()
# main loop
try:
while True:
keyp = readkey()
if keyp == 'w' or ord(keyp) == 16:
pi2go.forward(speed)
print 'Forward', speed
elif keyp == 'z' or ord(keyp) == 17:
pi2go.reverse(speed)
print 'Reverse', speed
elif keyp == 's' or ord(keyp) == 18:
pi2go.spinRight(speed)
print 'Spin Right', speed
elif keyp == 'a' or ord(keyp) == 19:
pi2go.spinLeft(speed)
print 'Spin Left', speed
elif keyp == '.' or keyp == '>':
speed = min(100, speed + 10)
print 'Speed+', speed
elif keyp == ',' or keyp == '<':
speed = max(0, speed - 10)
print 'Speed-', speed
elif keyp == ' ':
pi2go.stop()
# 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()
try:
while True:
key=raw_input("Where do you want to go (W, A, S, D or other)?")
if key=="d":
pi2go.setAllLEDs(0, 4095, 0)
print "right"
pi2go.spinRight(100)
elif key=="a":
pi2go.setAllLEDs(0, 0, 4095)
print "left"
pi2go.spinLeft(100)
elif key=="s":
pi2go.setAllLEDs(4095, 0, 0)
print "back"
pi2go.reverse(100)
elif key=="w":
pi2go.setAllLEDs(4095, 4095, 4095)
print "up"
pi2go.forward(100)
else:
pi2go.setAllLEDs(0, 0, 0)
pi2go.stop()
except KeyboardInterrupt:
print
pi2go.cleanup()
else:
state=state+1
if state==1:
pi2go.setAllLEDs(0, 4095, 0)
elif state==1:
pi2go.forward(60)
pi2go.setAllLEDs(4095, 4095, 0)
elif state==2:
pi2go.setAllLEDs(0, 4095, 4095)
elif state==3:
pi2go.setAllLEDs(4095, 0, 4095)
else:
pi2go.setAllLEDs(0, 0, 4095)
elif inp=="w":
pi2go.forward(60)
elif inp=="a":
pi2go.spinLeft(60)
elif inp=="d":
pi2go.spinRight(60)
elif inp=="s":
pi2go.reverse(60)
else:
pi2go.stop()
except KeyboardInterrupt:
#except:
finished = True
time.sleep(1) # wait for threads to finish
pi2go.cleanup()
sys.exit()
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))
def intersection():
centerx = 317
center_y = 404
radius = 370
global prev_dist[]= {1,2,3,4,5,6}
init_dist[] = {270,270,270,270,270,270}
tty.setcbreak(sys.stdin.fileno())
speed = 50
try:
while 1:
linefollow(speed)
print 'linefollow 1'
# ultrasonic sensor's obstacle detection
obs = ult_dist()
if (obs <= 4):
pi2go.stop()
print 'stop when bot is in front'
time.sleep(1.2)
elif (obs == 5):
speed = 35
#print 'speed = 45'
# manual input to control bot
if isData():
c = sys.stdin.read(1)
if c == '\x6c': # lane change 'l'
print 'left-lane change'
# ard.write('2')
left()
elif c == '\x72': # lane change 'r'
print 'right-lanechange'
# ard.write('3')
right()
elif c == '\x20': #spacebar
print 'stop'
# ard.write('1')
#ard.write('4')
pi2go.stop()
time.sleep(5)
# sys.exit(0)
elif c == '\x77': # 'w'
print 'forward'
pi2go.forward(speed)
time.sleep(3)
elif c == '\x73': #'s'
print 'reverse' # 's'
pi2go.reverse(speed)
time.sleep(3)
elif c == '\x61': # 'a'
print 'left'
pi2go.spinLeft(speed)
time.sleep(3)
elif c == '\x64': # 'd'
print 'right'
pi2go.spinRight(speed)
time.sleep(3)
elif c == '\x2e': #period
print 'increase speed'
speed = max(0, speed + 10)
elif c == '\x2c': # comma
print 'decrease speed'
speed = min(100, speed-10)
elif c == '\x65': #e to exit from program
print 'exiting...'
sys.exit(0)
id_1, x_1, y_1 = gps.gps_client(1)
id_2, x_2, y_2 = gps.gps_client(2)
id_3, x_3, y_3 = gps.gps_client(3)
id_4, x_4, y_4 = gps.gps_client(4)
id_5, x_5, y_5 = gps.gps_client(5)
id_6, x_6, y_6 = gps.gps_client(6)
print id_6, x_6, y_6
if id_6 == 6:
if (x_6 == 0 and y_6 == 0) or (x_6 == -1 and y_6 == -1) : # not valid or GPS values 0 or -1
print 'no id'
continue
else:
dx = abs(x_6 - centerx)
dy = abs(y_6 - centery)
if ((dx + dy) <= radius):
print 'true'
distance_6 = math.sqrt((dx * dx) + (dy * dy)) # 200 - to stop at intersection
quadrant_6 = dir(x_6, y_6)
if (int_dist[5] > distance_6 or (distance_6+ 5 <= init_dist[5] or init_dist[5] >= distance_6 - 5)):
print 'ID %d arriving Intersection' %id_6
if id_1 == 1:
init_dist[0] = find(id_1, x_1, y_1, init_dist[0], quadrant_6, distance_6)
if id_2 == 2:
init_dist[1] = find(id_2, x_2, y_2, init_dist[1], quadrant_6, distance_6)
if id_3 == 3:
prev_dist_3 = find(id_3, x_3, y_3, init_dist[2], quadrant_6, distance_6)
if id_4 == 4:
init_dist[3] = find(id_4, x_4, y_4, init_dist[3], quadrant_6, distance_6)
if id_5 == 5:
init_dist[4] = find(id_4, x_4, y_4, init_dist[4], quadrant_6, distance_6)
init_dist[5] = distance_6
else:
print 'ID %away from Intersection' % id_6
init_dist[5] = distance_6
else:
print 'Id %d not at Intersection' %id_6
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
pi2go.cleanup()