movingSpeed = 38

# Initialize pi2go
pi2go.init()

# Robot loop
try:
    while True:
        
        # -----------------------------------------------
        # Movement modes
        # -----------------------------------------------
        
        # Go forward
        if (goForward):
            pi2go.go(movingSpeed, movingSpeed)
            
        # Turn right
        if (turnRight):
            # Turn right until right sensor doesn't recognize the line anymore 
            pi2go.go(30, -30)
            if (pi2go.irRightLine()):
                goForward = True
                turnLeft = False
                turnRight = False        
                turnAround = False
        # Turn left
        if (turnLeft):
            # Turn left until left sensor doesn't recognize the line anymore 
            pi2go.go(-30, 30)
            if (pi2go.irLeftLine()):
def right():
    pi2go.go(60,40)
    time.sleep(1.1)#1.45
    pi2go.go(35,60)
    time.sleep(1) 
Beispiel #3
0
            elif mode == 'STOP':
                if prev_mode != 'STOP':
                    speed = c.SPEED_STOP
                    set_element(flags, 'set_motor', True)

            #print 'mode: ', mode , 'speed: ', speed , 'dist: ' , distance

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

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

            prev_state = state

            # Button
Beispiel #4
0
                    pi2go.setAllLEDs(LED_ON,LED_ON,LED_OFF)
                elif mode == 'STOP':
                    # set LEDs to red
                    pi2go.setAllLEDs(LED_ON,LED_OFF,LED_OFF)
            
            # Motor
            if mode != prev_mode:                          
                if mode == 'RUN':
                    speed = SPEED_RUN
                elif mode == 'SLOW':
                    speed = SPEED_SLOW
                elif mode == 'WARN':
                    speed = SPEED_WARN
                elif mode == 'STOP':
                    speed = SPEED_STOP
                pi2go.go(speed,speed)

            # Switch
            if time.time() - prev_measurement_time_switch > WAIT_SWITCH:
                prev_measurement_time_switch = time.time()  
                button = pi2go.getSwitch()
                if button:
                    state = 'IDLE'
                    prev_measurement_time_switch += SWITCH_DEAD_TIME
                    pi2go.stop()
                else: 
                    state = 'RUNNING'
       
        else:
            print 'impossible state'
            state == 'IDLE'
def left():
    pi2go.go(40,60)
    time.sleep(2.45)#1.92
    pi2go.go(60,35)
    time.sleep(1)
Beispiel #6
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'
Beispiel #7
0
	def mueve(self, velDer,velIzq):
		pi2go.go(velIzq,velDer)
Beispiel #8
0
            # Motor
            if MODE != prev_MODE:                          
                if MODE == 'RUN':
                    SPEED = SPEED_RUN
                elif MODE == 'SLOW':
                    SPEED = SPEED_SLOW
                elif MODE == 'WARN':
                    SPEED = SPEED_WARN
                elif MODE == 'STOP':
                    SPEED = SPEED_STOP 
                # Speedlimits
                if SPEED > 100:
                    SPEED = 100
                elif SPEED < 0:
                    SPEED = 0
                pi2go.go(SPEED, SPEED)
                
#            # Send
#            if MODE != prev_MODE:                          
#                if prev_MODE == 'STOP':
#                    #print 'RELEASE'
#                    message = 'RELEASE'
#                    for x in range(PUSH):
#                        communication.send_broadcast_message(PORT, message)
#                        time.sleep(WAIT_SEND)                    
#                elif MODE == 'STOP':
#                    #print 'PROBLEM'
#                    message = 'PROBLEM'
#                    for x in range(PUSH):
#                        communication.send_broadcast_message(PORT, message)
#                        time.sleep(WAIT_SEND)
Beispiel #9
0
    #
    # 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
            if time.time() > tickStart + tickCount:
                ctrSpinGo = ctrSpinGo + 1
                if ctrSpinLeft > ctrSpinRight:
                    turn = 'Left'
                elif ctrSpinLeft < ctrSpinRight:
                    turn = 'Straight'
Beispiel #10
0
import pi2go, time

# Reading single character by forcing stdin to raw mode
import sys
import tty
import termios
import threading

# main loop
speed = 30
pi2go.init()

try:
    while True:
        if (pi2go.irLeftLine() == True and pi2go.irRightLine() == True):
            pi2go.go(speed * 0.94, speed)
        elif (pi2go.irLeftLine() == False):
            pi2go.spinLeft(speed)
        elif (pi2go.irRightLine() == False):
            pi2go.spinRight(speed)
        elif (pi2go.irLeftLine() == False and pi2go.irRightLine() == False):
            pi2go.stop()
        #print(pi2go.irLeftLine())
        #print(pi2go.irRightLine())
        #print('#######')
        #time.sleep(3)

finally:
    pi2go.cleanup()
Beispiel #11
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()
Beispiel #12
0
            # Motor
            if mode != prev_mode:
                if mode == 'RUN':
                    SPEED = SPEED_RUN
                elif mode == 'SLOW':
                    SPEED = SPEED_SLOW
                elif mode == 'WARN':
                    SPEED = SPEED_WARN
                elif mode == 'STOP':
                    SPEED = SPEED_STOP
                # Speedlimits
                if SPEED > 100:
                    SPEED = 100
                elif SPEED < 0:
                    SPEED = 0
                pi2go.go(SPEED, SPEED)

            # Send
            if mode != prev_mode:
                if prev_mode == 'STOP':
                    #print 'RELEASE'
                    message = 'RELEASE'
                    for x in range(SENDING_ATTEMPTS):
                        communication.send_broadcast_message(PORT, message)
                        time.sleep(WAIT_SEND)
                elif mode == 'STOP':
                    #print 'PROBLEM'
                    message = 'PROBLEM'
                    for x in range(PUSH):
                        communication.send_broadcast_message(PORT, message)
                        time.sleep(WAIT_SEND)
    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))
Beispiel #14
0
                calibrateLeft()

            else:
                leftSpeed = baseSpeed
                rightSpeed = baseSpeed

                if (not leftLine and rightLine):
                    lastTouchLeft = millis()
                    rightSpeed = -baseSpeed
                elif (leftLine and not rightLine):
                    lastTouchRight = millis()
                    leftSpeed = -baseSpeed
                elif (not leftLine and not rightLine):
                    if (millis() - lastTouchLeft > reassureTime):
                        calibrateLeft()
                        lastTouchLeft = millis()
                        lastTouchRight = millis()
                    elif (millis() - lastTouchRight > reassureTime):
                        calibrateRight()
                        lastTouchRight = millis()
                        lastTouchLeft = millis()

#             print "IR sensors: ", leftLine, rightLine
            pi2go.go(leftSpeed, rightSpeed)

except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
  #
  # 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
       if time.time() > tickStart + tickCount:
        ctrSpinGo = ctrSpinGo + 1
        if ctrSpinLeft > ctrSpinRight:
          turn = 'Left'
        elif ctrSpinLeft < ctrSpinRight:
          turn = 'Straight'
Beispiel #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'