コード例 #1
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()
コード例 #2
0
ファイル: go4.py プロジェクト: s9795312/1234567890
                    set_element(flags, 'button_release', True)
                if button and get_element(flags, 'button_release'):
                    set_element(flags, 'button_release', False)
                    prev_state = state
                    state = 'RUNNING'

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

            # 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])
コード例 #3
0
ファイル: ygo.py プロジェクト: s9795312/1234567890
            message, addr = communication.receive_message(sock)
            print OWN_ID
            if message != '':
                # print message
                pass
            # if  "MASTER_START" in message:
            if "START" in message:
                state = "NOT_IDLE"
        if state == "NOT_IDLE":
            # #### SENSOR
            if time.time() - last_dist_check > 0.2:
                last_dist_check = time.time()
                distance = pi2go.getDistance()

                # print state_list
            irCentre = pi2go.irCentre()

            if irCentre or distance < MIN_DIST:
                state_list[OWN_ID] = OBSTACLE
                # print "After setting it: own state: " + str(state_list[OWN_ID])
                # print "irCentre: " + str(irCentre)
            else:
                state_list[OWN_ID] = CLEAR
            # print "P : ", prev_state_list[OWN_ID]
            # print "C : ", state_list[OWN_ID]
            if prev_state_list[OWN_ID] != state_list[OWN_ID]:
                print "Sending state : ", str(state_list[OWN_ID])
                for x in range(10):
                    communication.send_broadcast_message(PORT, str(state_list[OWN_ID]))

            if distance < SLOW_DIST:
コード例 #4
0
ファイル: pi2godemo.py プロジェクト: ZacharyIgielman/pi2go
#!/usr/bin/python
#
# Pi2Go Demo Code using the Pi2Go library
#
# Created by Gareth Davies, May 2014
# Copyright 4tronix
#
# This code is in the public domain and may be freely copied and used
# No warranty is provided or implied
#
#======================================================================

import pi2go, time

pi2go.init()

pi2go.setAllLEDs(0, 0, 4095)

light = pi2go.getLight(0)
print light

middle = pi2go.irCentre()
print middle

distance = pi2go.getDistance()
print distance

time.sleep (3)

pi2go.cleanup()
コード例 #5
0
ファイル: auto_bot.py プロジェクト: s9795312/1234567890
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'
コード例 #6
0
ファイル: auto_bot.py プロジェクト: s9795312/1234567890
                else: 
                    state = 'IDLE'
                print state

        elif state == 'RUNNING':
            # Distance 
            
            if time.time() - prev_measurement_time > WAIT_DIST:
                prev_measurement_time = time.time()                
                distance = pi2go.getDistance()
                
                print "US sensor time: ", time.time() - prev_measurement_time, " with distance: ", distance
                
                
            # Obstacle = 1, No Obstacle = 0
            irCentre = pi2go.irCentre() # TODO: edit pi2go library and uncomment!
            # Obstacle Analysis
            if irCentre or (distance < DIST_MIN):
                distance_area = 0
            elif distance > DIST_MAX:
                distance_area = 2
            else:
                distance_area = 1
                    
            # Analyse --> Calculate mode
            prev_mode = mode
            if distance_area == 0:
                mode = 'STOP'
            elif distance_area == 1:
                mode = 'SLOW'
            elif distance_area == 2:
コード例 #7
0
    # keyp = l for learn mode
    # keyp = o for optimum mode
    keyp = readkey()

    #
    #
    # Process learning maze mode
    while keyp == 'l':

        # Defining the sensors on the bottom of the Pi2Go
        left = pi2go.irLeftLine()
        right = pi2go.irRightLine()

        # If an dead end is detected, turn around 180 degrees and go back to parent:
        if pi2go.irCentre():
            currentNode = currentNode.insertTurn('dead end', '')
            currentNode = currentNode.gotoParent()
            tickStart = time.time()
            # Code required to find the line again during a 180 degree turn
            while pi2go.irRightLine() == True:
                pi2go.go(deadEndSpeedForward, deadEndSpeedReverse)

        # If both line sensors do not detect the line, and there are no obstacles, move forward:
        elif left == True and right == True and not pi2go.irCentre():
            pi2go.forward(speed)

        # If both sensors (i.e T junction, crossroads or end of maze) detect a line spin left:
        elif left == False and right == False:
            pi2go.go(speedReverse, speedForward)
            # Extra test to distinguish between a junction and minor direction corrections
コード例 #8
0
ファイル: IRTest.py プロジェクト: lesp/Pi2Go_Lite_demo
#!/usr/bin/env python
#Simply prints the state of the obstacle sensors

# Must be run as root - sudo python .py 

import time, pi2go

pi2go.init()

vsn = pi2go.version()
if vsn == 1:
  print "Running on Pi2Go"
else:
  print "Running on Pi2Go-Lite"


try:
    while True:
        print 'Left:', pi2go.irLeft()
        if vsn == 1:
            print 'Centre:', pi2go.irCentre()
        print 'Right:', pi2go.irRight()
        print
        time.sleep(1)
                          
except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
コード例 #9
0
vsn = pi2go.version()
if vsn == 1:
  print "Running on Pi2Go"
else:
  print "Running on Pi2Go-Lite"


try:
    while True:
	print 'obstacle'
	print 'irLeft', GPIO.input(11)
	print pi2go.irLeft()
	print 'irRight', GPIO.input(7)
	print pi2go.irRight()
	print 'irCentre', GPIO.input(13)
	print pi2go.irCentre()
	print 'line'
	print 'LeftLine', GPIO.input(12)
	print pi2go.irLeftLine()
	print 'RightLine', GPIO.input(15)
	print pi2go.irRightLine()
        #print 'Left:', pi2go.irLeftLine()
        #print 'Right:', pi2go.irRightLine()
	print
        time.sleep(5)
                          
except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
コード例 #10
0
ファイル: IRTest.py プロジェクト: s9795312/1234567890
#!/usr/bin/env python
#Simply prints the state of the obstacle sensors

# Must be run as root - sudo python .py

import time, pi2go

pi2go.init()

vsn = pi2go.version()
if vsn == 1:
    print "Running on Pi2Go"
else:
    print "Running on Pi2Go-Lite"

try:
    while True:
        print 'Left:', pi2go.irLeft()
        if vsn == 1:
            print 'Centre:', pi2go.irCentre()
        print 'Right:', pi2go.irRight()
        print
        time.sleep(1)

except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
コード例 #11
0
  
  # keyp = l for learn mode
  # keyp = o for optimum mode
  keyp = readkey()
  
  #
  #
  # Process learning maze mode
  while keyp=='l':

    # Defining the sensors on the bottom of the Pi2Go
    left = pi2go.irLeftLine()
    right = pi2go.irRightLine()
    
    # If an dead end is detected, turn around 180 degrees and go back to parent: 
    if pi2go.irCentre():
      currentNode = currentNode.insertTurn('dead end','')    
      currentNode = currentNode.gotoParent()
      tickStart = time.time()
      # Code required to find the line again during a 180 degree turn
      while pi2go.irRightLine() == True:
        pi2go.go(deadEndSpeedForward,deadEndSpeedReverse)

    # If both line sensors do not detect the line, and there are no obstacles, move forward: 
    elif left == True and right == True and not pi2go.irCentre(): 
      pi2go.forward(speed)
      
    # If both sensors (i.e T junction, crossroads or end of maze) detect a line spin left:  
    elif left == False and right == False:
       pi2go.go(speedReverse,speedForward)   
       # Extra test to distinguish between a junction and minor direction corrections
コード例 #12
0
ファイル: auto_bot.py プロジェクト: s9795312/1234567890
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'
コード例 #13
0
ファイル: auto_bot.py プロジェクト: s9795312/1234567890
                else:
                    state = 'IDLE'
                print state

        elif state == 'RUNNING':
            # Distance

            if time.time() - prev_measurement_time > WAIT_DIST:
                prev_measurement_time = time.time()
                distance = pi2go.getDistance()

                print "US sensor time: ", time.time(
                ) - prev_measurement_time, " with distance: ", distance

            # Obstacle = 1, No Obstacle = 0
            irCentre = pi2go.irCentre(
            )  # TODO: edit pi2go library and uncomment!
            # Obstacle Analysis
            if irCentre or (distance < DIST_MIN):
                distance_area = 0
            elif distance > DIST_MAX:
                distance_area = 2
            else:
                distance_area = 1

            # Analyse --> Calculate mode
            prev_mode = mode
            if distance_area == 0:
                mode = 'STOP'
            elif distance_area == 1:
                mode = 'SLOW'
            elif distance_area == 2: