def blink(color='white', sleeptime=0.1): if color == 'white': red = c.LED_ON green = c.LED_ON blue = c.LED_ON elif color == 'red': red = c.LED_ON green = c.LED_OFF blue = c.LED_OFF elif color == 'yellow': red = c.LED_ON green = c.LED_ON blue = c.LED_OFF elif color == 'blue': red = c.LED_OFF green = c.LED_OFF blue = c.LED_ON elif color == 'green': red = c.LED_OFF green = c.LED_ON blue = c.LED_OFF else: red = c.LED_ON green = c.LED_ON blue = c.LED_ON pi2go.setAllLEDs(red, green, blue) time.sleep(sleeptime) pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_OFF) time.sleep(sleeptime) pi2go.setAllLEDs(red, green, blue) time.sleep(sleeptime) pi2go.setAllLEDs(c.LED_OFF, c.LED_OFF, c.LED_OFF)
def calibrateRight(): print "Calibrating right..." pi2go.setAllLEDs(0, 400, 0) pi2go.spinRight(20) leftLine = not pi2go.irLeftLine() rightLine = not pi2go.irRightLine() while (not leftLine and not rightLine): leftLine = not pi2go.irLeftLine() rightLine = not pi2go.irRightLine() if (rightLine): while (rightLine): leftLine = not pi2go.irLeftLine() rightLine = not pi2go.irRightLine() while (not leftLine): leftLine = not pi2go.irLeftLine() rightLine = not pi2go.irRightLine() pi2go.spinLeft(20) while (leftLine): leftLine = not pi2go.irLeftLine() else: pi2go.spinLeft(20) while (not rightLine): leftLine = not pi2go.irLeftLine() rightLine = not pi2go.irRightLine() pi2go.spinRight(20) while (rightLine): leftLine = not pi2go.irLeftLine() rightLine = not pi2go.irRightLine() pi2go.stop() pi2go.setAllLEDs(0, 0, 0)
def distance(): stime = time.time() while True: # Defining the sensors left = pi2go.irLeftLine() right = pi2go.irRightLine() ntime = time.time() # timecheck for distance if ntime > (stime + 0.1): #print "%.5f" %time.time() dist = (int(pi2go.getDistance() * 10)) / 10.0 #print "%.5f" %time.time() stime = ntime # distance groups if dist < 10: pi2go.setAllLEDs(2000, 0, 0) elif dist > 20: pi2go.setAllLEDs(0, 0, 2000) else: pi2go.setAllLEDs(0, 2000, 0)
def distance(): stime = time.time() while True: # Defining the sensors left = pi2go.irLeftLine() right = pi2go.irRightLine() ntime = time.time() # timecheck for distance if ntime > (stime + 0.1): #print "%.5f" %time.time() dist = (int(pi2go.getDistance()*10))/10.0 #print "%.5f" %time.time() stime = ntime # distance groups if dist < 10: pi2go.setAllLEDs(2000, 0, 0) elif dist > 20: pi2go.setAllLEDs(0, 0, 2000) else: pi2go.setAllLEDs(0, 2000, 0)
# irNav.py # navigate using ir obstacle detectors with pi2go library # Author : Zachary Igielman import time import pi2go pi2go.init() fast=50 slow=30 while True: if pi2go.irAll()==False: pi2go.forward(fast) pi2go.setAllLEDs(4095, 4095, 4095) else: ir=pi2go.irRight() pi2go.setAllLEDs(4095, 0, 0) pi2go.reverse(slow) time.sleep(0.5) if ir: pi2go.setAllLEDs(0, 0, 4095) pi2go.turnReverse(fast,slow) time.sleep(3) else: pi2go.setAllLEDs(0, 4095, 0) pi2go.turnReverse(slow,fast) time.sleep(3) pi2go.cleanup()
# Parameters SPEED_RUN = c.SPEED_RUN SPEED_MIN = 20 SPEED_WARN = c.SPEED_WARN SPEED_CONTROL_MAX = c.SPEED_CONTROL_MAX 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()
def update_state_list(): for msg, ID in message_list: if ID == OWN_ID: continue # print OWN_ID, " : ", message, " from ", ID if msg == "False": msg = False elif msg == "True" or msg == "SLOW": msg = True state_list[ID] = msg # return state_list while True: if state == "IDLE": pi2go.setAllLEDs(LED_OFF, LED_OFF, LED_ON) 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
#!/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()
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'): helper.set_element(flags, 'master_set_type', False) if helper.get_element(flags, 'robot_type_com'): pi2go.setAllLEDs(c.LED_ON, c.LED_OFF, c.LED_ON) else: pi2go.setAllLEDs(c.LED_OFF, c.LED_ON, 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' helper.set_element(flags, 'master_set_LED', True) helper.set_element(flags, 'set_motor', True)
if dist < DIST_STOP: state = STATE_STOP state_string = "STOP" elif dist < DIST_RUN or ("WARN" in data and not OWN_IP in addr): state = STATE_WARN state_string = "WARN" else: state = STATE_RUN state_string = "RUN" if state != prev_state: print state_string if state == STATE_RUN: pi2go.setAllLEDs(LEDoff,LEDon,LEDoff) elif state == STATE_WARN: pi2go.setAllLEDs(LEDon,LEDon,LEDoff) elif state == STATE_STOP: pi2go.setAllLEDs(LEDon,LEDoff, LEDoff) else: print "unknown state!" #print time.time() #message = "State: " + state + " at " + str(time.time()) print "Starting broadcast" for x in range(0,10): message = " State: " + state_string + " at " + str(time.time()) + " #" + str(x) communication.send_broadcast_message(38234, message) #print time.time() print "Broadcast finished" prev_state = state
# Parameters SPEED_RUN = c.SPEED_RUN SPEED_MIN = 20 SPEED_WARN = c.SPEED_WARN SPEED_CONTROL_MAX = c.SPEED_CONTROL_MAX 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)
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'
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'
import pi2go, time pi2go.init() vsn = pi2go.version() try: if vsn != 1: print "This program only runs on the full Pi2Go" else: while True: pi2go.setAllLEDs(0, 0, 0) # start with all OFF for i in range(4): pi2go.setLED(i, 4095, 0, 0) # set to Red print 'Red' time.sleep(0.2) pi2go.setLED(i, 0, 0, 0) for i in range(4): pi2go.setLED(i, 0, 4095, 0) # set to Green print 'Green' time.sleep(0.2) pi2go.setLED(i, 0, 0, 0) for i in range(4): pi2go.setLED(i, 0, 0, 4095) # set to Blue print 'Blue' time.sleep(0.2) pi2go.setLED(i, 0, 0, 0) for i in range(4): pi2go.setLED(i, 4095, 4095, 4095) # set to White print 'White' time.sleep(0.2) pi2go.setLED(i, 0, 0, 0)
def line_follower(): """ State: L R 1 1 - Both White - Depends on P 1 0 - Left White - Turn Left 0 1 - Right White - Turn Right 0 0 - Both Black - Go Forward P - previous State ------------------ 0 - Left 1 - Right :return: """ dist = 0 speed = 70 change = 20 start = 0 STATE = 00 prev_STATE = 11 STOP = False stop = False prev_stop = True while True: #print "line follower %f" %time.time() # print 'get dist: %f' % time.time() left = pi2go.irLeftLine() right = pi2go.irRightLine() # print 'get ir: %f' % time.time() if not left and not right: # If both sensors are the on --> forward STATE = 00 elif left and not right: # If the left sensor is Off --> move right STATE = 10 elif right and not left: # If the right sensor is off --> move left STATE = 01 else: STATE = 11 if time.time() - start > 0.15: dist = (int(pi2go.getDistance()*10))/10.0 print dist while dist < 25: dist = (int(pi2go.getDistance()*10))/10.0 pi2go.stop() STATE = 69 time.sleep(0.15) if stop == False: pi2go.setAllLEDs(4095,0,0) stop = True start = time.time() stop = False if stop == prev_stop: pass elif stop == False: pi2go.setAllLEDs(0,4095,0) if STATE == prev_STATE: pass elif STATE == 00: pi2go.forward(speed) elif STATE == 10: pi2go.turnForward(speed + change, speed - change) elif STATE == 01: pi2go.turnForward(speed - change, speed + change) elif STATE == 11: pi2go.stop() prev_stop = stop prev_STATE = STATE
while True: # Defining the sensors left = pi2go.irLeftLine() right = pi2go.irRightLine() ntime = time.time() # timecheck for distance if ntime > (stime + 0.1): #print "%.5f" %time.time() dist = (int(pi2go.getDistance() * 10)) / 10.0 #print "%.5f" %time.time() stime = ntime # distance groups if dist < 10: pi2go.setAllLEDs(2000, 0, 0) elif dist > 20: pi2go.setAllLEDs(0, 0, 2000) else: pi2go.setAllLEDs(0, 2000, 0) # line follower if left == right: # If both sensors are the same (either on or off) --> forward pi2go.forward(speed) elif left == True: # If the left sensor is on --> move right pi2go.turnForward(speed + change, speed - change) elif right == True: #If the right sensor is on --> move left pi2go.turnForward(speed - change, speed + change) finally: # Even if there was an error, cleanup pi2go.cleanup()
# # 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() vsn = pi2go.version() try: if vsn != 1: print "This program only runs on the full Pi2Go" else: pi2go.setAllLEDs(0, 0, 0) while True: light0 = pi2go.getLight(0) light1 = pi2go.getLight(1) light2 = pi2go.getLight(2) light3 = pi2go.getLight(3) print "Light sensors: ", light0, light1, light2, light3 time.sleep(1) except KeyboardInterrupt: print finally: pi2go.cleanup()
# No warranty is provided or implied # #====================================================================== import pi2go, time LEDon = 4095 LEDoff = 0 pi2go.init() vsn = pi2go.version() if vsn != 1: print "This program only runs on the full Pi2Go" else: pi2go.setAllLEDs(LEDoff, LEDoff, LEDoff) pi2go.setAllLEDs(LEDon, LEDoff, LEDoff) print "Red" time.sleep(1) pi2go.setAllLEDs(LEDoff, LEDon, LEDoff) print "Green" time.sleep(1) pi2go.setAllLEDs(LEDoff, LEDoff, LEDon) print "Blue" time.sleep(1) pi2go.setAllLEDs(LEDon, LEDon, LEDon) print "White" time.sleep(1) pi2go.setAllLEDs(LEDoff, LEDoff, LEDoff) for i in range(2):
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'): helper.set_element(flags, 'master_set_type', False) if helper.get_element(flags, 'robot_type_com'): pi2go.setAllLEDs(c.LED_ON, c.LED_OFF, c.LED_ON) else: pi2go.setAllLEDs(c.LED_OFF, c.LED_ON, 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' helper.set_element(flags, 'master_set_LED', True)
while True: # Defining the sensors left = pi2go.irLeftLine() right = pi2go.irRightLine() ntime = time.time() # timecheck for distance if ntime > (stime + 0.1): #print "%.5f" %time.time() dist = (int(pi2go.getDistance()*10))/10.0 #print "%.5f" %time.time() stime = ntime # distance groups if dist < 10: pi2go.setAllLEDs(2000, 0, 0) elif dist > 20: pi2go.setAllLEDs(0, 0, 2000) else: pi2go.setAllLEDs(0, 2000, 0) # line follower if left == right: # If both sensors are the same (either on or off) --> forward pi2go.forward(speed) elif left == True: # If the left sensor is on --> move right pi2go.turnForward(speed+change, speed-change) elif right == True: #If the right sensor is on --> move left pi2go.turnForward(speed-change, speed+change) finally: # Even if there was an error, cleanup pi2go.cleanup()
# slow line follower using pi2go library # Author : Zachary Igielman import sys, time import pi2go pi2go.init() pwmMax = 4095 fast = 40 try: while True: if pi2go.irLeftLine() and pi2go.irRightLine(): pi2go.forward(fast) pi2go.setAllLEDs(pwmMax, pwmMax, pwmMax) # Turn LEDs White for Forwards print('straight') elif pi2go.irRightLine()==False: pi2go.spinRight(fast) pi2go.setAllLEDs(pwmMax, 0, 0) # Turn LEDs Red for Right print('right') elif pi2go.irLeftLine()==False: pi2go.spinLeft(fast) pi2go.setAllLEDs(0, 0, pwmMax) # Turn LEDs Blue to Left print('left') except KeyboardInterrupt: pi2go.setAllLEDs (0, 0, 0) pi2go.cleanup() sys.exit()