def read_distance(): global running while running: print pi2go.getDistance() if pi2go.getDistance() <= 10: pi2go.stop() time.sleep(0.25)
def getmyDistance(): """Get the distance three times and average not time sensitive""" d1 = pi2go.getDistance() time.sleep(0.05) d2 = pi2go.getDistance() time.sleep(0.05) d3 = pi2go.getDistance() time.sleep(0.05) return (d1 + d2 + d3) / 3
def send_distance(): while True: distance = (int(pi2go.getDistance() * 10)) / 10.0 communication.send_udp_unicast_message("127.0.0.1", 38235, str(distance)) print "Sent distance: " + str(distance) + " at: %f " % time.time() time.sleep(0.1)
def moveAround(): while True: while not (pi2go.irLeft() or pi2go.irRight() or pi2go.irCentre()): if pi2go.getDistance() <= 2.0: pi2go.spinLeft(speed) time.sleep(1.5) else: pi2go.forward(speed) pi2go.stop() if pi2go.irLeft(): while pi2go.irLeft(): pi2go.spinRight(speed) time.sleep(0.5) pi2go.stop() if pi2go.irRight(): while pi2go.irRight(): pi2go.spinLeft(speed) time.sleep(0.5) pi2go.stop() if pi2go.irCentre(): while pi2go.irCentre(): pi2go.reverse(speed) time.sleep(2) pi2go.spinLeft(speed) time.sleep(1) pi2go.stop()
def getmyDistance(): """Get the distance three times and average not time sensitive""" d1 = pi2go.getDistance() time.sleep(0.05) d2 = pi2go.getDistance() time.sleep(0.05) d3 = pi2go.getDistance() time.sleep(0.05) # discard outlyers if abs(d1 - (d2 + d3) / 2) < abs(d1) / 10: return (d2 + d3) / 2 elif abs(d2 - (d1 + d3) / 2) < abs(d2) / 10: return (d1 + d3) / 2 elif abs(d3 - (d1 + d2) / 2) < abs(d3) / 10: return (d1 + d2) / 2 else: return (d1 + d2 + d3) / 3
def displayStatus(self): left=pi2go.irLeft() right=pi2go.irRight() leftLine=pi2go.irLeftLine() rightLine=pi2go.irRightLine() distance=pi2go.getDistance() myscreen.addstr(2,30,"sonar "+str(distance)) myscreen.addstr(3,30,"left sensor "+str(left)) myscreen.addstr(4,30,"right sensor "+str(right)) myscreen.addstr(5,30,"leftIR sensor "+str(leftLine)) myscreen.addstr(6,30,"rightIR sensor "+str(rightLine)) myscreen.refresh()
def displayStatus(self): left = pi2go.irLeft() right = pi2go.irRight() leftLine = pi2go.irLeftLine() rightLine = pi2go.irRightLine() distance = pi2go.getDistance() myscreen.addstr(2, 30, "sonar " + str(distance)) myscreen.addstr(3, 30, "left sensor " + str(left)) myscreen.addstr(4, 30, "right sensor " + str(right)) myscreen.addstr(5, 30, "leftIR sensor " + str(leftLine)) myscreen.addstr(6, 30, "rightIR sensor " + str(rightLine)) myscreen.refresh()
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)
def proximity_test(): """Perform whole PiWars Proximity Test challenge.""" MIN_DIST = 1.45 # drive forward 1.3m so that sensors are in range. # use wheel turn calibration on distance #pi2go.goBoth(100) #time.sleep(time10cm*1) # test #time.sleep(time10cm*13) pi2go.stop() # square off to the wall distance = square_up() logging.debug('Distance={}'.format(distance)) # proceed towards the wall slowing down as we approach # speeed 1 (max) at 20cm and 0 (min) at 0.5cm while distance >= MIN_DIST: logging.debug('Speed={}'.format((distance - MIN_DIST) / (20 - MIN_DIST) * 100)) pi2go.goBoth(min((distance - MIN_DIST) / (20 - MIN_DIST) * 100, 100)) distance = pi2go.getDistance() logging.debug('Distance={}'.format(distance)) pi2go.stop()
#!/usr/bin/env python import pi2go import time pi2go.init() speed = 50 pi2go.stepForward(speed,15) time.sleep(1) pi2go.stepSpinL(speed, 12) time.sleep(1) linkerSensor = pi2go.irLeft() rechterSensor = pi2go.irRight() distanz = pi2go.getDistance() knopf = pi2go.getSwitch() pi2go.cleanup() print "Linker Sensor: ", linkerSensor print "Rechter Sensor: ", rechterSensor print "Distanz: ", distanz print "Knopf: ", knopf
local_prev_value = value except: print "Error interpreting message from master! Continuing anyway" elif state == 'RUNNING': # Distance if mode == 'STOP': interval = c.WAIT_DIST_STOP else: interval = c.WAIT_DIST 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: prev_distance = distance distance = new_dist print 'dt:', time_between, distance # Obstacle = 1, No Obstacle = 0 irCentre = pi2go.irCentre() # Obstacle Analysis prev_distance_level = distance_level if mode == 'STOP': if irCentre or (distance < DIST_MIN and prev_distance < DIST_MIN): distance_level = 0 elif distance > c.DIST_MAX and prev_distance > c.DIST_MAX: distance_level = 2
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 keyp == 'd': print pi2go.getDistance() elif ord(keyp) == 3: break except KeyboardInterrupt: pass finally: running = False time.sleep(0.25) pi2go.cleanup()
pi2go.init() # Here we set the speed to 40 out of 100 - feel free to change! speed = 40 # Here is the main body of the program - a lot of while loops and ifs! # In order to get your head around it go through the logical steps slowly! try: while True: if pi2go.irLeft(): while pi2go.irLeft(): # While the left sensor detects something - spin right pi2go.spinRight(speed) pi2go.stop() if pi2go.irRight(): while pi2go.irRight(): # While the right sensor detects something - spin left pi2go.spinLeft(speed) pi2go.stop() while not (pi2go.irLeft() or pi2go.irRight()): if pi2go.getDistance() <= 0.3: # If the distance is less than 0.3, spin right for 1 second pi2go.spinRight(speed) time.sleep(1) else: pi2go.forward(speed) pi2go.stop() finally: # Even if there was an error, cleanup pi2go.cleanup()
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'): set_element(flags, 'button_release', False) prev_state = state state = 'RUNNING' elif state == 'RUNNING': # Distance if check_time_limit(times, 'prev_get_dist', c.WAIT_DIST): distance = pi2go.getDistance() time_between = time.time() - last_time print 'dt:', time_between, distance last_time = time.time() # Obstacle = 1, No Obstacle = 0 irCentre = pi2go.irCentre() # Obstacle Analysis if irCentre or (distance < DIST_MIN): distance_level = 0 elif distance > c.DIST_MAX: distance_level = 2 else: distance_level = 1
# Test for movement in stright line import pi2go, time # Reading single character by forcing stdin to raw mode import sys import tty import termios import threading # main loop speed = 100 pi2go.init() while True: print pi2go.getDistance() if pi2go.getDistance() <= 50: pi2go.reverse(speed) time.sleep(3) pi2go.stop() time.sleep(0.1) pi2go.cleanup()
if pi2go.irAll(): return True else: return False # for command line interaction def isData(): return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) old_settings = termios.tcgetattr(sys.stdin) try: tty.setcbreak(sys.stdin.fileno()) i = 0 # mode while 1: dist = int(pi2go.getDistance()) dist = dist/4 # dist/4 is approx 18~20 cm print 'mode=',i print 'linefollow' line(speed) # Tollgate 3 Task 1 # if detects obstacle at 18~20 cm, it slows down & if less, the bot stops else it continues following line with speed = 50 if dist< 4: pi2go.stop() print 'stop' time.sleep(1.5) elif dist == 4: speed = 30 print 'speed = 30' if isData(): c = sys.stdin.read(1)
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 while True: #print "line follower %f" %time.time() # we don't need to start a thread if theres only one..... # 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 # pi2go.forward(speed) STATE = 00 elif left and not right: # If the left sensor is Off --> move right # pi2go.turnForward(speed+change, speed-change) STATE = 10 elif right and not left: # If the right sensor is off --> move left # pi2go.turnForward(speed - change, speed + change) STATE = 01 else: # stop # pi2go.stop() STATE = 11 if time.time() - start > 0.2: dist = (int(pi2go.getDistance() * 10)) / 10.0 #out_q.put(dist) start = time.time() if dist > 10: STOP = False STATE = 69 else: STOP = True if STOP: pi2go.stop() if STATE == prev_STATE: pass elif STATE == 00: pi2go.forward(speed) # pi2go.stop() elif STATE == 10: pi2go.turnForward(speed + change, speed - change) elif STATE == 01: pi2go.turnForward(speed - change, speed + change) elif STATE == 11: # pi2go.forward(speed) pi2go.stop() prev_STATE = STATE
# Here we set the speed to 40 out of 100 - feel free to change! speed = 40 # Here is the main body of the program - a lot of while loops and ifs! # In order to get your head around it go through the logical steps slowly! try: while True: if pi2go.irLeft(): while pi2go.irLeft(): # While the left sensor is true - spin right pi2go.spinRight(speed) # This stops the motors pi2go.stop() if pi2go.irRight(): while pi2go.irRight(): # While the right sensor is true - spin left pi2go.spinLeft(speed) pi2go.stop() while not (pi2go.irLeft() or pi2go.irRight()): if pi2go.getDistance( ) <= 3: # If the distance is less than 3cm, spin right for 1 second pi2go.spinRight(speed) time.sleep(1) else: pi2go.forward(speed) pi2go.stop() finally: # Even if there was an error, cleanup pi2go.cleanup()
# 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' else: turn = 'Right' if time.time()-tickStart>4: currentNode = currentNode.insertTurn('Turn ',turn) ctrSpinLeft = 0 ctrSpinRight = 0 tickStart = time.time() # End of maze detected, end learn mode in preparation for optimum run elif pi2go.getDistance() <= 9: print ('Exit is found! ending programme...') keyp = 'o' # If the left sensor is on, spin right: elif left == True: pi2go.spinRight(turnSpeed) ctrSpinRight = ctrSpinRight + 1 #If the right sensor is on, spin left: elif right == True: pi2go.spinLeft(turnSpeed) ctrSpinLeft = ctrSpinLeft + 1 # Process learning maze mode if keyp!='l':
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
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))
def updateDistance(): global globalDistance, finished while finished == False: globalDistance=pi2go.getDistance() time.sleep(0.2)
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'
#!/usr/bin/python # sonarNav.py # navigator with utrasonic distance sensor using pi2go library # Author : Zachary Igielman import time import pi2go pi2go.init() fast=50 slow=30 while True: if pi2go.getDistance()>10: pi2go.forward(fast) pi2go.setAllLEDs(4095, 4095, 4095) else: pi2go.setAllLEDs(4095, 0, 0) pi2go.reverse(slow) time.sleep(0.5) pi2go.turnReverse(slow,fast) pi2go.setAllLEDs(0, 4095, 4095) time.sleep(3) pi2go.cleanup()
import pi2go as p2 from time import sleep p2.init() a = p2.getDistance() b = p2.irAll() while True: if p2.getSwitch() == True: while a > 30 and b != True: p2.forward(75) p2.LsetLED(1,1) sleep(0.5) p2.LsetLED(1,0) a = p2.getDistance() b = p2.irAll() else: p2.spinRight(75) p2.LsetLED(1,1) sleep(0.7) p2.LsetLED(1,0) a = p2.getDistance() b = p2.irAll()
pi2go.init() # Here we set the speed to 40 out of 100 - feel free to change! speed = 40 # Here is the main body of the program - a lot of while loops and ifs! # In order to get your head around it go through the logical steps slowly! try: while True: if pi2go.irLeft(): while pi2go.irLeft(): # While the left sensor is true - spin right pi2go.spinRight(speed) # This stops the motors pi2go.stop() if pi2go.irRight(): while pi2go.irRight(): # While the right sensor is true - spin left pi2go.spinLeft(speed) pi2go.stop() while not (pi2go.irLeft() or pi2go.irRight()): if pi2go.getDistance() <= 3: # If the distance is less than 3cm, spin right for 1 second pi2go.spinRight(speed) time.sleep(1) else: pi2go.forward(speed) pi2go.stop() finally: # Even if there was an error, cleanup pi2go.cleanup()
def ult_dist(): dist = pi2go.getDistance() dist = dist/4 return dist
#!/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()
#!/usr/bin/env python import pi2go import time pi2go.init() speed = 50 pi2go.stepForward(speed, 15) time.sleep(1) pi2go.stepSpinL(speed, 12) time.sleep(1) linkerSensor = pi2go.irLeft() rechterSensor = pi2go.irRight() distanz = pi2go.getDistance() knopf = pi2go.getSwitch() pi2go.cleanup() print "Linker Sensor: ", linkerSensor print "Rechter Sensor: ", rechterSensor print "Distanz: ", distanz print "Knopf: ", knopf
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 send_distance(): while True: distance = (int(pi2go.getDistance()*10))/10.0 communication.send_udp_unicast_message("127.0.0.1", 38235, str(distance)) print "Sent distance: " + str(distance) + " at: %f " %time.time() time.sleep(0.1)
speed = 40 change = 25 try: 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) # 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
# Check right sensor if (not pi2go.irRightLine()): goForward = False turnLeft = False turnRight = True turnAround = False # Check left sensor if (not pi2go.irLeftLine()): goForward = False turnLeft = True turnRight = False turnAround = False # Check front proximity sensor distance1 = pi2go.getDistance() distance2 = pi2go.getDistance() # Check distance twice to avoid measuring errors that sometimes occur if (distance1 < 8.0 and distance1 < 8.0 and not turnAround and not alreadyTurned): print "Obstacle detected. Initializing turn..." print "Distance 1: ", distance1 print "Distance 2: ", distance2 goForward = False turnLeft = False turnRight = False turnAround = True except KeyboardInterrupt: print
helper.set_element(flags, 'robot_type_com', False) com.send_x_broadcast_messages( c.PORT, "RELEASE", c.SENDING_ATTEMPTS, c.WAIT_SEND) print "MASTER: Changing from type " + local_prev_value + " to type " + value local_prev_value = 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: prev_distance = distance distance = new_dist print 'dt:', time_between, distance # Obstacle = 1, No Obstacle = 0 irCentre = pi2go.irCentre() # Obstacle Analysis prev_distance_level = distance_level if mode == 'STOP': if irCentre or (distance < DIST_MIN and prev_distance < DIST_MIN): distance_level = 0 elif distance > c.DIST_MAX and prev_distance > c.DIST_MAX:
speed = 40 change = 25 try: 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) # 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
# 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' else: turn = 'Right' if time.time() - tickStart > 4: currentNode = currentNode.insertTurn('Turn ', turn) ctrSpinLeft = 0 ctrSpinRight = 0 tickStart = time.time() # End of maze detected, end learn mode in preparation for optimum run elif pi2go.getDistance() <= 9: print('Exit is found! ending programme...') keyp = 'o' # If the left sensor is on, spin right: elif left == True: pi2go.spinRight(turnSpeed) ctrSpinRight = ctrSpinRight + 1 #If the right sensor is on, spin left: elif right == True: pi2go.spinLeft(turnSpeed) ctrSpinLeft = ctrSpinLeft + 1 # Process learning maze mode if keyp != 'l':