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
