SPEED_CONTROL_MIN = c.SPEED_STOP DIST_MIN = c.DIST_MIN # Programm try: while True: if state == 'INIT': 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) #print 'ID:' , OWN_ID prev_state = state #state = 'IDLE' state = 'RUNNING' if state == 'IDLE': if prev_state != 'IDLE': pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_OFF) pi2go.stop() if check_time_limit(times, 'prev_get_switch', c.WAIT_SWITCH): # Pressed = 1, Released = 0 button = pi2go.getSwitch() if not button: set_element(flags, 'button_release', True) if button and get_element(flags, 'button_release'):
import communication as com import com_bot import auto_bot import constants as c import helper helper.blink() # sock = com.init_receiver('', c.PORT) OWN_IP = com.get_ip() OWN_ID = com.get_id_from_ip(OWN_IP) value = helper.determine_team(OWN_ID) sock = com.init_nonblocking_receiver('', c.PORT) try: # data, addr = com.receive_message(sock) # command, value = com.string_to_command(data) # com.close_socket(sock) while True: if c.VALUE_TYPE_COM == value: status = "I'm in com_mode now!" print status # communication.send_broadcast_message(c.PORT, status) com.close_socket(sock) value = com_bot.start() sock = com.init_nonblocking_receiver('', c.PORT) status = "Exiting com_mode" com.send_x_broadcast_messages(c.PORT, "RELEASE", c.SENDING_ATTEMPTS, c.WAIT_SEND) print status # communication.send_broadcast_message(c.PORT, status)
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'
['button_release', False], ['master_set_speed', False], ['master_set_button', False], ['master_set_LED', False], ['master_set_state', False], ['robot_type_com', False], ['master_set_type', False]] try: while True: if state == 'INIT': pi2go.init() sock = com.init_nonblocking_receiver('', c.PORT) OWN_IP = com.get_ip() OWN_ID = com.get_id_from_ip(OWN_IP) value = helper.determine_team(OWN_ID) if helper.determine_team(OWN_ID) == c.VALUE_TYPE_COM: print "I'm in com_mode now!" helper.set_element(flags, 'robot_type_com', True) elif helper.determine_team(OWN_ID) == c.VALUE_TYPE_AUTO: print "I'm in auto_mode now!" helper.set_element(flags, 'robot_type_com', False) local_prev_value = helper.determine_team(OWN_ID) # TODO: IDLE-STATE? prev_state = state state = 'IDLE' if state == 'IDLE': if prev_state != 'IDLE' or helper.get_element(flags, 'master_set_type'):
prev_messurement_time = 0 message_buffer_slave = [] message_buffer_master = [] # Programm try: while True: if STATE == 'INIT': pi2go.init() sock = communication.init_nonblocking_receiver('', PORT) for x in range(SQUAD_SIZE): SQUAD.append(True) message_buffer_slave.append(True) OWN_IP = communication.get_ip() OWN_ID = communication.get_id_from_ip(OWN_IP) print OWN_ID STATE = 'RUNNING' elif STATE == 'RUNNING': # distance if time.time() - prev_messurement_time > WAIT_DIST: prev_messurement_time = time.time() distance = pi2go.getdistance() # Obstacle = 1, No Obstacle = 0 irCentre = pi2go.irCentre() # Obstacle Analysis if irCentre or (distance < DIST_MIN): distance_level = 0
PORT = 5005 BROADCAST_IP = '192.168.178.255' # Program try: while True: if STATE == 'INIT': pi2go.init() sock = communication.init_nonblocking_receiver('', PORT) for x in range(SQUAD_SIZE): SQUAD.append(True) message_buffer_slave.append(True) OWN_IP = communication.get_ip() print OWN_IP OWN_ID = communication.get_id_from_ip(OWN_IP) print OWN_ID STATE = 'RUNNING' print DIST_MIN elif STATE == 'RUNNING': # Distance if time.time() - prev_measurement_time > WAIT_DIST: prev_measurement_time = time.time() distance = pi2go.getDistance() # Obstacle = 1, No Obstacle = 0 # irCentre = pi2go.irCentre() # TODO: edit pi2go library and uncomment! irCentre = False # Obstacle Analysis if irCentre or (distance < DIST_MIN):
message_buffer_slave = [] message_buffer_master = [] # Programm try: while True: if STATE == "INIT": pi2go.init() sock = communication.init_nonblocking_receiver("", PORT) for x in range(SQUAD_SIZE): SQUAD.append(True) message_buffer_slave.append(True) OWN_IP = communication.get_ip() OWN_ID = communication.get_id_from_ip(OWN_IP) print OWN_ID STATE = "RUNNING" elif STATE == "RUNNING": # distance if time.time() - prev_messurement_time > WAIT_DIST: prev_messurement_time = time.time() distance = pi2go.getdistance() # Obstacle = 1, No Obstacle = 0 irCentre = pi2go.irCentre() # Obstacle Analysis if irCentre or (distance < DIST_MIN): distance_level = 0
distance_level = 0 elif distance > 40: distance_level = 2 else: distance_level = 1 # Receive data = 'new_round' while data != '': data, addr = communication.receive_message(sock) # ############### update list if ID is in SQUAD, # ############### write other ID's in different list # ############### repeat until buffer is empty if data != '': IP = addr[0] ID = communication.get_id_from_ip(addr) if ID == OWN_ID: print 'OWN: ', ID, ' : ', data continue if 100 <= ID <= 100 + NUMBER_OF_ROBOTS: print 'ROBOT: ', ID, ' : ', data if data == 'PROBLEM': curr_STATUS = False SQUAD[ID-100] = curr_STATUS elif data == 'RELEASE': curr_STATUS = True SQUAD[ID-100] = curr_STATUS else: print 'Master:', ID, ' : ', data # Analyse --> Calculate MODE
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'
distance = 0 squad = [] prev_measurement_time = 0 # Programm try: while True: if state == 'INIT': pi2go.init() sock = communication.init_nonblocking_receiver('',PORT) for x in range(SQUAD_SIZE): squad.append(True) OWN_IP = communication.get_ip() OWN_IDENTIFIER = communication.get_id_from_ip(OWN_IP) print OWN_IDENTIFIER state = 'RUNNING' elif state == 'RUNNING': # distance if time.time() - prev_measurement_time > WAIT_DIST: prev_measurement_time = time.time() distance = pi2go.getDistance() # Obstacle = 1, No Obstacle = 0 irCentre = pi2go.irCentre() #irCentre = False # Obstacle Analysis if irCentre or (distance < DIST_MIN):