Example #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()
Example #2
0
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
Example #3
0
def read_distance():
    global running
    while running:
        print pi2go.getDistance()
        if pi2go.getDistance() <= 10:
            pi2go.stop()
        time.sleep(0.25)
Example #4
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)
Example #5
0
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()
Example #6
0
def straight_line():
    """Straight line speed test."""
    # Full speed
    speed = 100
    
    # start inside a marked, A3-sized box.
    
    # cross the start line
    gotoline(speed, True)
    
    # continue to finish line
    gotoline(speed,True)
    
    # go 20cm further (cross the line)
    pi2go.goBoth(speed)
    time.sleep(time10cm*2)
    
    pi2go.stop()
Example #7
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)
Example #8
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)
Example #9
0
def proximity_test():
    """Perform whole PiWars Proximity Test challenge."""
    MIN_DIST = 1.45

    # drive forward 1.3m so that sensors are in range.
    # use wheel turn calibration on distance
    #pi2go.goBoth(100)
    #time.sleep(time10cm*1)  # test
    #time.sleep(time10cm*13)
    pi2go.stop()
    
    # square off to the wall
    distance = square_up()
    logging.debug('Distance={}'.format(distance))

    # proceed towards the wall slowing down as we approach
    # speeed 1 (max) at 20cm and 0 (min) at 0.5cm
    while distance >= MIN_DIST:
        logging.debug('Speed={}'.format((distance - MIN_DIST) / (20 - MIN_DIST) * 100))
        pi2go.goBoth(min((distance - MIN_DIST) / (20 - MIN_DIST) * 100, 100))
        distance = pi2go.getDistance()
        logging.debug('Distance={}'.format(distance))
    pi2go.stop()
Example #10
0
def start():
    state = 'INIT'
    prev_state = ''
    mode = 'STOP'
    prev_mode = ''

    try:
        while True:
            if state == 'INIT':
                SPEED_RUN = c.SPEED_RUN
                SPEED_WARN = round(float(SPEED_RUN) / 3, 0)
                SPEED_CONTROL_MAX = SPEED_RUN
                SPEED_CONTROL_MIN = SPEED_WARN
                DIST_MIN = c.DIST_MIN
                speed = 0
                distance = 0
                warning = []
                last_meas_time = 0
                times = []
                times.append(['prev_get_dist', 0.0])
                times.append(['prev_get_switch', 0.0])
                times.append(['prev_set_motor', 0.0])
                times.append(['get_warning', 0.0])
                times.append(['prev_set_LED', 0.0])
                flags = []
                flags.append(['set_motor', False])
                flags.append(['status_warn_LED', False])
                flags.append(['button_release', False])
                flags.append(['master_set_speed', False])
                flags.append(['master_set_button', False])
                flags.append(['master_set_LED', False])
                flags.append(['master_set_state', False])
                pi2go.init()
                pi2go.setAllLEDs(c.LED_ON, c.LED_ON, c.LED_ON)
                time.sleep(1)
                sock = com.init_nonblocking_receiver('', c.PORT)
                for x in range(c.TEAM_SIZE):
                    warning.append(True)
                OWN_IP = com.get_ip()
                OWN_ID = com.get_id_from_ip(OWN_IP)
                prev_state = state
                state = 'IDLE'

            if state == 'IDLE':
                if prev_state != 'IDLE':
                    pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_ON)
                    pi2go.stop()
                if helper.check_time_limit(times, 'prev_get_switch',
                                           c.WAIT_SWITCH):
                    # Pressed = 1, Released = 0
                    button = pi2go.getSwitch()
                    if not button:
                        helper.set_element(flags, 'button_release', True)
                    if button and helper.get_element(flags, 'button_release'):
                        helper.set_element(flags, 'button_release', False)
                        prev_mode = ''
                        prev_state = state
                        state = 'RUNNING'

                # change to sensor-based or master_idle type
                data = 'new_round'
                while data != '':
                    data, addr = com.receive_message(sock)
                    if data != '':
                        sender_ID = com.get_id_from_ip(addr[0])
                        if sender_ID < c.TEAM_START or sender_ID > c.TEAM_END:
                            command, value = com.string_to_command(data)
                            #
                            print 'MASTER:', sender_ID, ' : ', data
                            try:
                                if command == c.COMMAND_SPEED:
                                    helper.set_element(flags,
                                                       'master_set_speed',
                                                       True)
                                    prev_SPEED_RUN = SPEED_RUN
                                    if value == '+':
                                        SPEED_RUN += 5
                                    elif value == '-':
                                        SPEED_RUN -= 5
                                    else:
                                        SPEED_RUN = value
                                    print 'Set SPEED_RUN from ' + str(
                                        prev_SPEED_RUN) + ' to ' + str(
                                            SPEED_RUN)
                                elif command == c.COMMAND_DIST:
                                    prev_DIST_MIN = DIST_MIN
                                    if not value.isdigit():
                                        print "Something went terribly wrong with the protocol..."
                                        raise KeyboardInterrupt
                                    DIST_MIN = value
                                    print 'Set DIST_MIN from ' + str(
                                        prev_DIST_MIN) + ' to ' + str(DIST_MIN)
                                elif command == c.COMMAND_BLINK:
                                    helper.blink('white')
                                    helper.set_element(flags, 'master_set_LED',
                                                       True)
                                elif command == c.COMMAND_RESET:
                                    SPEED_RUN = c.SPEED_RUN
                                    SPEED_WARN = round(float(SPEED_RUN) / 3, 0)
                                    SPEED_CONTROL_MAX = SPEED_RUN
                                    SPEED_CONTROL_MIN = SPEED_WARN
                                    DIST_MIN = c.DIST_MIN
                                    helper.set_element(times, 'prev_get_dist',
                                                       0)
                                    helper.set_element(flags, 'master_set_LED',
                                                       True)
                                    helper.set_element(flags,
                                                       'master_set_speed',
                                                       True)
                                    warning = [True] * len(warning)
                                    print "Reset major values"
                                elif command == c.COMMAND_STATE:
                                    local_prev_state = state
                                    if value == c.VALUE_STATE_RUNNING:
                                        state = 'RUNNING'
                                    elif value == c.VALUE_STATE_IDLE:
                                        state = 'IDLE'
                                    print 'Going from state ' + local_prev_state + ' to state ' + state
                                elif command == c.COMMAND_TYPE:
                                    if value == c.VALUE_TYPE_ORIGINAL:
                                        value = helper.determine_team(OWN_ID)
                                    return value
                            except:
                                print "Error interpreting message from master! Continuing anyway"

            elif state == 'RUNNING':
                # Distance
                if helper.check_time_limit(times, 'prev_get_dist',
                                           c.WAIT_DIST):
                    time_between = time.time() - last_meas_time
                    last_meas_time = time.time()
                    new_dist = pi2go.getDistance()
                    if new_dist > 1:
                        distance = new_dist
                        #print 'dt:', time_between , distance

                # Obstacle = 1, No Obstacle = 0
                irCentre = pi2go.irCentre()

                # Obstacle Analysis
                if irCentre or (distance < DIST_MIN):
                    distance_level = 0
                elif distance > c.DIST_MAX:
                    distance_level = 2
                else:
                    distance_level = 1

                # Receive
                data = 'new_round'
                while data != '':
                    data, addr = com.receive_message(sock)
                    if data != '':
                        sender_ID = com.get_id_from_ip(addr[0])
                        if sender_ID == OWN_ID:
                            #print 'OWN: ' , sender_ID, ' : ' , data
                            continue
                        if sender_ID >= c.TEAM_START and sender_ID <= c.TEAM_END:
                            #print 'ROBOT: ', sender_ID, ' : ' , data
                            if data == 'PROBLEM':
                                warning[sender_ID - c.TEAM_START] = False
                            elif data == 'RELEASE':
                                warning[sender_ID - c.TEAM_START] = True
                        else:
                            try:
                                #print 'MASTER:' , sender_ID , ' : ' , data
                                command, value = com.string_to_command(data)
                                if command == c.COMMAND_SPEED:
                                    helper.set_element(flags,
                                                       'master_set_speed',
                                                       True)
                                    prev_SPEED_RUN = SPEED_RUN
                                    if value == '+':
                                        SPEED_RUN += 5
                                    elif value == '-':
                                        SPEED_RUN -= 5
                                    else:
                                        SPEED_RUN = value
                                    print 'MASTER: Set SPEED_RUN from ' + str(
                                        prev_SPEED_RUN) + ' to ' + str(
                                            SPEED_RUN)
                                elif command == c.COMMAND_DIST:
                                    prev_DIST_MIN = DIST_MIN
                                    if not value.isdigit():
                                        print "Something went terribly wrong with the protocol..."
                                        raise KeyboardInterrupt
                                    DIST_MIN = value
                                    print 'MASTER: Set DIST_MIN from ' + str(
                                        prev_DIST_MIN) + ' to ' + str(DIST_MIN)
                                elif command == c.COMMAND_BLINK:
                                    helper.blink('white')
                                    helper.set_element(flags, 'master_set_LED',
                                                       True)
                                elif command == c.COMMAND_RESET:
                                    SPEED_RUN = c.SPEED_RUN
                                    SPEED_WARN = round(float(SPEED_RUN) / 3, 0)
                                    SPEED_CONTROL_MAX = SPEED_RUN
                                    SPEED_CONTROL_MIN = SPEED_WARN
                                    DIST_MIN = c.DIST_MIN
                                    helper.set_element(times, 'prev_get_dist',
                                                       0)
                                    helper.set_element(flags, 'master_set_LED',
                                                       True)
                                    helper.set_element(flags,
                                                       'master_set_speed',
                                                       True)
                                    warning = [True] * len(warning)
                                    print 'MASTER: Reset major values'
                                elif command == c.COMMAND_STATE:
                                    helper.set_element(flags,
                                                       'master_set_state',
                                                       True)
                                    if value == c.VALUE_STATE_RUNNING:
                                        next_state = 'RUNNING'
                                    elif value == c.VALUE_STATE_IDLE:
                                        next_state = 'IDLE'
                                    print 'MASTER: Going from state ' + state + ' to state ' + next_state
                                #elif command == c.COMMAND_TYPE and value != c.VALUE_TYPE_COM:
                                elif command == c.COMMAND_TYPE:
                                    local_prev_value = value
                                    if value == c.VALUE_TYPE_ORIGINAL:
                                        value = helper.determine_team(OWN_ID)
                                    print "MASTER: Changing from type " + local_prev_value + " to type " + value
                                    return value
                            except:
                                print "Error interpreting message from master! Continuing anyway"

                # Analyse --> Calculate MODE
                if prev_state == 'RUNNING':
                    prev_mode = mode
                if distance_level == 0:
                    mode = 'STOP'
                elif distance_level == 1 and all(warning):
                    mode = 'SLOW'
                elif distance_level == 2 and all(warning):
                    mode = 'RUN'
                elif distance_level != 0 and not all(warning):
                    mode = 'WARN'

                # Set own Warning-Flag
                if mode != prev_mode:
                    if mode == 'STOP':
                        warning[OWN_ID - c.TEAM_START] = False
                    else:
                        warning[OWN_ID - c.TEAM_START] = True

                # LEDs
                if mode != prev_mode or helper.get_element(
                        flags, 'master_set_LED'):
                    if helper.get_element(flags, 'master_set_LED'):
                        helper.set_element(flags, 'master_set_LED', False)
                    if mode == 'RUN':
                        pi2go.setAllLEDs(c.LED_OFF, c.LED_ON, c.LED_OFF)
                    elif mode == 'SLOW':
                        pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF,
                                         c.LED_ON)  #TODO: test
                        #pi2go.setAllLEDs(c.LED_OFF,c.LED_ON,c.LED_OFF)      #TODO: presentation
                    #elif mode == 'WARN':
                    #pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_OFF)
                    elif mode == 'STOP':
                        pi2go.setAllLEDs(c.LED_ON, c.LED_OFF, c.LED_OFF)
                # Blinking-Mode
                if mode == 'WARN':
                    if helper.check_time_limit(times, 'prev_set_LED',
                                               c.WAIT_LED):
                        if helper.get_element(flags, 'status_warn_LED'):
                            pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_OFF)
                            helper.set_element(flags, 'status_warn_LED', False)
                        else:
                            pi2go.setAllLEDs(c.LED_ON, c.LED_ON, c.LED_OFF)
                            helper.set_element(flags, 'status_warn_LED', True)

                # Calculate new speed
                if mode == 'RUN':
                    if prev_mode != 'RUN' or helper.get_element(
                            flags, 'master_set_speed'):
                        speed = SPEED_RUN
                        helper.set_element(flags, 'master_set_speed', False)
                        helper.set_element(flags, 'set_motor', True)

                # Blocking Avoidance
                elif mode == 'SLOW':
                    #linear
                    gradient = float(SPEED_CONTROL_MAX -
                                     SPEED_CONTROL_MIN) / float(c.DIST_MAX -
                                                                DIST_MIN)
                    error = c.DIST_MAX - distance
                    new_value = round(SPEED_RUN - error * gradient, 1)

                    if new_value < SPEED_CONTROL_MIN:
                        new_value = SPEED_CONTROL_MIN
                    elif new_value > SPEED_CONTROL_MAX:
                        new_value = SPEED_CONTROL_MAX

                    if new_value != speed:
                        speed = new_value
                        helper.set_element(flags, 'set_motor', True)

                # Slow-Down in Warning-Mode
                elif mode == 'WARN':
                    if prev_mode != 'WARN':
                        helper.set_element(times, 'get_warning', time.time())
                        speed_get_warning = speed
                    new_value = round(
                        speed_get_warning *
                        (1 - (time.time() - helper.get_element(
                            times, 'get_warning')) / c.TIME_TO_SLOW_DOWN), 1)

                    if new_value < SPEED_WARN:
                        new_value = SPEED_WARN

                    if new_value != speed:
                        speed = new_value
                        helper.set_element(flags, 'set_motor', True)

                elif mode == 'STOP':
                    if prev_mode != 'STOP':
                        speed = c.SPEED_STOP
                        helper.set_element(flags, 'set_motor', True)

                # Motor
                if helper.get_element(flags, 'set_motor'):
                    if speed > c.SPEED_LIMIT_MAX:
                        speed = c.SPEED_LIMIT_MAX
                    elif speed < c.SPEED_LIMIT_MIN:
                        speed = c.SPEED_LIMIT_MIN
                    if mode == 'SLOW' or mode == 'WARN':
                        if helper.check_time_limit(times, 'prev_set_motor',
                                                   c.WAIT_MOTOR):
                            pi2go.go(speed, speed)
                            helper.set_element(flags, 'set_motor', False)
                    else:
                        pi2go.go(speed, speed)
                        helper.set_element(flags, 'set_motor', False)

                # Send
                if mode != prev_mode:
                    if prev_mode == 'STOP':
                        com.send_x_broadcast_messages(c.PORT, "RELEASE",
                                                      c.SENDING_ATTEMPTS,
                                                      c.WAIT_SEND)
                    elif mode == 'STOP':
                        com.send_x_broadcast_messages(c.PORT, "PROBLEM",
                                                      c.SENDING_ATTEMPTS,
                                                      c.WAIT_SEND)

                # Next State
                prev_state = state
                if helper.get_element(flags, 'master_set_state'):
                    helper.set_element(flags, 'master_set_state', False)
                    state = next_state

                # Button
                if helper.check_time_limit(times, 'prev_get_switch',
                                           c.WAIT_SWITCH):
                    # Pressed = 1, Released = 0
                    button = pi2go.getSwitch()
                    if not button:
                        helper.set_element(flags, 'button_release', True)
                    if button and helper.get_element(flags, 'button_release'):
                        helper.set_element(flags, 'button_release', False)
                        prev_state = state
                        state = 'IDLE'
                        com.send_x_broadcast_messages(c.PORT, "RELEASE",
                                                      c.SENDING_ATTEMPTS,
                                                      c.WAIT_SEND)

    except KeyboardInterrupt:
        print 'KEYBOARD'

    finally:
        pi2go.stop()
        pi2go.cleanup()
        sock.close()
        print 'END'
Example #11
0
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
Example #12
0
def find(id_, x_, y_, init_dist_, quadrant_6, distance_6):

    centerx = 317
    centery = 404
    radius = 450  
    i_radius = 150


    if (x_ == 0 and y_ == 0) or (x_ == -1 and y_ == -1):    # GPS 
        print 'false' #if 0 or -1

    else:

        dx = abs(x_ - centerx)
        dy = abs(y_ - centery)


        if ((dx + dy) <= radius):
            print 'true'

            dist_ = math.sqrt((dx * dx) + (dy * dy))

            quadrant_ = dir(x_, y_)
            print 'quadrant_', quadrant_
            print 'quadrant_6', quadrant_6

            if ((dx + dy) <= i_radius):
                if distance_6 < 380: 
                    pi2go.stop()                
                    print 'stop1'
                    time.sleep(2)
                    
                if quadrant_6 == -2:
                    if quadrant_ == -1:
                        print 'ID %d found on right quadrant 0' %id_
                        if init_dist_ > distance_:
                            print 'near the center'
                            if distance_6 < 380:
                                print 'stop2'
                                pi2go.stop()
                                #print 'stop'
                                time.sleep(2)   # stop
                                
                            init_dist_ = distance_

                        elif (distance_ + 5 <= init_dist_ or init_dist_ >= distance_ - 5):
                            print 'ID %d has stopped' %id_
                            if distance_6 < 380:
                                pi2go.stop()
                                print 'stop3'
                                time.sleep(2)

                        else:
                            init_dist_ = distance_

                if quadrant_6 == -1:
                    if quadrant_ == 0:
                        print 'ID %d found on right quarant -1' % id_
                        if init_dist_ > distance_:
                            print 'arriving center'
                            if distance_6 < 380:
                                print 'stop4'
                                pi2go.stop()          # stop
                                time.sleep(2)
                            init_dist_ = distance_

                        elif (distance_ + 5 <= init_dist_ or init_dist_ >= distance_ - 5):
                            print 'ID %d has stopped' %id_
                            if distance_6 < 380:
                                pi2go.stop()
				          time.sleep(2)
                        else:
                      #      print 'moving away'
                            init_dist_ = distance_

                if quadrant_6 == 0:
                    if quadrant_ == 1:
                        print 'ID %d found on right quadrant 1' % id_

                        if init_dist_ > distance_:
                            print 'arriving center'
                            if distance_6 < 380:
                                print 'stop5'
                                pi2go.stop()       # stop
                                time.sleep(2)
                            prev_dist_ = dist_

                        elif (dist_ + 5 <= prev_dist_ or prev_dist_ >= dist_ - 5):
                            print 'ID %d has stopped moving' %id_
                            if dist_6 < 380:
                                pi2go.irAll()
			               pi2go.stop()#new
			 	          time.sleep(2)

                        else:
                            init_dist_ = distance_

                if quad_6 == 1:
                    if quad_ == -2:
                        print 'ID %d found on right quadrant -2' % id_

                        if init_dist_ > distance_:
                            print 'arriving center'
                            if distance_6 < 380:
                                print 'stop6'
                                pi2go.stop()   # stop
                                time.sleep(2)
                            init_dist_ = distance_

                        elif (distance_ + 5 <= init_dist_ or init_dist_ >= distance_ - 5):
                            print 'ID %d has stopped ' %id_
                            if distance_6 < 380:
                                pi2go.irAll()
			           	pi2go.stop()
				          time.sleep(2)
                        else:
Example #13
0
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
Example #14
0
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
    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))
Example #16
0
def start():
    state = 'INIT'
    prev_state = ''
    mode = 'STOP'
    prev_mode = ''

    try:
        while True:       
            if state == 'INIT':
                SPEED_RUN = c.SPEED_RUN
                SPEED_WARN = round(float(SPEED_RUN)/3,0)
                SPEED_CONTROL_MAX = SPEED_RUN
                SPEED_CONTROL_MIN = SPEED_WARN
                DIST_MIN = c.DIST_MIN
                speed = 0
                distance = 0
                warning = []
                last_meas_time = 0
                times = []
                times.append(['prev_get_dist',0.0])
                times.append(['prev_get_switch',0.0])
                times.append(['prev_set_motor',0.0])
                times.append(['get_warning',0.0])
                times.append(['prev_set_LED',0.0])    
                flags = []
                flags.append(['set_motor',False])
                flags.append(['status_warn_LED',False])
                flags.append(['button_release',False])
                flags.append(['master_set_speed',False])
                flags.append(['master_set_button',False])
                flags.append(['master_set_LED',False])
                flags.append(['master_set_state',False])
                pi2go.init()
                pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_ON)
                time.sleep(1)
                sock = com.init_nonblocking_receiver('',c.PORT)
                for x in range(c.TEAM_SIZE):
                    warning.append(True)
                OWN_IP = com.get_ip()
                OWN_ID = com.get_id_from_ip(OWN_IP)
                prev_state = state
                state = 'IDLE'
   
    
            if state == 'IDLE':
                if prev_state != 'IDLE':
                    pi2go.setAllLEDs(c.LED_OFF,c.LED_OFF,c.LED_ON)
                    pi2go.stop()
                if helper.check_time_limit(times,'prev_get_switch',c.WAIT_SWITCH):
                    # Pressed = 1, Released = 0
                    button = pi2go.getSwitch()
                    if not button:
                        helper.set_element(flags,'button_release',True)
                    if button and helper.get_element(flags,'button_release'):
                        helper.set_element(flags,'button_release',False)
                        prev_mode = ''
                        prev_state = state
                        state = 'RUNNING'

                # change to sensor-based or master_idle type        
                data = 'new_round'
                while data != '':
                    data, addr = com.receive_message(sock) 
                    if data != '':
                        sender_ID = com.get_id_from_ip(addr[0])
                        if sender_ID < c.TEAM_START or sender_ID > c.TEAM_END:
                            command, value = com.string_to_command(data)
                            #
                            print 'MASTER:' , sender_ID , ' : ' , data
                            try:
                                if command == c.COMMAND_SPEED:
                                    helper.set_element(flags,'master_set_speed',True)
                                    prev_SPEED_RUN = SPEED_RUN
                                    if value == '+':
                                        SPEED_RUN += 5
                                    elif value == '-':
                                        SPEED_RUN -= 5
                                    else:
                                        SPEED_RUN = value
                                    print 'Set SPEED_RUN from '+ str(prev_SPEED_RUN) + ' to ' + str(SPEED_RUN)           
                                elif command == c.COMMAND_DIST:
                                    prev_DIST_MIN = DIST_MIN
                                    if not value.isdigit():
                                        print "Something went terribly wrong with the protocol..."
                                        raise KeyboardInterrupt
                                    DIST_MIN = value
                                    print 'Set DIST_MIN from '+ str(prev_DIST_MIN) + ' to ' + str(DIST_MIN)  
                                elif command == c.COMMAND_BLINK:
                                    helper.blink('white')
                                    helper.set_element(flags, 'master_set_LED', True)
                                elif command == c.COMMAND_RESET:    
                                    SPEED_RUN = c.SPEED_RUN
                                    SPEED_WARN = round(float(SPEED_RUN)/3,0)
                                    SPEED_CONTROL_MAX = SPEED_RUN
                                    SPEED_CONTROL_MIN = SPEED_WARN
                                    DIST_MIN = c.DIST_MIN
                                    helper.set_element(times,'prev_get_dist',0)
                                    helper.set_element(flags,'master_set_LED', True)
                                    helper.set_element(flags,'master_set_speed', True)
                                    warning = [True] * len(warning)
                                    print "Reset major values"
                                elif command == c.COMMAND_STATE:
                                    local_prev_state = state
                                    if value == c.VALUE_STATE_RUNNING:
                                        state = 'RUNNING'
                                    elif value == c.VALUE_STATE_IDLE:
                                        state = 'IDLE'
                                    print 'Going from state ' + local_prev_state + ' to state ' + state
                                elif command == c.COMMAND_TYPE:
                                    if value == c.VALUE_TYPE_ORIGINAL:
                                        value = helper.determine_team(OWN_ID)
                                    return value
                            except:
                                print "Error interpreting message from master! Continuing anyway"


            elif state == 'RUNNING':
                # Distance         
                if helper.check_time_limit(times,'prev_get_dist',c.WAIT_DIST):
                    time_between = time.time() - last_meas_time
                    last_meas_time = time.time()                
                    new_dist = pi2go.getDistance()
                    if new_dist > 1:
                        distance = new_dist
                        #print 'dt:', time_between , distance
                
                # Obstacle = 1, No Obstacle = 0
                irCentre = pi2go.irCentre()
                
                # Obstacle Analysis
                if irCentre or (distance < DIST_MIN):
                    distance_level = 0
                elif distance > c.DIST_MAX:
                    distance_level = 2
                else:
                    distance_level = 1
    
                    
                # Receive
                data = 'new_round'
                while data != '':
                    data, addr = com.receive_message(sock) 
                    if data != '':
                        sender_ID = com.get_id_from_ip(addr[0])
                        if sender_ID == OWN_ID:
                            #print 'OWN: ' , sender_ID, ' : ' , data
                            continue
                        if sender_ID >= c.TEAM_START and sender_ID <= c.TEAM_END:
                            #print 'ROBOT: ', sender_ID, ' : ' , data
                            if data == 'PROBLEM':
                                warning[sender_ID-c.TEAM_START] = False
                            elif data == 'RELEASE':
                                warning[sender_ID-c.TEAM_START] = True
                        else:
                            try:
                            #print 'MASTER:' , sender_ID , ' : ' , data
                                command, value = com.string_to_command(data)
                                if command == c.COMMAND_SPEED:
                                    helper.set_element(flags,'master_set_speed',True)
                                    prev_SPEED_RUN = SPEED_RUN
                                    if value == '+':
                                        SPEED_RUN += 5
                                    elif value == '-':
                                        SPEED_RUN -= 5
                                    else:
                                        SPEED_RUN = value
                                    print 'MASTER: Set SPEED_RUN from '+ str(prev_SPEED_RUN) + ' to ' + str(SPEED_RUN)           
                                elif command == c.COMMAND_DIST:
                                    prev_DIST_MIN = DIST_MIN
                                    if not value.isdigit():
                                        print "Something went terribly wrong with the protocol..."
                                        raise KeyboardInterrupt
                                    DIST_MIN = value
                                    print 'MASTER: Set DIST_MIN from '+ str(prev_DIST_MIN) + ' to ' + str(DIST_MIN)  
                                elif command == c.COMMAND_BLINK:
                                    helper.blink('white')
                                    helper.set_element(flags, 'master_set_LED', True)
                                elif command == c.COMMAND_RESET:    
                                    SPEED_RUN = c.SPEED_RUN
                                    SPEED_WARN = round(float(SPEED_RUN)/3,0)
                                    SPEED_CONTROL_MAX = SPEED_RUN
                                    SPEED_CONTROL_MIN = SPEED_WARN
                                    DIST_MIN = c.DIST_MIN
                                    helper.set_element(times,'prev_get_dist',0)
                                    helper.set_element(flags,'master_set_LED', True)
                                    helper.set_element(flags,'master_set_speed', True)
                                    warning = [True] * len(warning)
                                    print 'MASTER: Reset major values'
                                elif command == c.COMMAND_STATE:
                                    helper.set_element(flags,'master_set_state', True)
                                    if value == c.VALUE_STATE_RUNNING:
                                        next_state = 'RUNNING'
                                    elif value == c.VALUE_STATE_IDLE:
                                        next_state = 'IDLE'
                                    print 'MASTER: Going from state ' + state + ' to state ' + next_state
                                #elif command == c.COMMAND_TYPE and value != c.VALUE_TYPE_COM:
                                elif command == c.COMMAND_TYPE:
                                    local_prev_value = value
                                    if value == c.VALUE_TYPE_ORIGINAL:
                                        value = helper.determine_team(OWN_ID)
                                    print "MASTER: Changing from type " + local_prev_value + " to type " + value
                                    return value
                            except:
                                print "Error interpreting message from master! Continuing anyway"

                                        
                # Analyse --> Calculate MODE
                if prev_state == 'RUNNING':                
                    prev_mode = mode                    
                if distance_level == 0:
                    mode = 'STOP'
                elif distance_level == 1 and all(warning):
                    mode = 'SLOW'
                elif distance_level == 2 and all(warning):
                    mode = 'RUN'
                elif distance_level != 0 and not all(warning):
                    mode = 'WARN'


                # Set own Warning-Flag 
                if mode != prev_mode:                          
                    if mode == 'STOP':
                        warning[OWN_ID-c.TEAM_START] = False
                    else:
                        warning[OWN_ID-c.TEAM_START] = True


                # LEDs  
                if mode != prev_mode or helper.get_element(flags,'master_set_LED'):
                    if helper.get_element(flags,'master_set_LED'):
                        helper.set_element(flags,'master_set_LED',False)
                    if mode == 'RUN':
                        pi2go.setAllLEDs(c.LED_OFF,c.LED_ON,c.LED_OFF)
                    elif mode == 'SLOW':
                        pi2go.setAllLEDs(c.LED_OFF,c.LED_OFF,c.LED_ON)      #TODO: test
                        #pi2go.setAllLEDs(c.LED_OFF,c.LED_ON,c.LED_OFF)      #TODO: presentation
                    #elif mode == 'WARN':
                        #pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_OFF)
                    elif mode == 'STOP':
                        pi2go.setAllLEDs(c.LED_ON,c.LED_OFF,c.LED_OFF)
                # Blinking-Mode
                if mode == 'WARN':
                    if helper.check_time_limit(times,'prev_set_LED',c.WAIT_LED):
                        if helper.get_element(flags,'status_warn_LED'):
                            pi2go.setAllLEDs(c.LED_OFF,c.LED_OFF,c.LED_OFF)
                            helper.set_element(flags,'status_warn_LED',False)
                        else:
                            pi2go.setAllLEDs(c.LED_ON,c.LED_ON,c.LED_OFF)
                            helper.set_element(flags,'status_warn_LED',True)
    
                        
                # Calculate new speed
                if mode == 'RUN':
                    if prev_mode != 'RUN' or helper.get_element(flags,'master_set_speed'):
                        speed = SPEED_RUN
                        helper.set_element(flags,'master_set_speed',False)
                        helper.set_element(flags,'set_motor',True)


                # Blocking Avoidance
                elif mode == 'SLOW':
                    #linear
                    gradient = float(SPEED_CONTROL_MAX - SPEED_CONTROL_MIN)/float(c.DIST_MAX-DIST_MIN)
                    error = c.DIST_MAX - distance
                    new_value = round(SPEED_RUN - error * gradient,1) 
                    
                    if new_value < SPEED_CONTROL_MIN:
                        new_value = SPEED_CONTROL_MIN
                    elif new_value > SPEED_CONTROL_MAX:
                        new_value = SPEED_CONTROL_MAX
                                     
                    if new_value != speed:
                        speed = new_value
                        helper.set_element(flags,'set_motor',True)
                
                
                # Slow-Down in Warning-Mode    
                elif mode == 'WARN':
                    if prev_mode != 'WARN':
                        helper.set_element(times,'get_warning',time.time())
                        speed_get_warning = speed
                    new_value = round(speed_get_warning * (1-(time.time()-helper.get_element(times,'get_warning'))/c.TIME_TO_SLOW_DOWN),1)
                    
                    if new_value < SPEED_WARN:
                        new_value = SPEED_WARN
                    
                    if new_value != speed:
                        speed = new_value
                        helper.set_element(flags,'set_motor',True)
             
                elif mode == 'STOP':
                    if prev_mode != 'STOP':
                        speed = c.SPEED_STOP
                        helper.set_element(flags,'set_motor',True)

                
                # Motor
                if helper.get_element(flags,'set_motor'):
                    if speed > c.SPEED_LIMIT_MAX:
                        speed = c.SPEED_LIMIT_MAX
                    elif speed < c.SPEED_LIMIT_MIN:
                        speed = c.SPEED_LIMIT_MIN  
                    if mode == 'SLOW' or mode == 'WARN':
                        if helper.check_time_limit(times,'prev_set_motor',c.WAIT_MOTOR):
                            pi2go.go(speed,speed)
                            helper.set_element(flags,'set_motor',False)
                    else:
                        pi2go.go(speed,speed)
                        helper.set_element(flags,'set_motor',False)

                    
                # Send
                if mode != prev_mode:                          
                    if prev_mode == 'STOP':
                        com.send_x_broadcast_messages(c.PORT, "RELEASE", c.SENDING_ATTEMPTS, c.WAIT_SEND)
                    elif mode == 'STOP':
                        com.send_x_broadcast_messages(c.PORT, "PROBLEM", c.SENDING_ATTEMPTS, c.WAIT_SEND)

                # Next State                
                prev_state = state
                if helper.get_element(flags,'master_set_state'):
                    helper.set_element(flags,'master_set_state',False)
                    state = next_state
                
                # Button
                if helper.check_time_limit(times,'prev_get_switch',c.WAIT_SWITCH):
                    # Pressed = 1, Released = 0
                    button = pi2go.getSwitch()
                    if not button:
                        helper.set_element(flags,'button_release',True)
                    if button and helper.get_element(flags,'button_release'):
                        helper.set_element(flags,'button_release',False)
                        prev_state = state
                        state = 'IDLE'
                        com.send_x_broadcast_messages(c.PORT, "RELEASE", c.SENDING_ATTEMPTS, c.WAIT_SEND)

    
                            
    except KeyboardInterrupt:
        print 'KEYBOARD'
    
    finally:
        pi2go.stop()
        pi2go.cleanup()
        sock.close()
        print 'END'
Example #17
0
        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

except KeyboardInterrupt:
    pass

finally:
    pi2go.cleanup()
Example #18
0
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()
Example #19
0
            pi2go.stopServos()
            print 'Servos stopped'

        elif keyp == 't':
            degree = degree-15
            pi2go.setServo(0, degree)
            print 'Servo 0', degree
        elif keyp == 'y':
            degree = degree+15
            pi2go.setServo(0, degree)
            print 'Servo 0', degree

        elif keyp == 'u':
            degree = degree-15
            pi2go.setServo(0, degree)
            print 'Servo 1', degree
        elif keyp == 'j':
            degree = degree-15
            pi2go.setServo(0, degree)
            print 'Servo 1', degree

        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()
Example #20
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()