class _SenseHat:
def __init__(self, board_object, colour=""):
self.board = board_object
self.colour = colour
self.sense = SenseHat()
def magnetometer_on(self):
self.sense.set_imu_config(True, False, False) # gyroscope only
@property
def temp_c(self):
return (self.sense.get_temperature_from_humidity() +
self.sense.get_temperature_from_pressure())/2
@property
def pressure(self):
return self.sense.pressure
@property
def humidity(self):
return self.sense.humidity
def led_all(self, colour):
lcd = []
for i in range(0, 64):
lcd.append(colour)
self.sense.set_pixels(lcd)
def led_1(self, colour):
self.sense.set_pixel(0, 0, colour)
self.sense.set_pixel(0, 1, colour)
self.sense.set_pixel(1, 0, colour)
self.sense.set_pixel(1, 1, colour)
def led_2(self, colour):
self.sense.set_pixel(2, 2, colour)
self.sense.set_pixel(2, 3, colour)
self.sense.set_pixel(3, 2, colour)
self.sense.set_pixel(3, 3, colour)
def led_3(self, colour):
self.sense.set_pixel(4, 4, colour)
self.sense.set_pixel(4, 5, colour)
self.sense.set_pixel(5, 4, colour)
self.sense.set_pixel(5, 5, colour)
def led_4(self, colour):
self.sense.set_pixel(6, 6, colour)
self.sense.set_pixel(6, 7, colour)
self.sense.set_pixel(7, 6, colour)
self.sense.set_pixel(7, 7, colour)
def clear(self):
self.sense.clear()
def init():
global dsp_mtx
dsp_mtx = [
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0],
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0],
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0],
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0],
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0],
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0],
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0],
[0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0], [0,0,0]
]
sense = SenseHat()
sense.set_imu_config(False, False, True)
global sense
tt = threading.Thread(target=doit)
tt.daemon = True
tt.start()
print "start sleeping..."
time.sleep(64)
def main():
cli = mosquitto.Mosquitto()
cli.on_message = on_message
cli.on_publish = on_publish
#cli.tls_set('root.ca',
#certfile='c1.crt',
#keyfile='c1.key')
#cli.username_pw_set("guigui", password="******")
cli.connect(config['host'], config['port'], config['keepalive'])
sense = SenseHat()
sense.set_imu_config(True, True, True)
d_sensors = all_sensors(sense)
while cli.loop() == 0:
#now = now_timestamp()
now = datetime.datetime.now(pytz.utc)
sensors = d_sensors.keys() # all sensors
#print(sensors)
for sensor in sensors:
get_value = d_sensors[sensor] # get a "callable"
#print(sensor, get_value)
data = {
'ts': now.isoformat(),
'd': {
sensor: get_value()
}
}
#print(data)
payload = json.dumps(data) # serialization
cli.publish(topic='/sensors/SenseHat01/%s' % sensor, payload=payload, qos=0, retain=False)
def pi3d_model():
from sense_hat import SenseHat
import math
import pi3d
sense = SenseHat()
display = pi3d.Display.create()
cam = pi3d.Camera.instance()
shader = pi3d.Shader("mat_light")
model = pi3d.Model(file_string="apollo-soyuz.obj",
name="model",
x=0,
y=-1,
z=40,
sx=1.5,
sy=1.5,
sz=1.5)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 72
while display.loop_running():
orientation = sense.get_orientation_radians()
if orientation is None:
pass
pitch = orientation["pitch"]
roll = orientation["roll"]
yaw = orientation["yaw"]
yaw_total = yaw + math.radians(yaw_offset)
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
sin_r = math.sin(roll)
cos_r = math.cos(roll)
abs_roll = math.degrees(
math.asin(sin_p * cos_y + cos_p * sin_r * sin_y))
abs_pitch = math.degrees(
math.asin(sin_p * sin_y - cos_p * sin_r * cos_y))
model.rotateToZ(abs_roll)
model.rotateToX(abs_pitch)
model.rotateToY(math.degrees(yaw_total))
model.draw()
keypress = keyb.read()
if keypress == 27:
keyb.close()
display.destroy()
break
elif keypress == ord('m'):
compass = not compass
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('g'):
gyro = not gyro
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('a'):
accel = not accel
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('='):
yaw_offset += 1
elif keypress == ord('-'):
yaw_offset -= 1
########################################################################################################
# This python script runs on the HAB pi3 computer. It is started on power up by adding the following line
# to the bottom of the /etc/rc.local file before the exit 0 line
# sudo python /home/pi/HAB/HABSim08.py
#
###############################################Python Library Imports######################
import time
import serial
import string
import enum
from random import randint
from time import sleep
from sense_hat import SenseHat
sense = SenseHat()
sense.set_imu_config(True, True,
True) # enable compass, gyro, and accelerometer
print("HAB pi3 is Running")
#######Turn a blue pixel on to indicate the program is running on powerup ##################
sense.clear()
pixel_x = 4
pixel_y = 4
red = 0
green = 0
blue = 255
sense.set_pixel(pixel_x, pixel_y, red, green, blue)
####################################New Shepard Flight Flight Status ##################################
IDLE = 0
LIFT_OFF = 1
MECO = 2
# Main sensor polling loop
while True:
# This time is used for logging purposes in the CSV data file
data_time = time.strftime("%H:%M:%S",time.gmtime())
### Readings from the SenseHat
## Environment sensors
SH_temp = sense.get_temperature() # value in degrees C
SH_pressure = sense.get_pressure() * 100 # convert output from millibars to Pascals for consistency
SH_humidity = sense.get_humidity() # % relative humidity
# Orientation
sense.set_imu_config(True,True,True) # Enable compass, gyro, and accelerometer
SH_orientation = sense.get_orientation() # orientation of pitch, roll, yaw axes in degrees
SH_orientation_x = SH_orientation.get('x')
SH_orientation_y = SH_orientation.get('y')
SH_orientation_z = SH_orientation.get('z')
# Magnetometer data
#sense.set_imu_config(True,False,False)
time.sleep(0.01) # sleep for 10 ms after changing IMU configuration
SH_compass_north = sense.get_compass() # direction of magnetometer from North, in degrees
SH_compass_raw = sense.get_compass_raw() # magnetic intensity of x, y, z axes in microteslas
SH_compass_raw_x = SH_compass_raw.get('x')
SH_compass_raw_y = SH_compass_raw.get('y')
SH_compass_raw_z = SH_compass_raw.get('z')
# Gyro Data
class Accelerometer(Thread):
def __init__(self, quantum, bin_logger):
Thread.__init__(self)
self.daemon = True
self.Sense = SenseHat()
self.Sense.set_imu_config(False, False, True)
self.cnt = 0
self.quantum = quantum
self.period = 0.01
self.iters = int(self.quantum / self.period)
self.que = Queue(1024)
self.dorun = True
self.acc_offset = 0
self.bin_logger = bin_logger
def run(self):
print("Calibrating...")
self.adjust()
print("Calibration done!")
while (self.dorun):
i = self.iters
max = -1000000000.0
min = 1000000000.0
sum = 0.0
acc_first = self.read_acc()
while (i > 0):
rd = self.read_acc()
av = self.get_adj_len()
sum = sum + av
if (max < av):
max = av
acc_max = rd
acc_max_i = (self.iters - i)
if (min > av):
min = av
acc_min = rd
acc_min_i = (self.iters - i)
i = i - 1
time.sleep
acc_last = self.read_acc()
ruck_avg = sqrt(pow(acc_first['x']-acc_last['x'], 2)+pow(acc_first['y']-acc_last['y'], 2)+pow(acc_first['z']-acc_last['z'], 2))/self.quantum
ruck_max = sqrt(pow(acc_max['x']-acc_min['x'], 2)+pow(acc_max['y']-acc_min['y'], 2)+pow(acc_max['z']-acc_min['z'], 2))/(abs(acc_min_i-acc_max_i)*self.period)
acc_dic = {'avg': (sum/float(self.iters)), 'min': min, 'max': max}
ruck_dic = {'avg': ruck_avg, 'max': ruck_max}
self.que.put([dt.datetime.now().strftime('%Y%m%d %H:%M.%S.%f'),
acc_dic, ruck_dic, acc_first, acc_last])
time.sleep(self.period)
def writeBinLog(self, entry):
if (self.bin_logger == None):
return
else:
self.bin_logger.putNext([dt.datetime.now().strftime('%Y%m%d %H:%M.%S.%f'), entry])
def adjust(self):
ac = 0.0
for i in range(0,100):
rd = self.read_acc()
av = self.get_len()
ac += av
self.acc_offset = ac/100.0
self.acc_offset *= 1.02
def read_acc(self):
a1 = self.Sense.get_accelerometer_raw()
self.writeBinLog(a1)
self.acc = a1
return a1
def get(self):
return [self.get_x(), self.get_y(), self.get_z()]
def get_x(self):
return self.acc['x']
def get_y(self):
return self.acc['y']
def get_z(self):
return self.acc['z']
def get_len(self):
self.Length = sqrt(pow(self.get_x(),2) + pow(self.get_y(),2) + pow(self.get_z(),2))
return self.Length
def get_adj_len(self):
return self.get_len()-self.acc_offset
def report(self):
print(self.get_x())
print(self.get_y())
print(self.get_z())
print(self.get())
print(self.get_len())
def stopit(self):
self.dorun = False
def getNext(self):
try:
return self.que.get(True, timeout=5)
except:
print("Exception")
def read_queue(self):
yield self.que.get(True, None)
import time
# import the datetime library which allows us to create a timestamp to include in the JSON file
from datetime import datetime, timedelta
# import json which allows us to read and write JSON files
import json
# instantiate new SenseHat object
sense = SenseHat()
# clear any existing readings
sense.clear()
# enable the three Inertial Motion Unit (IMU) sensors
sense.set_imu_config(True, True,
True) # compass, gyroscope, accelerometer in that order
# initialise the list{} structure used to hold the sensor readings
# This is actually a Dict structure because curly brackets
# Refactoring after discussions with Zac to use Dict literals instead
#data = {}
#data['basic'] = []
#data['imu'] = {}
#data['imu']['orientation'] = []
#data['imu']['acceleration'] = []
#data['meta'] = []
# amount of time, in seconds, that the script should run for
scriptDuration = timedelta(seconds=300)
import ConfigParser
# New instance with 'bar' and 'baz' defaulting to 'Life' and 'hard' each
config = ConfigParser.ConfigParser()
config.read('/home/pi/ybot/ybot.cfg')
print config.get('Motors', 'MLA') # -> "Python is fun!"
print config.get('Ranger', 'TRIG') # -> "Life is hard!"
Tolerance = 5
LinearTolerance=2
sense = SenseHat()
#sense.set_imu_config(True, False, False) # magnet only
sense.set_imu_config(True, True, True)
#motors pins
MLA=5 #GRIS pin 29 M2 IN1
MLB=6 #BLANC pin 31 M2 IN2
MRA=16 #JAUNE pin 36 M1 IN1
MRB=26 #ORANGE pin 37 M1 IN2
#telemeter pins
TRIG = 17 #17 en pin
ECHO = 27 #27 en pin
#derive compensation
COMPENSATION_LEFT=1
COMPENSATION_RIGHT=1
def main():
global running
signal.signal(signal.SIGINT, handle_exit)
# Read config file
conf = init()
# Connect to broker
client = mqtt.Client(conf['id'])
client.tls_set("./cert/CARoot.pem",
"./cert/" + conf['id'] + "-certificate.pem.crt",
"./cert/" + conf['id'] + "-private.pem.key",
cert_reqs=ssl.CERT_NONE,
tls_version=ssl.PROTOCOL_TLSv1_2)
client.on_connect = on_connect
client.on_message = on_message
# client.on_log = on_log #debug
client.connect(conf['entrypoint'], conf['port'], 60)
# Init hardware
if (conf['type'] == 'sensehat'):
sense = SenseHat()
sense.set_imu_config(True, False, False) # Campass Only
elif (conf['type'] == 'grovepi'):
pinMode(conf['button'], "INPUT")
pinMode(conf['potentiometer'], "INPUT")
grove_potentiometer = conf['potentiometer'] - 14
# Publish
while running:
if (conf['type'] == 'sensehat'):
publish(client, conf['id'], "temperature",
repr(sense_get_temperature(sense)))
publish(client, conf['id'], "humidity",
repr(sense_get_humidity(sense)))
publish(client, conf['id'], "pressure",
repr(sense_get_pressure(sense)))
publish(client, conf['id'], "compass",
repr(sense_get_compass(sense)))
eventList = sense.stick.get_events()
for event in eventList[:]:
if (event.direction == 'middle' and
(event.action == 'pressed' or event.action == 'released')):
publish(client, conf['id'], "button", event.action)
time.sleep(1)
elif (conf['type'] == 'grovepi'):
[temp, hum] = dht(conf['dht_port'], conf['dht_type'])
time.sleep(1)
potentiometer = analogRead(grove_potentiometer)
button = digitalRead(conf['button'])
publish(client, conf['id'], "temperature", repr(temp))
publish(client, conf['id'], "humidity", repr(hum))
publish(client, conf['id'], "button", repr(button))
publish(client, conf['id'], "potentiometer", repr(potentiometer))
# Disconnect
client.disconnect()
class Worker(QObject):
signalStatus = pyqtSignal(str)
updateButtons = pyqtSignal(bool, bool)
updateLcd = pyqtSignal(float, float, float)
updateGraphs = pyqtSignal(float, float, float, float)
def __init__(self, parent=None):
super(self.__class__, self).__init__(parent)
self.filename = "log.txt"
self.temperature = 0
self.humidity = 0
self.pressure = 0
self.interval = 5
self.sense = SenseHat()
self.sense.set_imu_config(False, False, True)
self.debugMode = False
self.stopEvent = threading.Event()
@pyqtSlot()
def start(self):
self.updateButtons.emit(False, True)
self.stopEvent.clear()
while not self.stopEvent.is_set():
self.rotate_display()
if self.debugMode == False:
self.temperature = self.sense.get_temperature()
self.humidity = self.sense.get_humidity()
self.pressure = self.sense.get_pressure()
self.updateLcd.emit(self.temperature, self.humidity, self.pressure)
self.updateGraphs.emit(self.temperature, self.humidity, self.pressure, self.interval)
self.show_temperature_on_led_matrix(self.temperature)
self.log_to_file(self.temperature, self.humidity, self.pressure, self.filename)
self.stopEvent.wait(timeout=(self.interval))
def stop(self):
self.stopEvent.set()
def update_temperature(self, value):
if self.debugMode == True:
self.temperature = value
def update_humidity(self, value):
if self.debugMode == True:
self.humidity = value
def update_pressure(self, value):
if self.debugMode == True:
self.pressure = value
def update_interval(self, value):
self.interval = value
def update_debug_mode(self, value):
self.debugMode = value
if value == True:
self.filename = "log_debug.txt"
else:
self.filename = "log.txt"
def log_to_file(self, temperature, humidity, pressure, filename):
temperatureString = "{:.1f}°C".format(temperature)
humidityString = "{:.0f}%".format(humidity)
pressureString = "{:.0f}mbar".format(pressure)
timeString = time.strftime("%d/%m/%Y %H:%M:%S")
file = open(filename, "a")
file.write(timeString + " | " + "Temperature: " + temperatureString + " | " + "Humidity: " + humidityString
+ " | " + "Pressure: " + pressureString + "\n")
file.close()
def show_temperature_on_led_matrix(self, temp):
positiveDegreeValue = 255 - temp * 5
negativeDegreeValue = 255 + temp * 5
if temp >= 0 and temp < 50:
tempGrade = (255, positiveDegreeValue, positiveDegreeValue)
elif temp < 0 and temp > -50:
tempGrade = (negativeDegreeValue, negativeDegreeValue, 255)
elif temp >= 50:
tempGrade = (255, 0, 0)
elif temp <= -50:
tempGrade = (0, 0, 255)
tempToString = "{:.1f}".format(temp)
self.sense.show_message(tempToString + "c", back_colour=[0, 0, 0], text_colour=tempGrade)
def rotate_display(self):
x = round(self.sense.get_accelerometer_raw()['x'], 0)
y = round(self.sense.get_accelerometer_raw()['y'], 0)
rotation = 0
if x == -1:
rotation = 90
elif y == -1:
rotation = 180
elif x == 1:
rotation = 270
self.sense.set_rotation(rotation)
class PiSenseHat(object):
"""Raspberry Pi 'IoT Sense Hat API Driver Class'."""
# Constructor
def __init__(self):
self.sense = SenseHat()
# enable all IMU functions
self.sense.set_imu_config(True, True, True)
# pixel display
def set_pixel(self,x,y,color):
# red = (255, 0, 0)
# green = (0, 255, 0)
# blue = (0, 0, 255)
self.sense.set_pixel(x, y, color)
# clear pixel display
def clear_display(self):
self.sense.clear()
# Pressure
def getPressure(self):
return self.sense.get_pressure()
# Temperature
def getTemperature(self):
return self.sense.get_temperature()
# Humidity
def getHumidity(self):
return self.sense.get_humidity()
def getHumidityTemperature(self):
return self.sense.get_temperature_from_humidity()
def getPressureTemperature(self):
return self.sense.get_temperature_from_pressure()
def getOrientationRadians(self):
return self.sense.get_orientation_radians()
def getOrientationDegrees(self):
return self.sense.get_orientation_degrees()
# degrees from North
def getCompass(self):
return self.sense.get_compass()
def getAccelerometer(self):
return self.sense.get_accelerometer_raw()
def getEnvironmental(self):
sensors = {'name' : 'sense-hat', 'environmental':{}}
return sensors
def getJoystick(self):
sensors = {'name' : 'sense-hat', 'joystick':{}}
return sensors
def getInertial(self):
sensors = {'name' : 'sense-hat', 'inertial':{}}
def getAllSensors(self):
sensors = {'name' : 'sense-hat', 'environmental':{}, 'inertial':{}, 'joystick':{}}
sensors['environmental']['pressure'] = { 'value':self.sense.get_pressure(), 'unit':'mbar'}
sensors['environmental']['temperature'] = { 'value':self.sense.get_temperature(), 'unit':'C'}
sensors['environmental']['humidity'] = { 'value':self.sense.get_humidity(), 'unit': '%RH'}
accel = self.sense.get_accelerometer_raw()
sensors['inertial']['accelerometer'] = { 'x':accel['x'], 'y':accel['y'], 'z': accel['z'], 'unit':'g'}
orientation = self.sense.get_orientation_degrees()
sensors['inertial']['orientation'] = { 'compass':self.sense.get_compass(), 'pitch':orientation['pitch'], 'roll':orientation['roll'], 'yaw': orientation['yaw'], 'unit':'degrees'}
return sensors
class sensors():
def __init__(self):
self.pitch = 0
self.roll = 0
self.yaw = 0
self.heading = 10
self.temp = 0
self.humidity = 0
self.pressure = 0
self.ax = 0
self.ay = 0
self.az = 0
self.altitude = 0
self.send_timer=0
def joinDelimiter(self, arr):
tmp=[None]*len(arr)
for i in range(len(arr)):
tmp[i]=str(arr[i])
return ",".join(tmp)
def getRandomStrArr(self):
pitch = r.randint(3, 5)
roll = r.randint(3, 5)
yaw = r.randint(0, 2)
compass = r.randint(240, 241)
temp = r.randint(19, 20)
humidity = r.randint(43, 46)
pressure = r.randint(983, 985)
ax = 0.1
ay = 0.1
az = 0.1
altitude = 286
return self.joinDelimiter([pitch, roll, yaw, compass, temp, humidity, pressure, ax, ay, az, altitude])
def run(self):
# Comment if not running on RPI
self.sense = SenseHat()
self.sense.clear()
self.sense.set_imu_config(True, True, True)
while True:
self.temp = round(self.sense.get_temperature(), 1)
self.humidity = round(self.sense.get_humidity(), 1)
self.pressure = round(self.sense.get_pressure(), 1)
self.sense.set_imu_config(True, True, True)
orientation = self.sense.get_orientation()
pitch = orientation['pitch']
roll = orientation['roll']
yaw = orientation['yaw']
ax, ay, az = self.sense.get_accelerometer_raw().values()
self.heading = round(self.sense.get_compass(), 1)
if (pitch > 180):
pitch -= 360
self.pitch = round(pitch, 1)
self.roll = round(roll, 1)
self.yaw = round(yaw, 1)
self.ax = round(ax, 2)
self.ay = round(ay, 2)
self.az = round(az, 2)
self.altitude = round((288.15 / -0.0065) * ((self.pressure * 100 / 101325) ** (-(8.31432 * -0.0065) / (9.80665 * 0.0289644)) - 1),1)
"""
self.pitch = r.randint(3, 5)
self.roll = r.randint(3, 5)
self.yaw = r.randint(0, 2)
self.compass = r.randint(240, 241)
self.temp = r.randint(19, 20)
self.humidity = r.randint(43, 46)
self.pressure = r.randint(983, 985)
self.ax = 0.1
self.ay = 0.1
self.az = 0.1
self.altitude = 286
"""
# sensors must initialize
try:
t=time.time()
if(time.time()-self.send_timer>delays.BROWSER):
sensors_publisher.send_string("%s %s" % (topic.SENSOR_TOPIC, self.getString()))
self.send_timer=t
#print(time.time()-t)
except:
print("sensors error")
time.sleep(delays.SENSOR_REFRESH_DELAY)
def getString(self):
return self.joinDelimiter([self.pitch, self.roll, self.yaw, self.heading, self.temp, self.humidity, self.pressure, self.ax, self.ay,
self.az, self.altitude])
# Update values if instance not doint reading with run()
def setValues(self,string):
self.pitch, self.roll, self.yaw, self.heading, self.temp, self.humidity, \
self.pressure, self.ax, self.ay, self.az, self.altitude = [float(x) for x in string.split(',')]
if (self.roll > 180):
self.roll=round(self.roll - 360,1)
def pi3d_model():
from sense_hat import SenseHat
import math
import pi3d
sense = SenseHat()
display = pi3d.Display.create()
cam = pi3d.Camera.instance()
shader = pi3d.Shader("mat_light")
# .obj file is read in and x,y,z size(s) are determined
model = pi3d.Model(
file_string="apollo-soyuz.obj",
name="model", x=0, y=-1, z=40, sx=1.5, sy=1.5, sz=1.5)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 133 # This offset aligns the model with the Pi
while display.loop_running():
orientation = sense.get_orientation_radians()
if orientation is None:
pass
pitch = orientation["pitch"]
roll = orientation["roll"]
yaw = orientation["yaw"]
yaw_total = yaw + math.radians(yaw_offset)
# Maths!
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
sin_r = math.sin(roll)
cos_r = math.cos(roll)
abs_roll = math.degrees(math.asin(sin_p * cos_y + cos_p * sin_r * sin_y))
abs_pitch = math.degrees(math.asin(sin_p * sin_y - cos_p * sin_r * cos_y))
model.rotateToZ(abs_roll)
model.rotateToX(abs_pitch)
model.rotateToY(math.degrees(yaw_total))
model.draw()
keypress = keyb.read()
if keypress == 27:
keyb.close()
display.destroy()
break
elif keypress == ord('m'): # Toggles Magnetometer
compass = not compass
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('g'): # Toggles Gyroscope
gyro = not gyro
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('a'): # Toggles Accelerometer
accel = not accel
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('='): # Increases yaw offset
yaw_offset += 1
elif keypress == ord('-'): # Decreases yaw offset
yaw_offset -= 1
cam.position((10, 10, -10))
cam.point_at((0, 0, 0))
cam2d = pi3d.Camera(is_3d=False)
texture1=pi3d.Texture("worldmap.jpg")
texture2=pi3d.Texture("worldmap2.png")
texture3=pi3d.Texture("worldmap1.png")
shader = pi3d.Shader("uv_flat")
model = pi3d.Model( file_string="apollo-soyuz.obj",name="model", x=0, y=0, z=0, sx=0.5, sy=0.5, sz=0.5)
#model = pi3d.Model(file_string="models/teapot.obj",camera=cam,sx=2,sy=2,sz=2)
#model.set_draw_details(shader,[texture2,texture1])
model.set_shader(shader)
keyb = pi3d.Keyboard()
#model=pi3d.Sphere(z=0,sx=4,sy=4,sz=4)
#model=pi3d.Cuboid(4,5,6)
compass = gyro = accel = True
if sense!=0: sense.set_imu_config(compass, gyro, accel)
#yaw_offset = 72
#model.rotateToZ(-22.5)
while display.loop_running():
try:
o = sense.get_orientation_radians()
if o is None:
pass
pitch = o["pitch"]
roll = o["roll"]
yaw = o["yaw"]
except:
yaw =math.radians(-45) #around y axis
pitch =math.radians(0) #around x axis
else:
color = COLOR_BLACK
domotica.append(color)
sense_hat.set_pixels(domotica)
sleep(1)
else:
sense_hat.show_message("404")
try:
# SenseHat
sense_hat = SenseHat()
sense_hat.set_imu_config(False, False, False)
except:
print('Unable to initialize the Sense Hat library: {}'.format(
sys.exc_info()[0]))
sys.exit(1)
def main():
while True:
get_domotica_from_db()
if __name__ == "__main__":
try:
main()
except (KeyboardInterrupt, SystemExit):
import sys
sys.path.insert(1,'/home/pi/Go4Code/g4cSense/skeleton')
sys.path.insert(1,'/home/pi/Go4Code/g4cSense/graphics')
from sense_hat import SenseHat
import random
from senselib import *
import numpy as np
import time
#### 1.2 Initialisation
sense = SenseHat() # This will be used to control the the SenseHat.
initialise(sense) # Initialises the senselib library, that provides us with some useful functions
sense.set_imu_config(False, False, True) # Enable accelerometer
sense.clear() # Clears all pixels on the screen.
def acceleration(sense):
ac = sense.get_accelerometer_raw()
x = ac["x"]
y = ac["y"]
z = ac["z"]
accel = np.sqrt(x**2 + y**2 + z**2)
return accel
reading = False
accel = []
green = [[0,255,0] for i in range(64)]
sense.show_message("ready")
time.sleep(0.3)
class Accelerometer(Thread):
def __init__(self, quantum, bin_logger):
Thread.__init__(self)
self.daemon = True
self.Sense = SenseHat()
self.Sense.set_imu_config(False, False, True)
self.cnt = 0
self.quantum = quantum
self.period = 0.01
self.iters = int(self.quantum / self.period)
self.que = Queue(1024)
self.dorun = True
self.acc_bias = [0.0, 0.0, 0.0]
self.Rmtx = IDmtx
self.bin_logger = bin_logger
#
# Lambdas
#
self.l_len = lambda x: self.calcLen2d(x)
def run(self):
print("Calibrating...")
self.adjust()
print("Calibration done! adjust={}".format(self.acc_bias))
print("quantum={} period={} iters={}".format(self.quantum, self.period,
self.iters))
acc_first = self.read_acc()
while (self.dorun):
i = self.iters
sum = 0.0
acc_list = []
while (i > 0):
adj = self.adjAcc(acc_first)
acc_list.append(adj)
i = i - 1
time.sleep(self.period)
acc_first = self.read_acc()
acc_len_list = map(self.l_len, acc_list)
max = reduce(l_max, acc_len_list)
min = reduce(l_min, acc_len_list)
sum = reduce(l_sum, acc_len_list)
avg = sum / float(len(acc_list))
sdev = sqrt(
reduce(lambda x, y: x + pow(
(y - avg), 2.0), acc_len_list, 0) / (len(acc_list) - 1.0))
ruc_len_list = map(lambda x, y: (x - y) / self.period,
acc_len_list[1:], acc_len_list[:-1])
ruck_max = reduce(l_max, ruc_len_list)
ruck_min = reduce(l_min, ruc_len_list)
ruck_sum = reduce(l_sum, ruc_len_list)
ruck_avg = ruck_sum / float(len(ruc_len_list))
ruck_sdev = sdev = sqrt(
reduce(lambda x, y: x + pow(
(y - ruck_avg), 2.0), ruc_len_list, 0) /
(len(ruc_len_list) - 1.0))
ruck_tot_avg = sqrt(
pow(acc_list[0][0] - acc_list[-1][0], 2) +
pow(acc_list[0][1] - acc_list[-1][1], 2) +
pow(acc_list[0][2] - acc_list[-1][2], 2)) / self.quantum
acc_dic = {'avg': avg, 'min': min, 'max': max, 'sdev': sdev}
ruck_dic = {
'avg': ruck_avg,
'min': ruck_min,
'max': ruck_max,
'sdev': ruck_sdev,
'tot_avg': ruck_tot_avg
}
self.que.put([
dt.datetime.now().strftime('%Y%m%d %H:%M.%S.%f'), acc_dic,
ruck_dic, acc_list[0], acc_list[-1]
])
def writeBinLog(self, entry):
if (self.bin_logger == None):
return
else:
self.bin_logger.putNext(
[dt.datetime.now().strftime('%Y%m%d %H:%M.%S.%f'), entry])
def adjust(self):
ac = 0.0
x = 0.0
y = 0.0
z = 0.0
for i in range(0, 100):
rd = self.read_acc()
x += rd[0]
y += rd[1]
z += rd[2]
time.sleep(0.001)
self.acc_bias = [x / 100.0, y / 100.0, z / 100.0]
def read_acc(self):
a1 = self.Sense.get_accelerometer_raw()
self.writeBinLog(a1)
return [a1['x'], a1['y'], a1['z']]
def get(self):
return [self.get_x(), self.get_y(), self.get_z()]
def calcLen(self, acc):
return sqrt(pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2))
def calcLen2d(self, acc):
return sqrt(pow(acc[0], 2) + pow(acc[1], 2))
def adjAcc(self, acc):
return [
acc[0] - self.acc_bias[0], acc[1] - self.acc_bias[1],
acc[0] + (Gdelta - self.acc_bias[0])
]
def stopit(self):
self.dorun = False
def getNext(self):
try:
return self.que.get(True, timeout=15)
except:
print("Exception")
def read_queue(self):
yield self.que.get(True, None)
from sense_hat import SenseHat
from time import sleep
sense = SenseHat()
sense.set_imu_config(True,True,True) # all functions on
orientation_rad = sense.get_orientation_radians()
direction = sense.orientation_radians
print("Direction: %s radians" % direction)
sense.show_message("Direction: %s radians" % direction, scroll_speed=.05)
temp = sense.get_temperature()
print("Temperature: %s C" % temp)
tempf = (9.0/5.0 * temp) + 32
print("Temperature: %s F" % tempf)
humidity = sense.get_humidity()
print("Humidity: %s %%rH" % humidity)
sense.show_message("Temperature: %s C" % temp)
class PiSenseHat(object):
"""Raspberry Pi 'IoT Sense Hat API Driver Class'."""
# Constructor
def __init__(self):
self.sense = SenseHat()
# enable all IMU functions
self.sense.set_imu_config(True, True, True)
# Pressure
def getPressure(self):
return self.sense.get_pressure()
# Temperature
def getTemperature(self):
return self.sense.get_temperature()
# Humidity
def getHumidity(self):
return self.sense.get_humidity()
def getHumidityTemperature(self):
return self.sense.get_temperature_from_humidity()
def getPressureTemperature(self):
return self.sense.get_temperature_from_pressure()
def getOrientationRadians(self):
return self.sense.get_orientation_radians()
def getOrientationDegrees(self):
return self.sense.get_orientation_degrees()
# degrees from North
def getCompass(self):
return self.sense.get_compass()
def getAccelerometer(self):
return self.sense.get_accelerometer_raw()
def getEnvironmental(self):
sensors = {'name' : 'sense-hat', 'environmental':{}}
return sensors
def getJoystick(self):
sensors = {'name' : 'sense-hat', 'joystick':{}}
return sensors
def getInertial(self):
sensors = {'name' : 'sense-hat', 'inertial':{}}
def getAllSensors(self):
sensors = {'name' : 'sense-hat', 'environmental':{}, 'inertial':{}, 'joystick':{}}
sensors['environmental']['pressure'] = { 'value':self.sense.get_pressure(), 'unit':'mbar'}
sensors['environmental']['temperature'] = { 'value':self.sense.get_temperature(), 'unit':'C'}
sensors['environmental']['humidity'] = { 'value':self.sense.get_humidity(), 'unit': '%RH'}
accel = self.sense.get_accelerometer_raw()
sensors['inertial']['accelerometer'] = { 'x':accel['x'], 'y':accel['y'], 'z': accel['z'], 'unit':'g'}
orientation = self.sense.get_orientation_degrees()
sensors['inertial']['orientation'] = { 'compass':self.sense.get_compass(), 'pitch':orientation['pitch'], 'roll':orientation['roll'], 'yaw': orientation['yaw'], 'unit':'degrees'}
return sensors
import sys
import asyncio
from sense_hat import SenseHat
import time
async def waitforthat(future):
# await asyncio.sleep(0.02)
future.set_result("ok")
async def getcompass():
print(sense.get_compass_raw())
sense = SenseHat()
sense.set_imu_config(True, False, False) #only compass active
t = int(round(time.time() * 1000))
while True:
loop = asyncio.get_event_loop()
future = asyncio.Future()
asyncio.ensure_future(waitforthat(future))
t_prev = t
t = int(round(time.time() * 1000))
loop.run_until_complete(getcompass())
loop.run_until_complete(future)
print(t - t_prev)
loop.close()
# Wait before taking another measurement
sleep(delay)
mean = find_mean(recorded_data)
# Get the ambient temperature in degrees C
temp = hat.get_temperature()
# Flag to use the ambient temp as the magnet temp
use_ambient = False
mag_temp = input_mag_temp()
write_to_file(temp, mag_temp, recorded_data, mean)
hat = SenseHat()
#Configure IMU so only the compass is active
hat.set_imu_config(True, False, False)
# Display blue W when waiting for button press to start recording
hat.show_letter('W', [0, 0, 255])
# Once any joystick button press is received start recording
event = hat.stick.wait_for_event()
if event.action == "pressed":
record_data(hat, 0.5, debug_flag)
hat.clear()
from sense_hat import SenseHat
from time import sleep
import math
sense = SenseHat()
sense.set_imu_config(False, False, True)
#start by setting a "level" state
base_level=sense.get_orientation_degrees()
base_pitch=base_level['pitch']
base_roll=base_level['roll']
#base_yaw=base_level['yaw']
bx=3
by=4
diffV=1
X=[255,100,0]
S=[0,0,0]
field=[S]*64
while(True):
orien = sense.get_orientation_degrees()
pdiff=orien['pitch']-base_pitch
rdiff=orien['roll']-base_roll
print '{} {} {}'.format(orien['pitch'],base_pitch,pdiff)
print '{} {} {}'.format(orien['roll'],base_roll,rdiff)
#print '{} {}'.format(orien['yaw'],base_yaw)
print'######################################'
if(pdiff> diffV):
if(bx>0):
bx-=1
elif(pdiff < -diffV):
if(bx<7):
bx+=1
if(rdiff >diffV):
#!/usr/bin/python3
from sense_hat import SenseHat
import time
sense = SenseHat()
sense.set_imu_config(True, True, True) #turn IMU sensors on
def door_state():
#global orientation
state = 0
while True:
orientation = sense.get_orientation()
if orientation["roll"] > 355.0 or orientation["roll"] < 5.0:
state = 0
else:
state = 1
print(state)
time.sleep(.1)
if __name__ == "__main__":
door_state()
class sensors(Thread):
def __init__(self):
Thread.__init__(self)
self.daemon=True
self.pitch = 0
self.roll = 0
self.yaw = 0
self.compass= 10
self.temp = 0
self.humidity = 0
self.pressure = 0
self.ax = 0
self.ay = 0
self.az = 0
self.altitude = 0
# Comment if not running on RPI
self.sense = SenseHat()
self.sense.clear()
self.sense.set_imu_config(True, True, True)
def joinDelimiter(self, arr):
tmp=[None]*len(arr)
for i in range(len(arr)):
tmp[i]=str(arr[i])
return ",".join(tmp)
def getRandomStrArr(self):
pitch = r.randint(3, 5)
roll = r.randint(3, 5)
yaw = r.randint(0, 2)
compass = r.randint(240, 241)
temp = r.randint(19, 20)
humidity = r.randint(43, 46)
pressure = r.randint(983, 985)
ax = 0.1
ay = 0.1
az = 0.1
altitude = 286
return self.joinDelimiter([pitch, roll, yaw, compass, temp, humidity, pressure, ax, ay, az, altitude])
def run(self):
while True:
self.temp = round(self.sense.get_temperature(), 1)
self.humidity = round(self.sense.get_humidity(), 1)
self.pressure = round(self.sense.get_pressure(), 2)
self.sense.set_imu_config(True, True, True)
pitch, yaw, roll = self.sense.get_orientation().values()
ax, ay, az = self.sense.get_accelerometer_raw().values()
self.compass = round(self.sense.get_compass(), 2)
self.pitch = round(pitch, 2)
self.roll = round(roll, 2)
if (self.pitch > 180):
self.pitch -= 360
if (self.roll > 180):
self.roll -= 360
self.yaw = round(yaw, 2)
self.ax = round(ax, 2)
self.ay = round(ay, 2)
self.az = round(az, 2)
self.altitude = round((288.15 / -0.0065) * ((self.pressure * 100 / 101325) ** (-(8.31432 * -0.0065) / (9.80665 * 0.0289644)) - 1),2)
"""
self.pitch = r.randint(3, 5)
self.roll = r.randint(3, 5)
self.yaw = r.randint(0, 2)
self.compass = r.randint(240, 241)
self.temp = r.randint(19, 20)
self.humidity = r.randint(43, 46)
self.pressure = r.randint(983, 985)
self.ax = 0.1
self.ay = 0.1
self.az = 0.1
self.altitude = 286
"""
time.sleep(REFRESH_DELAY)
def getStrArr(self):
return self.joinDelimiter([self.pitch, self.roll, self.yaw, self.compass, self.temp, self.humidity, self.pressure, self.ax, self.ay,
self.az, self.altitude])
def getTuple(self):
return (self.getStrArr(),'')
class Host(object):
"""Main class used by the ROS node supporting dialogue between the ROS framework and the Sense HAT hardware.
:param rotation: control Sense HAT LED matrix orientation. Defaults to 0.
:type rotation: int, optional
:param low_light: enable Sense HAT low-light mode. Defaults to False.
:type low_light: bool, optional
:param calibration: linear fixing for environmental readings (ex. {"humidity": -20.0} ). Defaults to {}.
:type calibration: dict, optional
:param smoothing: number of observations to be used for median smoothing (0 = smoothing disabled). Smoothing is applied only to environmental data (humidity, temperature and pressure). Defaults to 0.
:type smoothing: int, optional
:param register_services: control ROS services registration. Defaults to True.
:type register_services: bool, optional
:param environmental_publishing: enable automatic publishing of environmental data (rate controlled by environmental_publishing_rate). Defaults to True.
:type environmental_publishing: bool, optional
:param environmental_publishing_rate: environmental data publication frequency in seconds. Defaults to 12 (5 times a minute).
:type environmental_publishing_rate: float, optional
:param imu_publishing: enable automatic publishing of IMU data (rate controlled by imu_publishing_rate). Defaults to False.
:type imu_publishing: bool, optional
:param imu_publishing_mode: specify the Sense HAT API function to be used to get x,y,z/roll,pitch,yaw. Valid values are: get_orientation_radians_rpy, get_orientation_degrees_rpy, get_compass_raw_xyz, get_gyroscope_rpy, get_gyroscope_raw_xyz, get_accelerometer_rpy, get_compass_raw_xyz. Defaults to get_orientation_degrees_rpy.
:type imu_publishing_mode: string, optional
:param imu_publishing_rate: IMU data publication frequency in seconds. Defaults to 1 (once a second).
:type imu_publishing_rate: float, optional
:param stick_publishing: enable automatic publishing of stick events. Defaults to True.
:type stick_publishing: bool, optional
:param stick_sampling: indicates how frequently the Stick is checked for new events. Defaults to 0.2 (5 times a second).
:type stick_sampling: float, optional
"""
def __init__(self,
rotation=0,
low_light=False,
calibration={},
smoothing=0,
register_services=True,
environmental_publishing=True,
environmental_publishing_rate=12,
imu_publishing=False,
imu_publishing_mode="get_orientation_degrees_rpy",
imu_publishing_config="1|1|1",
imu_publishing_rate=1,
stick_publishing=True,
stick_sampling=0.2):
"""Constructor method"""
# Init sensor
self.sense = SenseHat()
self.sense.clear(0, 0, 0)
self.sense.set_rotation(rotation)
self.sense.low_light = low_light
self.measures = {
'humidity': self.sense.get_humidity,
'temperature_from_humidity':
self.sense.get_temperature_from_humidity,
'temperature_from_pressure':
self.sense.get_temperature_from_pressure,
'pressure': self.sense.get_pressure,
'compass': self.sense.get_compass,
}
self.imu_modes = {
'get_orientation_radians_rpy': self.sense.get_orientation_radians,
'get_orientation_degrees_rpy': self.sense.get_orientation_degrees,
'get_compass_raw_xyz': self.sense.get_compass_raw,
'get_gyroscope_rpy': self.sense.get_gyroscope,
'get_gyroscope_raw_xyz': self.sense.get_gyroscope_raw,
'get_accelerometer_rpy': self.sense.get_accelerometer,
'get_accelerometer_raw_xyz': self.sense.get_accelerometer_raw,
}
# Init parameters
self.stick_publishing = stick_publishing
self.environmental_publishing = environmental_publishing
self.imu_publishing = imu_publishing
self.imu_publishing_mode = imu_publishing_mode
self.stick_sampling = stick_sampling
self.environmental_publishing_rate = environmental_publishing_rate
self.imu_publishing_rate = imu_publishing_rate
self.calibration = calibration
self.register_services = register_services
self.smoothing = smoothing
self.history = {}
for measure in self.measures:
self.history[measure] = collections.deque(
[], maxlen=smoothing
if smoothing > 0 else None) if smoothing >= 0 else None
# Init Lock to serialize access to the LED Matrix
self.display_lock = Lock()
# Register publishers
if self.stick_publishing:
self.stick_pub = rospy.Publisher("Stick", Stick, queue_size=10)
if self.environmental_publishing:
self.environmental_pub = rospy.Publisher("Environmental",
Environmental,
queue_size=10)
if self.imu_publishing:
self.sense.set_imu_config(
*[i == "1" for i in imu_publishing_config.split("|")])
self.imu_pub = rospy.Publisher("IMU", IMU, queue_size=10)
# Register services
if self.register_services:
self.getEnvironmentalService = rospy.Service(
"GetEnvironmental", GetEnvironmental, self.getEnvironmental)
self.getHumidityService = rospy.Service("GetHumidity", GetHumidity,
self.getHumidity)
self.getTemperatureService = rospy.Service("GetTemperature",
GetTemperature,
self.getTemperature)
self.getPressureService = rospy.Service("GetPressure", GetPressure,
self.getPressure)
self.getIMUService = rospy.Service("GetIMU", GetIMU, self.getIMU)
self.getCompassService = rospy.Service("GetCompass", GetCompass,
self.getCompass)
self.emulateStick = rospy.Service("EmulateStick", EmulateStick,
self.emulateStick)
self.clearService = rospy.Service("Clear", Clear, self.clear)
self.setPixelsService = rospy.Service("SetPixels", SetPixels,
self.setPixels)
self.switchLowLightService = rospy.Service("SwitchLowLight",
SwitchLowLight,
self.switchLowLight)
self.showLetterService = rospy.Service("ShowLetter", ShowLetter,
self.showLetter)
self.showMessageService = rospy.Service("ShowMessage", ShowMessage,
self.showMessage)
rospy.loginfo("sensehat_ros initialized")
def __del__(self):
if self.sense:
self.sense.clear(0, 0, 0)
rospy.loginfo("sensehat_ros destroyed")
##############################################################################
# Private methods
##############################################################################
def _get_environmental(self, timestamp):
msg = Environmental()
msg.header.stamp = timestamp
msg.humidity = self._get_measure('humidity')
msg.temperature_from_humidity = self._get_measure(
'temperature_from_humidity')
msg.temperature_from_pressure = self._get_measure(
'temperature_from_pressure')
msg.pressure = self._get_measure('pressure')
return msg
def _get_imu(self, mode, timestamp):
msg = IMU()
msg.header.stamp = timestamp
msg.mode = mode
imu = self.imu_modes[mode]()
if mode[-3:] == 'xyz':
msg.x, msg.y, msg.z = imu['x'], imu['y'], imu['z']
elif mode[-3:] == 'rpy':
msg.x, msg.y, msg.z = imu['roll'], imu['pitch'], imu['yaw']
return msg
def _get_measure(self, measure, disableSmooting=False):
def _get_measure_median(smoothing):
"""Return median value from the last <smoothing> elements in the history
Args:
smoothing (int): values to be extracted from the history
Returns:
double: median value
"""
lst = [
history[-i] for i in range(1, 1 + min(smoothing, len(history)))
]
n = len(lst)
s = sorted(lst)
return (sum(s[n // 2 - 1:n // 2 + 1]) / 2.0,
s[n // 2])[n % 2] if n else None
if not measure in self.measures:
raise ValueError('Invalid measure %s' % measure)
val = self.measures[measure]() + self.calibration.get(measure, 0.0)
history = self.history[measure]
if history is not None:
history.append(val)
if self.smoothing > 0 and not disableSmooting:
val = _get_measure_median(self.smoothing)
return val
def _publish_stick(self, event):
for stick_event in self.sense.stick.get_events():
stick = Stick(direction=stick_event.direction,
action=stick_event.action)
rospy.logdebug("Publishing Stick (D: %s, A: %s)", stick.direction,
stick.action)
self.stick_pub.publish(stick)
def _publish_environmental(self, event):
environmental = self._get_environmental(rospy.Time.now())
rospy.logdebug(
"Publishing Environmental (H: %s, TH: %s, TP: %s, P: %s)",
environmental.humidity, environmental.temperature_from_humidity,
environmental.temperature_from_pressure, environmental.pressure)
self.environmental_pub.publish(environmental)
def _publish_imu(self, event):
imu = self._get_imu(self.imu_publishing_mode, rospy.Time.now())
rospy.logdebug(
"Publishing IMU (Mode: %s, X: %s, Y: %s, Z: %s)",
imu.mode,
imu.x,
imu.y,
imu.z,
)
self.imu_pub.publish(imu)
##############################################################################
# ROS service methods
##############################################################################
def clear(self, req):
"""ROS service: sets all of the 64 LED matrix pixels to the specified RGB color and waits for the specified amount of seconds"""
self.display_lock.acquire()
try:
self.sense.clear(req.colour)
rospy.sleep(req.duration)
finally:
self.display_lock.release()
return ClearResponse()
def switchLowLight(self, req):
"""ROS service: switches on/off the LED matrix \"low light\" mode and returns the current state."""
self.sense.low_light = not self.sense.low_light
return SwitchLowLightResponse(low_light=self.sense.low_light)
def setPixels(self, req):
"""ROS service: sets each of the 64 LED matrix pixels to a specific RGB color and waits for the specified amount of seconds."""
self.display_lock.acquire()
try:
self.sense.set_pixels(
list(zip(req.pixels_red, req.pixels_green, req.pixels_blue)))
rospy.sleep(req.duration)
finally:
self.display_lock.release()
return SetPixelsResponse()
def showLetter(self, req):
"""ROS service: shows a letter on the LED matrix and waits for the specified amount of seconds."""
self.display_lock.acquire()
try:
self.sense.show_letter(req.text, req.text_colour, req.back_colour)
rospy.sleep(req.duration)
finally:
self.display_lock.release()
return ShowLetterResponse()
def showMessage(self, req):
"""ROS service: scrolls a text message from right to left across the LED matrix and at the specified speed, in the specified colour and background colour."""
self.display_lock.acquire()
try:
self.sense.show_message(req.text, req.scroll_speed,
req.text_colour, req.back_colour)
finally:
self.display_lock.release()
return ShowMessageResponse()
def getIMU(self, req):
"""ROS service: queries Sense HAT IMU sensor. Warning: not allowed when imu_publishing=True due to potential set_imu_config interference."""
if self.imu_publishing:
raise RuntimeError(
"Method getIMU not allowed when imu_publishing=True (due to potential set_imu_config interference)"
)
imu = self._get_imu(req.mode, rospy.Time.now())
rospy.logdebug(
"Sending IMU (Mode: %s, X: %s, Y: %s, Z: %s)",
imu.mode,
imu.x,
imu.y,
imu.z,
)
return imu
def getCompass(self, req):
"""ROS service: gets the direction of North from the magnetometer in degrees. Warning: not allowed when imu_publishing=True due to potential set_imu_config interference."""
if self.imu_publishing:
raise RuntimeError(
"Method getCompass not allowed when imu_publishing=True due to potential set_imu_config interference"
)
compass = self._get_measure('compass', disableSmooting=True)
rospy.logdebug("Sending Compass (Compass: %s)", compass)
return compass
def getHumidity(self, req):
"""ROS service: gets the percentage of relative humidity from the humidity sensor."""
humidity = self._get_measure('humidity')
rospy.logdebug("Sending Humidity (H: %s)", humidity)
return humidity
def getTemperature(self, req):
"""ROS service: gets the current temperature in degrees Celsius from the humidity sensor."""
temperature = self._get_measure('temperature_from_humidity')
rospy.logdebug("Sending Temperature (TH: %s)", temperature)
return temperature
def getPressure(self, req):
"""ROS service: gets the current pressure in Millibars from the pressure sensor."""
pressure = self._get_measure('pressure')
rospy.logdebug("Sending Pressure (P: %s)", pressure)
return pressure
def getEnvironmental(self, req):
"""ROS service: gets the current humidity and temperature from the humidity sensor and temperature and pressure from the pressure sensor."""
environmental = self._get_environmental(rospy.Time.now())
rospy.logdebug("Sending Environmental (H: %s, TH: %s, TP: %s, P: %s)",
environmental.humidity,
environmental.temperature_from_humidity,
environmental.temperature_from_pressure,
environmental.pressure)
return environmental
def emulateStick(self, req):
"""ROS service: remotely triggers a stick event without interacting with the Stick device."""
if self.stick_publishing:
rospy.loginfo("Emulating Stick (D: %s, A: %s)", req.direction,
req.action)
stick = Stick(direction=req.direction, action=req.action)
self.stick_pub.publish(stick)
return EmulateStickResponse()
##############################################################################
# Run
##############################################################################
def run(self):
"""Starts configured data publishing (stick, environmental, IMU) and spins waiting for service calls. Triggered by the ROS node after initialization."""
if self.stick_publishing:
# Clear buffer
self.sense.stick.get_events()
# Start publishing events
rospy.Timer(rospy.Duration(self.stick_sampling),
self._publish_stick)
rospy.loginfo("sensehat_ros publishing stick")
if self.environmental_publishing:
# Start publishing events
rospy.Timer(rospy.Duration(self.environmental_publishing_rate),
self._publish_environmental)
rospy.loginfo("sensehat_ros publishing environmental")
if self.imu_publishing:
# Start publishing events
rospy.Timer(rospy.Duration(self.imu_publishing_rate),
self._publish_imu)
rospy.loginfo("sensehat_ros publishing IMU")
rospy.spin()
def collectData(self):
self.tf = datetime.time.second
gps_socket = gps3.GPSDSocket()
data_stream = gps3.DataStream()
gps_socket.connect()
gps_socket.watch()
sense = SenseHat()
sense.set_imu_config(True, True, True)
self.createFile()
while self.ON:
'''Collect Sense Hat Data'''
timestamp = datetime.datetime.now() #Date + timestamp
self.timestr = timestamp.strftime('%H:%M:%S.%f') #pretty format timestamp
self.gpstimestr = timestamp.strftime('%H:%M:%S') #pretty format for GPS
self.tf = time.time()
self.dt = self.tf - self.ti
self.ti = self.tf
orientation = sense.get_orientation_degrees()
gyroS = ('{pitch},{roll},{yaw}').format(**orientation)
if (orientation['pitch'] == 'n/a'):
self.pitch = 'n/a'
else:
self.pitch = '{0:.6f}'.format(float(orientation['pitch']))
if (orientation['roll'] == 'n/a'):
self.roll = 'n/a'
else:
self.roll = '{0:.6f}'.format(float(orientation['roll']))
if (orientation['yaw'] == 'n/a'):
self.yaw = 'n/a'
else:
self.yaw = '{0:.6f}'.format(float(orientation['yaw']))
self.rollrate = (float(self.roll) - float(self.oldroll)) / self.dt
self.yawrate = (float(self.yaw) - float(self.oldyaw)) / self.dt
self.pitchrate = (float(self.pitch) - float(self.oldpitch)) / self.dt
self.oldroll = self.roll
self.oldpitch = self.pitch
self.oldyaw = self.yaw
self.temp = '{0:.6f}'.format(sense.temp)
self.humi = '{0:.6f}'.format(sense.humidity)
self.pres = '{0:.6f}'.format(sense.pressure)
#collect GPS Data
for new_data in gps_socket:
if new_data:
#print colored(new_data,"green")
data_stream.unpack(new_data)
if (data_stream.TPV['alt'] == 'n/a'):
self.alt = ''
else:
self.alt = '{0:.6f}'.format(float(data_stream.TPV['alt']))
if (data_stream.TPV['lat'] == 'n/a'):
self.lat = ''
else:
self.lat = '{0:.6f}'.format(float(data_stream.TPV['lat']))
if (data_stream.TPV['lon'] == 'n/a'):
self.lon = ''
else:
self.lon = '{0:.6f}'.format(float(data_stream.TPV['lon']))
else:
print colored("No gps data", 'red')
break
if (self.supressOutput != True):
print("Time Stamp: " + str(self.timestr))
print(colored(('Roll: ' + str(self.roll)), 'magenta'))
print(colored(('Pitch: ' + str(self.pitch)), 'magenta'))
print(colored(('Yaw: ' + str(self.yaw)), 'magenta'))
print(colored(('Roll Rate: ' + str(self.rollrate)), 'cyan'))
print(colored(('Pitch Rate: ' + str(self.pitchrate)), 'cyan'))
print(colored(('YawRate: ' + str(self.yawrate)), 'cyan'))
print("Temperature: " + self.temp + " C")
print("Humidity: " + self.humi + " rh")
print("Pressure: " + self.pres + " Mb")
print(colored(('Altitude: ', self.alt), 'yellow'))
print(colored(('Latitude: ', self.lat), 'yellow'))
print(colored(('Longitude: ',self.lon), 'yellow'))
self.write2File()
print self.timestr
time.sleep(1/self.RATE) #currently set to 10 Hz
self.f2.close()
print self.datacsv + " closed."
def get_compass_raw():
sense = SenseHat()
sense.set_imu_config(True, False, False) # Compass only
return sense.get_compass_raw()
name="model",
x=original_pos_x,
y=original_pos_y,
z=original_pos_y,
sx=1,
sy=1,
sz=1)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 72
while display.loop_running():
o = sense.get_orientation_radians()
accraw = sense.get_accelerometer_raw()
acc_x = accraw["x"] * 2.0
acc_y = accraw["y"] * 2.0
acc_z = accraw["z"] * 2.0
if o is None:
pass
pitch = o["pitch"]
roll = o["roll"]
def get_gyroscope_raw():
sense = SenseHat()
sense.set_imu_config(False, True, False) # Gyroscope only
return sense.get_gyroscope_raw()
from sense_hat import SenseHat sense = SenseHat() sense.set_imu_config(True, True, True) # enables all
def get_accelerometer_raw():
sense = SenseHat()
sense.set_imu_config(False, False, True) # Accelerometer only
return sense.get_accelerometer_raw()
original_pos_x = 0
original_pos_y = -1
original_pos_z = 40
model = pi3d.Model(
file_string="apollo-soyuz.obj",
name="model", x=original_pos_x, y=original_pos_y, z=original_pos_y, sx=1, sy=1, sz=1)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 72
while display.loop_running():
o = sense.get_orientation_radians()
accraw = sense.get_accelerometer_raw()
acc_x = accraw["x"]* 2.0
acc_y = accraw["y"]* 2.0
acc_z = accraw["z"]* 2.0
if o is None:
pass
pitch = o["pitch"]
roll = o["roll"]
class Roll:
def __init__(self, layout: Layout, dim=8):
self.dim = dim
# prep hat
self.hat = SenseHat()
self.hat.clear()
self.hat.set_imu_config(False, True, False)
# set maze colors
self.ball_color = (255, 0, 0)
self.target_color = (0, 255, 0)
self.background_color = (0, 0, 0)
self.wall_color = (255, 255, 255)
# set the layout of the maze
self.layout = layout
self.set_layout()
def set_layout(self):
for i in range(self.dim):
for j in range(self.dim):
if self.layout.arr[i][j] == 1:
self.hat.set_pixel(i, j, self.wall_color)
# create ball
self.x = self.layout.start.x
self.y = self.layout.start.y
self.hat.set_pixel(self.x, self.y, self.ball_color)
# create target
self.target_x = self.layout.end.x
self.target_y = self.layout.end.y
self.hat.set_pixel(self.target_x, self.target_y, self.target_color)
# is ball == target?
self.done = False
def celebrate_win(self):
for _ in range(10):
self.hat.set_pixels(Special.random())
time.sleep(0.25)
self.hat.set_pixels(Face.happy)
time.sleep(1)
self.hat.set_pixels(Face.wink_left)
time.sleep(0.3)
self.hat.set_pixels(Face.happy)
time.sleep(1)
def move_ball(self, roll, pitch):
self.hat.set_pixel(self.x, self.y, self.background_color)
if roll < 0:
if self.y - 1 >= 0:
if self.layout.arr[self.x][self.y - 1] == 0:
self.y -= 1
elif roll > 0:
if self.y + 1 < self.dim:
if self.layout.arr[self.x][self.y + 1] == 0:
self.y += 1
if pitch > 0:
if self.x - 1 >= 0:
if self.layout.arr[self.x - 1][self.y] == 0:
self.x -= 1
elif pitch < 0:
if self.x + 1 < self.dim:
if self.layout.arr[self.x + 1][self.y] == 0:
self.x += 1
self.hat.set_pixel(self.x, self.y, self.ball_color)
def run(self):
while self.done is False:
o = self.hat.get_orientation_radians()
roll = o['roll']
pitch = o['pitch']
if abs(roll) > 0.3 or abs(pitch) > 0.3:
epsilon = 0.1
else:
epsilon = 0.3
self.move_ball(roll=roll, pitch=pitch)
if self.x == self.target_x and self.y == self.target_y:
self.done = True
time.sleep(epsilon)
self.celebrate_win()
class PiSenseHat(object):
"""Raspberry Pi 'IoT Sense Hat API Driver Class'."""
# Constructor
def __init__(self):
self.sense = SenseHat()
# enable all IMU functions
self.sense.set_imu_config(True, True, True)
# pixel display
def set_pixel(self, x, y, color):
# red = (255, 0, 0)
# green = (0, 255, 0)
# blue = (0, 0, 255)
self.sense.set_pixel(x, y, color)
# clear pixel display
def clear_display(self):
self.sense.clear()
# Pressure
def getPressure(self):
return self.sense.get_pressure()
# Temperature
def getTemperature(self):
return self.sense.get_temperature()
# Humidity
def getHumidity(self):
return self.sense.get_humidity()
def getHumidityTemperature(self):
return self.sense.get_temperature_from_humidity()
def getPressureTemperature(self):
return self.sense.get_temperature_from_pressure()
def getOrientationRadians(self):
return self.sense.get_orientation_radians()
def getOrientationDegrees(self):
return self.sense.get_orientation_degrees()
# degrees from North
def getCompass(self):
return self.sense.get_compass()
def getAccelerometer(self):
return self.sense.get_accelerometer_raw()
def getEnvironmental(self):
sensors = {'name': 'sense-hat', 'environmental': {}}
return sensors
def getJoystick(self):
sensors = {'name': 'sense-hat', 'joystick': {}}
return sensors
def getInertial(self):
sensors = {'name': 'sense-hat', 'inertial': {}}
def getAllSensors(self):
sensors = {
'name': 'sense-hat',
'environmental': {},
'inertial': {},
'joystick': {},
'location': {}
} # add location
sensors['environmental']['pressure'] = {
'value': self.sense.get_pressure(),
'unit': 'mbar'
}
sensors['environmental']['temperature'] = {
'value': self.sense.get_temperature(),
'unit': 'C'
}
sensors['environmental']['humidity'] = {
'value': self.sense.get_humidity(),
'unit': '%RH'
}
accel = self.sense.get_accelerometer_raw()
sensors['inertial']['accelerometer'] = {
'x': accel['x'],
'y': accel['y'],
'z': accel['z'],
'unit': 'g'
}
orientation = self.sense.get_orientation_degrees()
sensors['inertial']['orientation'] = {
'compass': self.sense.get_compass(),
'pitch': orientation['pitch'],
'roll': orientation['roll'],
'yaw': orientation['yaw'],
'unit': 'degrees'
}
sensors['location']['lat'] = {
'value': 0
} # initialize these with 0 values to start
sensors['location']['lon'] = {'value': 0}
sensors['location']['alt'] = {'value': 0}
sensors['location']['sats'] = {'value': 0}
sensors['location']['speed'] = {'value': 0}
return sensors
from sense_hat import SenseHat
import psycopg2
import numpy as np
import time
sense = SenseHat()
sense.set_imu_config(True, False, False) # compass, not gyro, not accel
database = "watermonitor"
try:
try:
conn = psycopg2.connect(user="******",
password="******",
host="127.0.0.1",
port="5432",
database=database)
except Exception:
message = "Error db conn"
raise
while True:
# time.sleep(0.02) # already a lag of 0.02s without sleep
xyz = sense.get_compass_raw() # get values in microteslas
# get timestamp in ms
timestamp = int(round(time.time() * 1000))
# get norm of compass xyz values
value = np.linalg.norm([xyz["x"], xyz["y"], xyz["z"]])
try:
curs = conn.cursor()
#!/usr/bin/python3
# Finding Values from sensor and Writes to a JSON file.
from sense_hat import SenseHat
import json
sense = SenseHat()
humidity = sense.get_humidity()
pressure = sense.get_pressure()
temperature = sense.get_temperature()
sense.set_imu_config(True, False, False)
north = sense.get_compass()
sense.set_imu_config(False, True, False)
gyro = sense.get_gyroscope()
sense.set_imu_config(False, False, True)
accel = sense.get_accelerometer()
sensor_dict = {
"Humidity": humidity,
"Pressure": pressure,
"Temperature": temperature,
"North": north,
"Gyroscope": gyro,
"Accelerometer": accel
}
with open('Sensor_out.json', "w") as f:
json.dump(sensor_dict, f, indent=4)
print("Done")
class DataWrite:
def __init__(self):
self.sense = SenseHat()
self.sense.set_imu_config(True, True, True)
def get_data(self):
"""Gets data from environmental sensors and IMU sensors
and formats it for writing to a CSV with time as the first item
"""
# get environmental data from the sensehat
def get_enviro():
"""Gets environmental data and formats it in the form:
pressure, temperature_pressure, temperature_humidity, humidity
"""
# Get readings from each sensor
pressure = self.sense.get_pressure()
temp_press = self.sense.get_temperature_from_pressure()
temp_humid = self.sense.get_temperature_from_humidity()
humidity = self.sense.get_humidity()
# Format the readings
enviro_results = [pressure, temp_press, temp_humid, humidity]
return enviro_results
# get IMU data from the sensehat
def get_imu():
"""Gets IMU data and formats it in the form:
accelX, accelY, accelZ, gyroX, gyroY, gyroZ, compassX, compassY, compassZ, orientationX, orientationY,
orientationZ
"""
# get raw data from IMU sensors
accelraw = self.sense.get_accelerometer_raw()
gyroraw = self.sense.get_gyroscope_raw()
compassraw = self.sense.get_compass_raw()
orientationraw = self.sense.get_orientation_degrees()
# Format raw data into a usable list
imu_results = [
accelraw['x'], accelraw['y'], accelraw['z'], gyroraw['x'],
gyroraw['y'], gyroraw['z'], compassraw['x'], compassraw['y'],
compassraw['z'], orientationraw['pitch'],
orientationraw['roll'], orientationraw['yaw']
]
return imu_results
# Get data from sensors and add time then append together
enviro_res = get_enviro()
imu_res = get_imu()
current_time = datetime.datetime.now().strftime(
"%d-%b-%Y (%H:%M:%S.%f)")
results = [current_time]
results.extend(enviro_res)
results.extend(imu_res)
print(results)
return results
def write_data(self):
"""Writes data to data.csv in append mode as to not delete headers or previous data"""
with open('data.csv', 'a') as f:
writer = csv.writer(f)
writer.writerow(self.get_data())
sense = SenseHat()
DIGITS = '0'
REFRESHRATE = 4.0 # Measures per second
MeasureIsRunning = False
# Main Loop
try:
logger = logging.getLogger('myapp')
hdlr = logging.FileHandler('/var/tmp/myapp.log')
formatter = logging.Formatter('%(asctime)s %(levelname)s %(message)s')
hdlr.setFormatter(formatter)
logger.addHandler(hdlr)
logger.setLevel(logging.INFO)
sense.set_imu_config(True, True, True)
sense.set_pixel(0, 0, [255, 0, 0])
while (MeasureIsRunning is False):
# Use joystick for starting Measurement
for event in sense.stick.get_events():
if (event.action == 'pressed' and event.direction == 'middle'):
MeasureIsRunning = True
while (MeasureIsRunning is True):
sense.set_pixel(0, 0, [0, 255, 0])
dictValues = sense.get_gyroscope()
strMessage = "Gyr:"
strMessage += " Pitch: {0:.{digits}f}°".format(dictValues.get('pitch'),
digits=DIGITS)
strMessage += " Roll: {0:.{digits}f}°".format(dictValues.get('roll'),
if abs(y) < 20:
y = 0
print(x, y)
return x, y
def envoi(donnees, client):
msg = str(donnees[0]) + "," + str(donnees[1])
r = client.publish(TOPIC, msg)
print("\t" + ("envoyé" if r[0] == 0 else "echec"))
if __name__ == "__main__":
clientMQTT = mqtt_client.Client(client_id=TOPIC + "Pub")
try:
clientMQTT.connect(host=IP, port=PORT, keepalive=ALIVE)
sense = SenseHat()
sense.set_imu_config(False, False,
True) # magnetometre, gyroscope, accelerometer
while True:
x, y = recupDonneeCapteur(sense)
envoi((x, y), clientMQTT)
sleep(0.1)
except KeyboardInterrupt:
pass
finally:
r = clientMQTT.publish(TOPIC, "fin")
print("\t" + ("envoyé" if r[0] == 0 else "echec"))
clientMQTT.disconnect()
from sense_hat import SenseHat
from sense_hat import SenseHat, ACTION_PRESSED, ACTION_HELD, ACTION_RELEASED
from time import sleep
import sys
import random
sense = SenseHat()
sense.set_imu_config(False, False, False)
def main():
sense.show_message("press up")
while True: # programma loopt zolang niet afgebroken
color = 255, 0, 0
timeout = 0.5
def square1():
sense.set_pixel(3, 3, color)
sense.set_pixel(4, 4, color)
sense.set_pixel(3, 4, color)
sense.set_pixel(4, 3, color)
sleep(timeout)
refresh()
square2()
def square2():
a = 5
b = 2
#! /usr/bin/env python
from sense_hat import SenseHat
from time import sleep
from math import *
sh = SenseHat()
sh.set_imu_config (True, True, True) # gyroscope only
#red = [255.0, 0.0, 0.0]
red = (255, 0, 0)
def drawDot (x, y):
sh.clear()
xi = trunc(x)
xf = x- xi
yi = trunc(y)
yf = y - yi
r = 1.0 - sqrt (xf*xf+yf*yf) / sqrt(2)
sh.set_pixel (xi, yi, int(red[0] * r), int(red[1] * r), int(red[2] * r))
r = 1.0 - sqrt (xf*xf+(1.0-yf)*(1.0-yf)) / sqrt(2)
sh.set_pixel (xi, yi+1, int(red[0] * r), int(red[1] * r), int(red[2] * r))
r = 1.0 - sqrt ((1.0-xf)*(1.0-xf)+yf*yf) / sqrt(2)
sh.set_pixel (xi+1, yi, int(red[0] * r), int(red[1] * r), int(red[2] * r))
r = 1.0 - sqrt ((1.0-xf)*(1.0-xf)+(1.0-yf)*(1.0-yf)) / sqrt(2)
sh.set_pixel (xi+1, yi+1, int(red[0] * r), int(red[1] * r), int(red[2] * r))
while 1:
orad = sh.get_orientation_radians()
# print("p: {pitch}, r: {roll}, y: {yaw}".format(**orad))
shader = pi3d.Shader("mat_light")
model = pi3d.Model(
file_string="apollo-soyuz.obj",
name="model", x=0, y=-1, z=40, sx=1, sy=1, sz=1)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
#sense.set_imu_config(compass, gyro, accel)
sense.set_imu_config(False, True, False )
pitch=math.pi/4
roll=0
yaw=0
yaw_offset = 0
while display.loop_running():
o = sense.get_orientation_radians()
if o is None:
pass
pitch = o["pitch"]
roll = o["roll"]
yaw = o["yaw"]
#roll +=math.pi/180
yaw_total = yaw + math.radians(yaw_offset)
from flask import Flask, render_template, request
from sense_hat import SenseHat
# sensehat instantiëren
sense = SenseHat()
sense.set_rotation(180)
sense.set_imu_config(False, False, False) # gyroscope only
# flask server instantiëren
app = Flask(__name__)
sense_values = {'value': '#000000', 'type': 'hex', 'message': ''}
# function: on_snapshot(doc_snapshot, changes, read_time)
def convertHexValueToTuple(hex_value, list=False):
r = int(hex_value[1:3], 16)
g = int(hex_value[3:5], 16)
b = int(hex_value[5:], 16)
if (list):
return [r, g, b]
return (r, g, b)
def colorTheMatrix():
rgb_value = convertHexValueToTuple(sense_values['value'])
sense.clear(rgb_value)
def messageOnMatrix():
rgb_value = convertHexValueToTuple(sense_values['value'], True)
def pi3d_model():
from sense_hat import SenseHat
import math
import pi3d
sense = SenseHat()
display = pi3d.Display.create()
cam = pi3d.Camera.instance()
shader = pi3d.Shader("mat_light")
model = pi3d.Model(
file_string="cow2.obj",
name="model", x=0, y=-1, z=40, sx=2.5, sy=2.5, sz=2.5)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 0
while display.loop_running():
orientation = sense.get_orientation_radians()
if orientation is None:
pass
pitch = orientation["pitch"]
roll = orientation["roll"]
yaw = orientation["yaw"]
yaw_total = yaw + math.radians(yaw_offset)
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
sin_r = math.sin(roll)
cos_r = math.cos(roll)
abs_roll = math.degrees(math.asin(sin_p * cos_y + cos_p * sin_r * sin_y))
abs_pitch = math.degrees(math.asin(sin_p * sin_y - cos_p * sin_r * cos_y))
model.rotateToZ(abs_roll)
model.rotateToX(abs_pitch)
model.rotateToY(math.degrees(yaw_total))
model.draw()
keypress = keyb.read()
if keypress == 27:
keyb.close()
display.destroy()
break
elif keypress == ord('m'):
compass = not compass
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('g'):
gyro = not gyro
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('a'):
accel = not accel
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('='):
yaw_offset += 1
elif keypress == ord('-'):
yaw_offset -= 1
from sense_hat import SenseHat
hat = SenseHat(
) # instantiating hat right away so we can use it in functions
except:
print("... problem finding SenseHat")
UseEmulator = True
print(" ....trying SenseHat Emulator instead")
if UseEmulator:
print("....importing SenseHat Emulator")
from sense_emu import SenseHat # class for controlling the SenseHat
hat = SenseHat(
) # instantiating hat emulator so we can use it in functions
while not SenseHatEMU:
try:
hat.set_imu_config(True, True,
True) #initialize the accelerometer simulation
except:
sleep(1)
else:
SenseHatEMU = True
# some variables and settings we are going to need as we start up
print("Setting up...")
# This (hiding deprecation warnings) is temporary because the libraries are changing again
warnings.filterwarnings("ignore", category=DeprecationWarning)
Looping = True # this will be set false after the first go-round if a real backend is called
angle = 180
result = None
runcounter = 0
maxpattern = '00000'
from autobahn.twisted.websocket import WebSocketServerProtocol, \
WebSocketServerFactory
from sense_hat import SenseHat
astroPi = SenseHat()
astroPi.set_imu_config(True, True, True)
import json
from twisted.internet import reactor, task
from twisted.internet.defer import Deferred, \
inlineCallbacks, \
returnValue
import sched, time
class AstroPiServerProtocol(WebSocketServerProtocol):
def updateEnv(self):
output = json.dumps({'command':'env-update', 'args': {'temperature':astroPi.temperature, 'pressure': astroPi.pressure, 'humidity': astroPi.humidity, 'direction': astroPi.get_compass(), 'orientation': astroPi.get_orientation(), 'accelerometer': astroPi.get_accelerometer_raw(), 'gyroscope': astroPi.get_gyroscope_raw(), 'compass': astroPi.get_compass_raw()}})
self.sendMessage(output.encode('utf8'))
def onOpen(self):
self.l = task.LoopingCall(self.updateEnv)
self.l.start(0.04)
def onClose(self, wasClean, code, reason):
self.l.stop();
def lowLight(self, lowLight):
if lowLight == "on":
astroPi.low_light = True
else:
class Accelerometer(Thread):
def __init__(self, quantum, bin_logger):
Thread.__init__(self)
self.daemon = True
self.Sense = SenseHat()
self.Sense.set_imu_config(False, False, True)
self.cnt = 0
self.quantum = quantum
self.period = 0.01
self.iters = int(self.quantum / self.period)
self.que = Queue(1024)
self.dorun = True
self.acc_bias = [0.0, 0.0, 0.0]
self.Rmtx = IDmtx
self.bin_logger = bin_logger
#
# Lambdas
#
self.l_len = lambda x: self.calcLen2d(x)
def run(self):
print("Calibrating...")
self.adjust()
print("Calibration done! adjust={}".format(self.acc_bias))
print("quantum={} period={} iters={}".format(self.quantum, self.period, self.iters))
acc_first = self.read_acc()
while (self.dorun):
i = self.iters
sum = 0.0
acc_list = []
while (i > 0):
adj = self.adjAcc(acc_first)
acc_list.append(adj)
i = i - 1
time.sleep(self.period)
acc_first = self.read_acc()
acc_len_list = map(self.l_len, acc_list)
max = reduce(l_max, acc_len_list)
min = reduce(l_min, acc_len_list)
sum = reduce(l_sum, acc_len_list)
avg = sum/float(len(acc_list))
sdev = sqrt(reduce(lambda x,y: x+pow((y-avg), 2.0), acc_len_list, 0)/(len(acc_list)-1.0))
ruc_len_list = map(lambda x,y: (x-y)/self.period,acc_len_list[1:], acc_len_list[:-1])
ruck_max = reduce(l_max, ruc_len_list)
ruck_min = reduce(l_min, ruc_len_list)
ruck_sum = reduce(l_sum, ruc_len_list)
ruck_avg = ruck_sum/float(len(ruc_len_list))
ruck_sdev = sdev = sqrt(reduce(lambda x,y: x+pow((y-ruck_avg), 2.0), ruc_len_list, 0)/(len(ruc_len_list)-1.0))
ruck_tot_avg = sqrt(pow(acc_list[0][0]-acc_list[-1][0], 2)+pow(acc_list[0][1]-acc_list[-1][1], 2)+pow(acc_list[0][2]-acc_list[-1][2], 2))/self.quantum
acc_dic = {'avg': avg, 'min': min, 'max': max, 'sdev': sdev}
ruck_dic = {'avg': ruck_avg, 'min': ruck_min, 'max': ruck_max, 'sdev': ruck_sdev, 'tot_avg': ruck_tot_avg}
self.que.put([dt.datetime.now().strftime('%Y%m%d %H:%M.%S.%f'),
acc_dic, ruck_dic, acc_list[0], acc_list[-1]])
def writeBinLog(self, entry):
if (self.bin_logger == None):
return
else:
self.bin_logger.putNext([dt.datetime.now().strftime('%Y%m%d %H:%M.%S.%f'), entry])
def adjust(self):
ac = 0.0
x = 0.0
y = 0.0
z = 0.0
for i in range(0,100):
rd = self.read_acc()
x += rd[0]
y += rd[1]
z += rd[2]
time.sleep(0.001)
self.acc_bias = [x/100.0, y/100.0, z/100.0]
def read_acc(self):
a1 = self.Sense.get_accelerometer_raw()
self.writeBinLog(a1)
return [a1['x'], a1['y'], a1['z']]
def get(self):
return [self.get_x(), self.get_y(), self.get_z()]
def calcLen(self, acc):
return sqrt(pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2))
def calcLen2d(self, acc):
return sqrt(pow(acc[0], 2) + pow(acc[1], 2))
def adjAcc(self, acc):
return [acc[0]-self.acc_bias[0], acc[1]-self.acc_bias[1], acc[0]+(Gdelta-self.acc_bias[0])]
def stopit(self):
self.dorun = False
def getNext(self):
try:
return self.que.get(True, timeout=15)
except:
print("Exception")
def read_queue(self):
yield self.que.get(True, None)
