示例#1
0
    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()
示例#2
0
文件: robot.py 项目: evenbrenna/pi2go
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()
示例#3
0
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)
示例#4
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)
示例#5
0
文件: motorM.py 项目: lenaback/Test
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
示例#6
0
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()
示例#7
0
# 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()
示例#8
0
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()
示例#9
0
      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))
示例#11
0
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()