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)