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 __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 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 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 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)
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)
#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)
# Main body of code - this detects your key press and changes direction depending on it try: while True: #keyp = readkey() #uncomment to receive command from keyboard keyp, addr = sock.recvfrom(1024) #uncomment to receive command from algo. #print "received" if keyp == 'w' or keyp == UP: #wont be used pi2go.forward(speed) print 'Forward', speed elif keyp == 's' or keyp == DOWN: #wont be used pi2go.reverse(speed) print 'Backward', speed elif keyp == 'd' or keyp == RIGHT: pi2go.spinRight(speed, roll) #pi2go.forward(speed) print 'Spin Right', roll print 'turnspeed', speed turnflag = 'r' #print 'turnflag' , turnflag elif keyp == 'a' or keyp == LEFT: pi2go.spinLeft(speed, roll) #pi2go.forward(speed) print 'Spin Left', roll print 'turnspeed', speed turnflag = 'l' #print 'turnflag' , turnflag elif keyp == 'h': speed = 0
#!/usr/bin/python # manual.py # manual keyboard control (w forward, a left, d right, s back, other key stop) press enter using library # Author : Zachary Igielman import pi2go, sys pi2go.init() 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)
# 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()
turn = 'Straight' else: turn = 'Right' if time.time() - tickStart > 4: currentNode = currentNode.insertTurn('Turn ', turn) ctrSpinLeft = 0 ctrSpinRight = 0 tickStart = time.time() # End of maze detected, end learn mode in preparation for optimum run elif pi2go.getDistance() <= 9: print('Exit is found! ending programme...') keyp = 'o' # If the left sensor is on, spin right: elif left == True: pi2go.spinRight(turnSpeed) ctrSpinRight = ctrSpinRight + 1 #If the right sensor is on, spin left: elif right == True: pi2go.spinLeft(turnSpeed) ctrSpinLeft = ctrSpinLeft + 1 # Process learning maze mode if keyp != 'l': secondrunchoice = raw_input( 'Would you like to begin the second run using the optimum path (y), or end the program (n)?' ) if secondrunchoice == 'y': print('Beginning optimum run!') keyp = 'o'
def square_up(): speed = 100 """Square up to the wall.""" # square up to the wall time5degrees = timespincircle * 5.0 / 360 * 2 logging.debug('Time 5 degrees={}'.format(time5degrees)) # sense distance of wall distance_mid = getmyDistance() logging.debug('Distance_mid={}'.format(distance_mid)) # turn 5 degrees left and sense distance of wall pi2go.spinLeft(speed) time.sleep(time5degrees) pi2go.stop() distance_left = getmyDistance() logging.debug('Distance_left={}'.format(distance_left)) # turn back and 5 degrees more right and sense distance of wall pi2go.spinRight(speed) time.sleep(2*time5degrees) pi2go.stop() distance_right = getmyDistance() logging.debug('Distance_right={}'.format(distance_right)) # turn back to where we started pi2go.spinLeft(speed) time.sleep(time5degrees) pi2go.stop() # calculate how much off we are if distance_left > distance_right: # scan right for minimum distance_old = distance_left distance_new = distance_mid while distance_new < distance_old: logging.debug('Distance old={} new={}'.format(distance_old, distance_new)) pi2go.spinRight(speed) time.sleep(time5degrees / 10.0) pi2go.stop() distance_old = distance_new distance_new = getmyDistance() logging.debug('Distance old={} new={}'.format(distance_old, distance_new)) # spin back pi2go.spinLeft(speed) time.sleep(time5degrees / 10.0) pi2go.stop() else: # scan left for minimum distance_old = distance_right distance_new = distance_mid while distance_new < distance_old: logging.debug('Distance old={} new={}'.format(distance_old, distance_new)) pi2go.spinLeft(speed) time.sleep(time5degrees / 10.0) pi2go.stop() distance_old = distance_new distance_new = getmyDistance() logging.debug('Distance old={} new={}'.format(distance_old, distance_new)) # spin back pi2go.spinRight(speed) time.sleep(time5degrees / 10.0) pi2go.stop() exit() # turn to face the wall #if angle <> 0: # if angle < 0: # spinleft(50) # angle # else: # spinright(50) # angle # time(t) # stop() # return distance to go distance_mid = getmyDistance() logging.debug('Distance_mid={}'.format(distance_mid)) return distance_mmid
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))
turn = 'Straight' else: turn = 'Right' if time.time()-tickStart>4: currentNode = currentNode.insertTurn('Turn ',turn) ctrSpinLeft = 0 ctrSpinRight = 0 tickStart = time.time() # End of maze detected, end learn mode in preparation for optimum run elif pi2go.getDistance() <= 9: print ('Exit is found! ending programme...') keyp = 'o' # If the left sensor is on, spin right: elif left == True: pi2go.spinRight(turnSpeed) ctrSpinRight = ctrSpinRight + 1 #If the right sensor is on, spin left: elif right == True: pi2go.spinLeft(turnSpeed) ctrSpinLeft = ctrSpinLeft + 1 # Process learning maze mode if keyp!='l': secondrunchoice = raw_input('Would you like to begin the second run using the optimum path (y), or end the program (n)?') if secondrunchoice =='y': print ('Beginning optimum run!') keyp = 'o' elif secondrunchoice == 'n': print ('Ending Program')
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()
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() print 'Stop' elif ord(keyp) == 3: break
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()
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 elif keyp == 'h': pi2go.stopServos() print 'Servos stopped'
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()