#!/usr/bin/python
# -*- coding: utf-8 -*-
import sys
import os
import argparse
import requests
import socket
import datetime
import threading
import struct
import sensor_beacon as envsensor
import conf
import ble
import math
import RPi.GPIO as GPIO
import time
import smbus
# constant
VER = 1.0
# ystem constant
GATEWAY = socket.gethostname()
# Global variables
influx_client = None
sensor_list = []
flag_update_sensor_status = False
#LED PWM設定
GPIO.setmode(GPIO.BCM)
GPIO.setup(4, GPIO.OUT) #GPIO4
p4 = GPIO.PWM(4, 100) #Setting GPIO4 Frequency at 100Hz
p4.start(0)
#モータ I2C設定
bus = smbus.SMBus(1)
motorL = 0x64
motorR = 0x60
#モータドライバ書き込み用関数
def writeMotorResister(address, rot, vset):
vdata = vset << 2 | rot
bus.write_word_data(address, 0x00, vdata)
def parse_events(sock, loop_count=10):
global sensor_list
pkt = sock.recv(255)
parsed_packet = ble.hci_le_parse_response_packet(pkt)
if "bluetooth_le_subevent_name" in parsed_packet and \
(parsed_packet["bluetooth_le_subevent_name"]
== 'EVT_LE_ADVERTISING_REPORT'):
for report in parsed_packet["advertising_reports"]:
if (ble.verify_beacon_packet(report)):
sensor = envsensor.SensorBeacon(
report["peer_bluetooth_address_s"],
ble.classify_beacon_packet(report),
GATEWAY,
report["payload_binary"])
#照度, 加速度表示
light = sensor.val_light
print "val_light = ", light
accX = sensor.val_ax
accY = sensor.val_ay
accZ = sensor.val_az
print "Acc = ", accX, accY, accZ
#LED輝度算出
lightLed = 100 *(1.0 - light / 50.0)
if lightLed < 0:
lightLed = 0
print "lightLed = ", lightLed
#LED点灯
p4.ChangeDutyCycle(lightLed)
#モータ
#モータ回転方向
if accX > 0:
rot = 1 #全進
elif accX < 0:
rot = 2 #後進
else:
rot = 0 #停止
#モータスピード計算
speed = math.sqrt(accX * accX + accY * accY)
if accY > 0: #右折
speedL = speed - accY
speedR = speed
else: #左折
speedL = speed
speedR = speed - abs(accY)
#モータスピード正規化
speedL = int(speedL / 30)
speedR = int(speedR / 30)
if speedL > 37:
speedL = 37
if speedR > 37:
speedR = 37
print "Speed = ", speedL, speedR
#モータドライバ書き込み
writeMotorResister(motorL, rot, speedL)
writeMotorResister(motorR, rot, speedR)
else:
pass
else:
pass
return
# check timeout sensor and update flag
def eval_sensor_state():
global flag_update_sensor_status
global sensor_list
nowtick = datetime.datetime.now()
for sensor in sensor_list:
if (sensor.flag_active):
pastSec = (nowtick - sensor.tick_last_update).total_seconds()
if (pastSec > conf.INACTIVE_TIMEOUT_SECONDS):
if debug:
print "timeout sensor : " + sensor.bt_address
sensor.flag_active = False
flag_update_sensor_status = True
timer = threading.Timer(conf.CHECK_SENSOR_STATE_INTERVAL_SECONDS,
eval_sensor_state)
timer.setDaemon(True)
timer.start()
def print_sensor_state():
print "----------------------------------------------------"
print ("sensor status : %s (Intvl. %ssec)" % (datetime.datetime.today(),
conf.CHECK_SENSOR_STATE_INTERVAL_SECONDS))
for sensor in sensor_list:
print " " + sensor.bt_address, ": %s :" % sensor.sensor_type, \
("ACTIVE" if sensor.flag_active else "DEAD"), \
"(%s)" % sensor.tick_last_update
print ""
# Utility function ###
def return_number_packet(pkt):
myInteger = 0
multiple = 256
for c in pkt:
myInteger += struct.unpack("B", c)[0] * multiple
multiple = 1
return myInteger
def return_string_packet(pkt):
myString = ""
for c in pkt:
myString += "%02x" % struct.unpack("B", c)[0]
return myString
def find_sensor_in_list(sensor, List):
index = -1
count = 0
for i in List:
if sensor.bt_address == i.bt_address:
index = count
break
else:
count += 1
return index
# main function
if __name__ == "__main__":
try:
flag_scanning_started = False
debug = True
# reset bluetooth functionality
try:
if debug:
print "-- reseting bluetooth device"
ble.reset_hci()
if debug:
print "-- reseting bluetooth device : success"
except Exception as e:
print "error enabling bluetooth device"
print str(e)
sys.exit(1)
# initialize bluetooth socket
try:
if debug:
print "-- open bluetooth device"
sock = ble.bluez.hci_open_dev(conf.BT_DEV_ID)
if debug:
print "-- ble thread started"
except Exception as e:
print "error accessing bluetooth device: ", str(conf.BT_DEV_ID)
print str(e)
sys.exit(1)
# set ble scan parameters
try:
if debug:
print "-- set ble scan parameters"
ble.hci_le_set_scan_parameters(sock)
if debug:
print "-- set ble scan parameters : success"
except Exception as e:
print "failed to set scan parameter!!"
print str(e)
sys.exit(1)
# start ble scan
try:
if debug:
print "-- enable ble scan"
ble.hci_le_enable_scan(sock)
if debug:
print "-- ble scan started"
except Exception as e:
print "failed to activate scan!!"
print str(e)
sys.exit(1)
flag_scanning_started = True
print ("envsensor_observer : complete initialization")
print ""
# activate timer for sensor status evaluation
timer = threading.Timer(conf.CHECK_SENSOR_STATE_INTERVAL_SECONDS,
eval_sensor_state)
timer.setDaemon(True)
timer.start()
# preserve old filter setting
old_filter = sock.getsockopt(ble.bluez.SOL_HCI,
ble.bluez.HCI_FILTER, 14)
# perform a device inquiry on bluetooth device #0
# The inquiry should last 8 * 1.28 = 10.24 seconds
# before the inquiry is performed, bluez should flush its cache of
# previously discovered devices
flt = ble.bluez.hci_filter_new()
ble.bluez.hci_filter_all_events(flt)
ble.bluez.hci_filter_set_ptype(flt, ble.bluez.HCI_EVENT_PKT)
sock.setsockopt(ble.bluez.SOL_HCI, ble.bluez.HCI_FILTER, flt)
while True:
# parse ble event
parse_events(sock)
if flag_update_sensor_status:
print_sensor_state()
flag_update_sensor_status = False
except Exception as e:
print "Exception: " + str(e)
import traceback
traceback.print_exc()
sys.exit(1)
finally:
if flag_scanning_started:
# restore old filter setting
sock.setsockopt(ble.bluez.SOL_HCI, ble.bluez.HCI_FILTER,
old_filter)
ble.hci_le_disable_scan(sock)
#モータドライバ書き込み
writeMotorResister(motorL, 0, 0)
writeMotorResister(motorR, 0, 0)
#LED STOP
p4.stop()
GPIO.cleanup()
print "Exit"
