def main(args): global pisugar, paper, ui buzzer = Buzzer(pin=16) buzzer.play(sound=buzzer.alert, blocking=False, repeat=100) ui.build = buildCustomUI ui.start() vbeacon.startScanning() vbeacon.startAdvertising() print("--------------------------\nDEBUG 1: " + (str)(vbeacon.isAdvertising) + "\n-----------------------") done = False while not done: try: beaconsNearby = vbeacon.getNearbyBeacons() print("-------------------------------------") print(f" {len(beaconsNearby)} beacons nearby") print("-------------------------------------") for beacon in beaconsNearby: print(f"{beacon}") time.sleep(1.0) except KeyboardInterrupt: done = True vbeacon.stopAdvertising() vbeacon.stopScanning()
def navigate(): ''' Function will call exploration() function to move robot, but will keep track of previous moves ''' # Test output print("Navigating ...") # Create motor and buzzer objects motor = Motor() buzzer = Buzzer() # Beep to indicate begining of navigate step buzzer.play(4) try: # Enter the exploration loop for i in xrange(params.p['MAX_ITER']): # Execute explore function and save results explore(motor, buzzer) # Wait between moves time.sleep(params.p['WAIT_TIME']) except Exception: pass finally: motor.stop_bot()
def navigate(room, new_area): # initialize room.current_area if it does not exsist if not room.current_area: room.current_area = Area() # Test output print "Navigating (moving to a known area) ..." # Beep to indicate begining of navigate step buzzer = Buzzer() buzzer.play(4) # Make sure a path exsists between the two areas if room.current_area not in new_area.previous: print "No path between nodes" return # Get move required to travel between areas move = new_area.moves_performed[room.current_area.name] # TODO: moves_performed will just be the latest move performed between the two areas # Need to find a way to find the "best" move between them # Execute motion primitives between nodes for (direction, amount) in move.primitives: print "Moving" + str(direction) + " " + str(amount) # moveBot(direction, amount, params.p['MOTOR_PWR']) # Get final position by summing initial position and delta move.final_pos = [init + delt for init, delt in zip(move.initial_pos, move.delta)] # TODO: put a kalman filter on the movement. Use camera and sonar as # truth? Not sure here # TODO: figure out what area you are in by looking at pictures new_area.localize # Add move to new area's dictionary of moves new_area.moves_performed[room.current_area.name] = move
class vBeacon(): #------------------------------------------------------------------------- # Initializer #------------------------------------------------------------------------- def __init__(self): self.advertLoop = None self.advertiser = None self.ad_manager = None self.advertThread = None self.scanThread = None self.scanExit = True self.scanMutex = threading.Lock() self.beaconList = {} self.company_id = 0x004c self.beacon_type = [0x02, 0x15] self.uuid = uuid.UUID('{2f234454-cf6d-4a0f-adf2-f4911ba9ffa6}') self.tx_power = [0xb3] self.major = 0 self.minor = 0 self.deviceSettings = {} self.pisugar = PiSugar2() self.buzzer = Buzzer(16) #------------------------------------------------------------------------- # Advertiser registration callback #------------------------------------------------------------------------- def register_ad_cb(self): return #------------------------------------------------------------------------- # Advertiser registration error callback #------------------------------------------------------------------------- def register_ad_error_cb(self, error): print('Failed to register advertisement: ' + str(error)) self.advertLoop.quit() #------------------------------------------------------------------------- # Find adaptor #------------------------------------------------------------------------- def find_adapter(self, bus): remote_om = dbus.Interface(bus.get_object(BLUEZ_SERVICE_NAME, '/'), DBUS_OM_IFACE) objects = remote_om.GetManagedObjects() for o, props in objects.items(): if LE_ADVERTISING_MANAGER_IFACE in props: return o return None #------------------------------------------------------------------------- # Given a beacon return device ID string by concatenating # major and minor in a 4 byte value #------------------------------------------------------------------------- def _deviceKey(self, beacon): key = hex(beacon['major']) + hex(beacon['minor']) return key #------------------------------------------------------------------------- # Return device ID which is concatenation of major and minor in hex #------------------------------------------------------------------------- def deviceId(self): id = hex(self.major) + hex(self.minor) return id #------------------------------------------------------------------------- # Stop advertisement #------------------------------------------------------------------------- def stopAdvert(self): self.advertLoop.quit() self.advertThread.join() self.ad_manager.UnregisterAdvertisement(self.advertiser) dbus.service.Object.remove_from_connection(self.advertiser) self.advertiser = None self.advertLoop = None print('Stopped advertising') #------------------------------------------------------------------------- # # Start advertisement for 'timeout' seconds # # timeout = 0, continuous advertisement # timeout > 0, stop advertisement after 'timeout' seconds # #------------------------------------------------------------------------- def startAdvert(self): if self.advertiser is not None: return False dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) bus = dbus.SystemBus() adapter = self.find_adapter(bus) if not adapter: print('LEAdvertisingManager1 interface not found') return adapter_props = dbus.Interface( bus.get_object(BLUEZ_SERVICE_NAME, adapter), "org.freedesktop.DBus.Properties") adapter_props.Set("org.bluez.Adapter1", "Powered", dbus.Boolean(1)) self.ad_manager = dbus.Interface( bus.get_object(BLUEZ_SERVICE_NAME, adapter), LE_ADVERTISING_MANAGER_IFACE) self.advertiser = Advertisement(bus, 0, 'peripheral') self.advertiser.add_manufacturer_data( self.company_id, self.beacon_type + list(self.uuid.bytes) + [self.major / 256, self.major % 256] + [self.minor / 256, self.minor % 256] + self.tx_power) self.advertLoop = GLib.MainLoop() self.ad_manager.RegisterAdvertisement( self.advertiser.get_path(), {}, reply_handler=self.register_ad_cb, error_handler=self.register_ad_error_cb) self.advertThread = threading.Thread(target=self.advertLoop.run) self.advertThread.setDaemon(True) self.advertThread.start() print("Start advertising") return True #------------------------------------------------------------------------- # Scan loop #------------------------------------------------------------------------- def _scanLoop(self): #Set bluetooth device. Default 0. dev_id = 0 try: sock = bluez.hci_open_dev(dev_id) except: print("Error accessing bluetooth") ScanUtility.hci_enable_le_scan(sock) self.scanExit = False self.scanMutex.acquire() self.beaconList = {} self.scanMutex.release() while not self.scanExit: beacons = ScanUtility.parse_events(sock, 10) currTime = datetime.datetime.now() self.scanMutex.acquire() for beacon in beacons: if beacon['type'] == 'iBeacon': if beacon['uuid'] == str(self.uuid): self.beaconList[self._deviceKey(beacon)] = (currTime, beacon) ''' elif beacon['type'] == 'Overflow': if beacon['uuid'] == '2f234454-cf6d-4a0f-adf2-f4911ba9ffa6': print("Overflow:-------------") print(beacon) print("") ''' self.scanMutex.release() #------------------------------------------------------------------------- # Stop scanning #------------------------------------------------------------------------- def stopScanning(self): self.scanExit = True self.scanThread.join() print("Stopped scanning") #------------------------------------------------------------------------- # Start scanning #------------------------------------------------------------------------- def startScanning(self): self.scanThread = threading.Thread(target=self._scanLoop) self.scanThread.setDaemon(True) self.scanThread.start() print("Start scanning") #------------------------------------------------------------------------- # Wake after function #------------------------------------------------------------------------- def setWakeAfter(self, seconds): now = datetime.datetime.now() res = self.pisugar.set_rtc_alarm(now + datetime.timedelta(0, seconds), [1, 1, 1, 1, 1, 1, 1]) #------------------------------------------------------------------------- # Sleep function #------------------------------------------------------------------------- def gotoSleep(self): self.pisugar.force_shutdown() #------------------------------------------------------------------------- # Check social distancing #------------------------------------------------------------------------- def checkSocialDistancing(self, dist): print("Checking social distance...") now = datetime.datetime.now() inViolation = False self.scanMutex.acquire() for key in self.beaconList: (t, b) = self.beaconList[key] if (b['rssi'] > dist): inViolation = True self.scanMutex.release() if inViolation: print("Social distance violation!!!") self.buzzer.play(sound=self.buzzer.alert, repeat=0) #------------------------------------------------------------------------- # Run the App #------------------------------------------------------------------------- def runApp(self, settings): with open(settings) as f: self.deviceSettings = json.load(f) self.uuid = uuid.UUID(self.deviceSettings['uuid']) self.major = self.deviceSettings['major'] self.minor = self.deviceSettings['minor'] self.name = self.deviceSettings['name'] self.org = self.deviceSettings['org'] self.onTime = self.deviceSettings['onTime'] self.wakeTime = self.deviceSettings['wakeTime'] self.socialDist = self.deviceSettings['socialDist'] print(f"uuid={str(self.uuid)}") self.startAdvert() self.startScanning() done = False seconds = 0 while not done: time.sleep(1.0) seconds = seconds + 1 # Report to backend # Log to file self.checkSocialDistancing(self.socialDist) if seconds >= self.onTime: break print("Shutting down....") self.stopScanning() self.stopAdvert() print(f"Wake after {self.wakeTime - seconds}")
display.resetCursor() display.write(ledHeader) audio.connected() time.sleep(1) #---------------------------- # Print Initialization to LED #---------------------------- buzz = [ 0 ] buzzfinal = [ 4 ] microsec = [ 16 ] for i in range (0, 3): message = ledHeader + "Starting\n\t\t\t" + str(3 - i) display.clear() display.resetCursor() display.write(message) audio.play(buzz, microsec) time.sleep(0.7) display.clear() display.resetCursor() display.write(ledHeader) audio.play(buzzfinal, microsec) #---------------------------- # Gesture Detection #---------------------------- while True: x = ges.run() if x == 1: message = ledHeader + "Freeze!!!" display.clear() display.resetCursor() display.write(message)
display.resetCursor() display.write(ledHeader) audio.connected() time.sleep(1) #---------------------------- # Print Initialization to LED #---------------------------- buzz = [0] buzzfinal = [4] microsec = [16] for i in range(0, 3): message = ledHeader + "Starting\n\t\t\t" + str(3 - i) display.clear() display.resetCursor() display.write(message) audio.play(buzz, microsec) time.sleep(0.7) display.clear() display.resetCursor() display.write(ledHeader) audio.play(buzzfinal, microsec) #---------------------------- # Gesture Detection #---------------------------- while True: x = ges.run() if x == 1: message = ledHeader + "Freeze!!!" display.clear() display.resetCursor() display.write(message)
def explore(room): # Move to a new area, returns area object # initialize room.current_area if it does not exsist if not room.current_area: room.current_area = Area() # Test output print "Exploring (moving to a new location) ..." # Beep to indicate begining of explore step buzzer = Buzzer() buzzer.play(5) # Create new area and move objects new_area = Area() move = Move() # Make sure move is set as a real move (performed by robot) move.type = "Real" # Link it to the previous object new_area.previous.append(room.current_area.name) # Initial position set to position of previous area move.initial_pos = room.current_area.pos # Vector of movement used move = get_move_vector(move) # Break down movement vector into motion primitives that robot can execute move.getMotionPlan() for (direction, amount) in move.primitives: print "Moving " + str(direction) + " " + str(amount) # moveBot(direction, amount, params.p['MOTOR_PWR']) # Get final position by summing initial position and delta move.final_pos = [init + delt for init, delt in zip(move.initial_pos, move.delta)] # TODO: put a kalman filter on the movement. Use camera and sonar as # truth? Not sure here # # Test print # print "Describing move in explore function" move.describe() # Update location of new area to final position of move new_area.pos = move.final_pos # Add move to new area's dictionary of moves new_area.moves_performed[room.current_area.name] = move # Redirect current area to this new_area room.current_area = new_area # Localize bot in new area # new_area.localize # # Test print # print "Describing new_area in explore function" # new_area.describe() # Append new_area to room room.areas.append(new_area) # Return updated room object return room