def __init__(self): pi2go.init() # Main body of code - this detects your key press and changes direction depending on it. try: while True: keyp = self.readkey if keyp == 'w' or keyp == UP: pi2go.forward(SPEED) print 'Forward', SPEED elif keyp == 's' or keyp == DOWN: pi2go.reverse(SPEED) print 'Backward', SPEED elif keyp == 'd' or keyp == RIGHT: pi2go.spinRight(SPEED) print 'Spin Right', SPEED elif keyp == 'a' or keyp == LEFT: pi2go.spinLeft(SPEED) print 'Spin Left', SPEED elif keyp == '.' or keyp == '>': SPEED = min(100, SPEED+10) print 'SPEED+', SPEED elif keyp == ',' or keyp == '<': SPEED = max (0, SPEED-10) print 'SPEED-', SPEED elif keyp == ' ': pi2go.stop() print 'Stop' elif ord(keyp) == 3: break # When you want to exit - press ctrl+c and it will generate a keyboard interrupt - this is handled nicely here! except KeyboardInterrupt: pi2go.cleanup()
def pi2goRun(): print "pi2go Run" pi2goInit() try: while True: pi2goMove() time.sleep(0.1) except KeyboardInterrupt: print "program end:" raise finally: pi2go.cleanup()
def main(): try: opts, args = getopt.getopt(sys.argv[1:],"drs") except getopt.GetoptError as err: print str(err) sys.exit(1) for o, a in opts: if o == "-d": debug=True if o == "-r": robotMode=True hostname=gethostname() print "Launched on "+hostname if robotMode: pi2go.init() c=myserver(name="robot") else: c=myserver(name="test") print "lauching in test mode" c.start() myscreen.getch() pi2go.cleanup() curses.endwin()
def main(): try: opts, args = getopt.getopt(sys.argv[1:], "drs") except getopt.GetoptError as err: print str(err) sys.exit(1) for o, a in opts: if o == "-d": debug = True if o == "-r": robotMode = True hostname = gethostname() print "Launched on " + hostname if robotMode: pi2go.init() c = myserver(name="robot") else: c = myserver(name="test") print "lauching in test mode" c.start() myscreen.getch() pi2go.cleanup() curses.endwin()
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()
keyp = readkey() if keyp == 'w' or ord(keyp) == 16: pi2go.forward(speed) print 'Forward', speed elif keyp == 'z' or ord(keyp) == 17: pi2go.reverse(speed) print 'Reverse', speed elif keyp == 's' or ord(keyp) == 18: pi2go.spinRight(speed) print 'Spin Right', speed elif keyp == 'a' or ord(keyp) == 19: pi2go.spinLeft(speed) print 'Spin Left', speed elif keyp == '.' or keyp == '>': speed = min(100, speed + 10) print 'Speed+', speed elif keyp == ',' or keyp == '<': speed = max(0, speed - 10) print 'Speed-', speed elif keyp == ' ': pi2go.stop() print 'Stop' elif ord(keyp) == 3: break except KeyboardInterrupt: pass finally: pi2go.cleanup()
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'
def intersection(): centerx = 317 center_y = 404 radius = 370 global prev_dist[]= {1,2,3,4,5,6} init_dist[] = {270,270,270,270,270,270} tty.setcbreak(sys.stdin.fileno()) speed = 50 try: while 1: linefollow(speed) print 'linefollow 1' # ultrasonic sensor's obstacle detection obs = ult_dist() if (obs <= 4): pi2go.stop() print 'stop when bot is in front' time.sleep(1.2) elif (obs == 5): speed = 35 #print 'speed = 45' # manual input to control bot if isData(): c = sys.stdin.read(1) if c == '\x6c': # lane change 'l' print 'left-lane change' # ard.write('2') left() elif c == '\x72': # lane change 'r' print 'right-lanechange' # ard.write('3') right() elif c == '\x20': #spacebar print 'stop' # ard.write('1') #ard.write('4') pi2go.stop() time.sleep(5) # sys.exit(0) elif c == '\x77': # 'w' print 'forward' pi2go.forward(speed) time.sleep(3) elif c == '\x73': #'s' print 'reverse' # 's' pi2go.reverse(speed) time.sleep(3) elif c == '\x61': # 'a' print 'left' pi2go.spinLeft(speed) time.sleep(3) elif c == '\x64': # 'd' print 'right' pi2go.spinRight(speed) time.sleep(3) elif c == '\x2e': #period print 'increase speed' speed = max(0, speed + 10) elif c == '\x2c': # comma print 'decrease speed' speed = min(100, speed-10) elif c == '\x65': #e to exit from program print 'exiting...' sys.exit(0) id_1, x_1, y_1 = gps.gps_client(1) id_2, x_2, y_2 = gps.gps_client(2) id_3, x_3, y_3 = gps.gps_client(3) id_4, x_4, y_4 = gps.gps_client(4) id_5, x_5, y_5 = gps.gps_client(5) id_6, x_6, y_6 = gps.gps_client(6) print id_6, x_6, y_6 if id_6 == 6: if (x_6 == 0 and y_6 == 0) or (x_6 == -1 and y_6 == -1) : # not valid or GPS values 0 or -1 print 'no id' continue else: dx = abs(x_6 - centerx) dy = abs(y_6 - centery) if ((dx + dy) <= radius): print 'true' distance_6 = math.sqrt((dx * dx) + (dy * dy)) # 200 - to stop at intersection quadrant_6 = dir(x_6, y_6) if (int_dist[5] > distance_6 or (distance_6+ 5 <= init_dist[5] or init_dist[5] >= distance_6 - 5)): print 'ID %d arriving Intersection' %id_6 if id_1 == 1: init_dist[0] = find(id_1, x_1, y_1, init_dist[0], quadrant_6, distance_6) if id_2 == 2: init_dist[1] = find(id_2, x_2, y_2, init_dist[1], quadrant_6, distance_6) if id_3 == 3: prev_dist_3 = find(id_3, x_3, y_3, init_dist[2], quadrant_6, distance_6) if id_4 == 4: init_dist[3] = find(id_4, x_4, y_4, init_dist[3], quadrant_6, distance_6) if id_5 == 5: init_dist[4] = find(id_4, x_4, y_4, init_dist[4], quadrant_6, distance_6) init_dist[5] = distance_6 else: print 'ID %away from Intersection' % id_6 init_dist[5] = distance_6 else: print 'Id %d not at Intersection' %id_6 finally: termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) pi2go.cleanup()