def gyroGame(maxDegs): sense = SenseHat() while 1: orientation = sense.get_orientation_degrees() if orientation['pitch'] < maxDegs: realpitch = orientation['pitch'] else: realpitch = -1 * (360 - orientation['pitch']) if orientation['roll'] < maxDegs: realroll = orientation['roll'] else: realroll = -1 * (360 - orientation['roll']) x_pos = 7 - int( round(((maxDegs + realpitch) / (2.0 * maxDegs)) * 8.0, 0)) y_pos = int(round(((maxDegs + realroll) / (2.0 * maxDegs)) * 8.0, 0)) if x_pos > 7: x_pos = 7 if x_pos < 0: x_pos = 0 if y_pos > 7: y_pos = 7 if y_pos < 0: y_pos = 0 sense.clear() if (1 <= x_pos <= 6) and (1 <= y_pos <= 6): sense.set_pixel(x_pos, y_pos, 0, 0, 255) else: sense.set_pixel(x_pos, y_pos, 255, 0, 0)
async def update_handler(): sense = SenseHat() sense.set_imu_config(False, True, False) while True: pressure = int(sense.get_pressure()) temp = int(sense.get_temperature()) humidity = int(sense.get_humidity()) orientation = sense.get_orientation_degrees() pitch = int(orientation["pitch"]) roll = int(orientation["roll"]) yaw = int(orientation["yaw"]) data = [{ "name": "temperture", "data": temp }, { "name": "pressure", "data": pressure }, { "name": "humidity", "data": humidity }, { "name": "pitch", "data": pitch }, { "name": "roll", "data": roll }, { "name": "yaw", "data": yaw }] await websocket.send(json.dumps(data))
def gyroGame(maxDegs): sense = SenseHat() while 1: orientation = sense.get_orientation_degrees() if orientation['pitch'] < maxDegs: realpitch=orientation['pitch'] else: realpitch=-1*(360-orientation['pitch']) if orientation['roll'] < maxDegs: realroll=orientation['roll'] else: realroll=-1*(360-orientation['roll']) x_pos=7-int(round(((maxDegs+realpitch)/(2.0*maxDegs))*8.0,0)) y_pos=int(round(((maxDegs+realroll)/(2.0*maxDegs))*8.0,0)) if x_pos > 7: x_pos = 7 if x_pos < 0: x_pos = 0 if y_pos > 7: y_pos = 7 if y_pos < 0: y_pos = 0 sense.clear() if (1 <= x_pos <=6) and (1 <= y_pos <=6): sense.set_pixel(x_pos,y_pos,0,0,255) else: sense.set_pixel(x_pos,y_pos,255,0,0)
def Start(): sense=SenseHat() rospy.init_node('sense_hat', anonymous=True) #TODO #temp_pub = rospy.Publisher("/sensehat/temperature", Temperature, queue_size= 10) #humidity_pub = rospy.Publisher("/sensehat/humidity", RelativeHumidity, queue_size= 10) pose_pub = rospy.Publisher("/sensehat/pose", Imu, queue_size= 10) compass_pub = rospy.Publisher("/sensehat/compass", MagneticField, queue_size=10) r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): gyro = Imu() gyro.angular_velocity.x = sense.get_gyroscope()['pitch'] gyro.angular_velocity.y = sense.get_gyroscope()['roll'] gyro.angular_velocity.z = sense.get_gyroscope()['yaw'] gyro.orientation.x = sense.get_orientation_degrees()['pitch'] gyro.orientation.y = sense.get_orientation_degrees()['roll'] gyro.orientation.z = sense.get_orientation_degrees()['yaw'] gyro.linear_acceleration.x = sense.get_accelerometer_raw()['x'] gyro.linear_acceleration.y = sense.get_accelerometer_raw()['y'] gyro.linear_acceleration.z = sense.get_accelerometer_raw()['z'] #rospy.loginfo("Sending IMU data:%s", gyro) r.sleep()
class SensehatScanner: worker = None logger = None sense = None repeated_timer_inst = None def start(self, logger): self.logger = logger self.repeated_timer_inst = RepeatedTimer(5, self.log_scan) self.worker = threading.Thread(target=self.start_continous_scan) self.sense = SenseHat() self.worker.do_run = True self.worker.start() self.repeated_timer_inst.start() def stop(self): self.logger = None self.repeated_timer_inst.stop() self.worker.do_run = False self.worker.join() def log_scan(self): self.logger.log_gyro(time.time(), self.orientation['yaw'], self.orientation['pitch'], self.orientation['roll']) def start_continous_scan(self): print 'starting continous sensehat scan' yaw = 0 while True: if (not getattr(self.worker, "do_run", True)): break gyro_only = self.sense.get_gyroscope() self.sense.set_pixel(0, int(yaw / 45), 0, 0, 0) self.orientation = self.sense.get_orientation_degrees() yaw = self.orientation['yaw'] # print yaw self.logger.log_gyro_raw(time.time(), self.orientation['yaw'], self.orientation['pitch'], self.orientation['roll']) self.sense.set_pixel(0, int(yaw / 45), 0, 255, 0) #accel_only = self.sense.get_accelerometer() #print("get_accelerometer p: {pitch}, r: {roll}, y: {yaw}".format(**accel_only)) print 'stopped continous self.sensehat scan'
class SenseController: def __init__(self): self.sense = SenseHat() def __enter__(self): self.clear() return self def __exit__(self, type, value, traceback): self.clear() def clear(self): self.sense.clear() def get_data(self): orientation_time = time.time() orientation = self.sense.get_orientation_degrees() compass_time = time.time() compass = self.sense.get_compass_raw() acceleration_time = time.time() acceleration = self.sense.get_accelerometer_raw() return [ # Environmental sensors (time.time(), "humidity", self.sense.get_humidity()), (time.time(), "pressure", self.sense.get_pressure()), (time.time(), "temperature_from_humidity", self.sense.get_temperature()), (time.time(), "temperature_from_pressure", self.sense.get_temperature_from_pressure()), # IMU sensors (orientation_time, "orientation.pitch", orientation['pitch']), (orientation_time, "orientation.roll", orientation['roll']), (orientation_time, "orientation.yaw", orientation['yaw']), (compass_time, "compass.x", compass['x']), (compass_time, "compass.y", compass['y']), (compass_time, "compass.z", compass['z']), (acceleration_time, "accelerometer.x", acceleration['x']), (acceleration_time, "accelerometer.y", acceleration['y']), (acceleration_time, "accelerometer.z", acceleration['z']) ] def show_message(self, message): self.sense.show_message(message, text_colour=[0, 64, 0])
class Acceleration: def __init__(self): self.sense = SenseHat() self.madgwick = MadgwickAHRS(.055, None, 1) def get_acceleration(self): acceleration = self.sense.get_accelerometer_raw() orientation = self.sense.get_orientation_degrees() # print("p: {pitch}, r: {roll}, y: {yaw}".format(**orientation)) ori_pitch = orientation['pitch'] acc_x = acceleration['x'] print("acceleration before compensation: {:.4f}".format(acc_x)) def get_quaternion(self): ninedofxyz = read_sensors() # Get data from sensors madgwick.update(ninedofxyz[0], ninedofxyz[1], ninedofxyz[2]) # Update self.ahrs = madgwick.quaternion.to_euler_angles()
class SenseClient(object): def __init__(self): self.sense = SenseHat() self.sense.clear() self.sense.set_imu_config(True, True, True) def getSensePoints(self, imperial_or_metric, bucket): dt = datetime.now(tz=pytz.timezone('US/Pacific')).isoformat() point = Point(measurement_name="sense") point.time(time=dt) # % relative point.field("humidity", self.sense.get_humidity()) if imperial_or_metric == "imperial": point.field( "temperature_from_humidity", convertCToF(self.sense.get_temperature_from_humidity())) point.field( "temperature_from_pressure", convertCToF(self.sense.get_temperature_from_pressure())) point.field("pressure", convertmbToPSI(self.sense.get_pressure())) else: point.field("temperature_from_humidity", self.sense.get_temperature_from_humidity()) point.field("temperature_from_pressure", self.sense.get_temperature_from_pressure()) point.field("pressure", self.sense.get_pressure()) point.field("orientation_radians", self.sense.get_orientation_radians()) point.field("orientation_degress", self.sense.get_orientation_degrees()) # magnetic intensity in microteslas point.field("compass_raw", self.sense.get_compass_raw()) # rotational intensity in radians per second point.field("gyroscope_raw", self.sense.get_gyroscope_raw()) # acceleration intensity in Gs point.field("accelerometer_raw", self.sense.get_accelerometer_raw()) return [{"bucket": bucket, "point": point}]
#sense.show_message("*",scroll_speed=0.05) tmp_res = json.loads(getData("http://localhost:9090/cmd","select count(*) from collect")) now_count = str(tmp_res['Content']['content']) sense.show_message( now_count[2:-2] + " rows", scroll_speed=0.05) sense.load_image("logo8.png") sense.set_pixel(7, 6, 20*(count%10)+50, 110, 20*(count%10)+50) sense.set_pixel(7, 7, 20*(count%10)+50, 110, 20*(count%10)+50) sense.set_pixel(6, 6, 20*(count%10)+50, 110, 20*(count%10)+50) sense.set_pixel(6, 7, 20*(count%10)+50, 110, 20*(count%10)+50) humidity = sense.get_humidity() temp = sense.get_temperature() pressure = sense.get_pressure() orientation = sense.get_orientation_degrees() pitch = orientation['pitch'] roll = orientation['roll'] yaw = orientation['yaw'] acceleration = sense.get_accelerometer_raw() acc_x = acceleration['x'] acc_y = acceleration['y'] acc_z = acceleration['z'] #now = strftime("%Y-%m-%d %H:%M:%S", gmtime()) now = strftime("%Y-%m-%d %H:%M:%S", localtime()) stmt = "insert into collect values('" + now + "','" stmt = stmt + str(humidity) + "','" + str(temp) + "','" + str(pressure) + "','" + str(pitch) stmt = stmt + "','" + str(roll) + "','" + str(yaw) + "','" + str(acc_x) stmt = stmt + "','" + str(acc_y) + "','" + str(acc_z) + "')"
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."
if (rawSize != 20): logging.debug('Rejected (%d != 20)', rawSize) continue logging.debug('Accepted') messageSize, messageType, messageTime, destRightAscension, destDeclination = struct.unpack( "<hhqIi", data) logging.info("Received Message Size: %d", messageSize) logging.info("Received Message Type: %d", messageType) logging.info("Received Message Time: %d", messageTime) logging.info("Destination Right Ascension: %d", destRightAscension) logging.info("Destination Declination: %d", destDeclination) sense = SenseHat() [yaw, roll, pitch] = sense.get_orientation_degrees().values() logging.info("pitch: %s", pitch) logging.debug("roll: %s", roll) logging.info("yaw: %s", yaw) coords = coordinates(datetime.utcnow()) [Ra, Dec] = coords.getRaDec(yaw, pitch) logging.info( "Right Ascension. Degrees: %s. Radians: %s. Hours: %s", Ra.d, Ra.r, Ra.h) logging.info( "Declination. Degrees: %s. Radians: %s. Hours: %s", Dec.d, Dec.r, Dec.h) [RaInt, DecInt] = angleToStellarium(Ra, Dec) #"<hhqIi"
for i in range(64): leds_template.append((0, 0, 0)) with open("data.json", "w") as data_json_init: json.dump(data_template, data_json_init) with open("leds.json", "w") as leds_json_init: json.dump(leds_template, leds_json_init) # Init sense hat sense = SenseHat() sense.clear() while True: # Gathering data and writing to data.json system("clear") data_template["temperature"] = sense.get_temperature() data_template["pressure"] = sense.get_pressure() data_template["humidity"] = sense.get_humidity() data_template["orientation_degrees"] = sense.get_orientation_degrees() with open("data.json", "w") as data_json: json.dump(data_template, data_json) # Gathering and setting LEDs with open("leds.json") as leds_json: led_matrix_config = json.load(leds_json) x = 0 y = 0 for i in range(64): sense.set_pixel(x, y, led_matrix_config[i][0], led_matrix_config[i][1], led_matrix_config[i][2]) if x == 7: x = 0 y += 1 else: x += 1
#!/usr/bin/python import sys import json from sense_hat import SenseHat sense = SenseHat() print json.dumps(sense.get_orientation_degrees())
# Configure CSV csvfile = util.path_for_data(1) logger.info(f'Logging to {csvfile}') # TODO write header while True: # Check for end time now = datetime.now() if now >= end_time: logger.info(f'Finished run at {now}') break # Main loop try: orientation = sh.get_orientation_degrees() compass = sh.get_compass() compass_raw = sh.get_compass_raw() gyro = sh.get_gyroscope() gyro_raw = sh.get_gyroscope_raw() accelerometer_raw = sh.get_accelerometer_raw() camera.capture(debug_capture=True) util.add_csv_data(csvfile, ( now, sh.get_humidity(), sh.get_temperature(), sh.get_temperature_from_humidity(), sh.get_temperature_from_pressure(), sh.get_pressure(), orientation['roll'],
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
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())
from sense_hat import SenseHat from time import sleep sense = SenseHat() sense.clear # Define variable level = 255 while True: bearing = sense.get_orientation_degrees() #sleep(0.5) roll = (round(bearing['roll'])) pitch = (round(bearing['pitch'])) yaw = (round(bearing['yaw'])) print(f"Roll:{roll} Pitch:{pitch} Yaw:{yaw}")
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):
f = open("/home/pi/RHAB/LOG/hum.txt", "a+") f.write(hum + "\n") f.close() #temperature temp = str(sense.get_temperature()) f = open("/home/pi/RHAB/LOG/temp.txt", "a+") f.write(temp + "\n") f.close() #pressure pres = str(sense.get_pressure()) f = open("/home/pi/RHAB/LOG/pres.txt", "a+") f.write(pres + "\n") f.close() #orientaion ori = str( "p:{pitch},r:{roll},y:{yaw}".format(**sense.get_orientation_degrees())) f = open("/home/pi/RHAB/LOG/ori.txt", "a+") f.write(ori + "\n") f.close() #compass comp = str("x:{x},y:{y},z:{z}".format(**sense.get_compass_raw())) f = open("/home/pi/RHAB/LOG/comp.txt", "a+") f.write(comp + "\n") f.close() #Combined file f = open("/home/pi/RHAB/LOG/combinedData", "a+") f.write(timevarH + "," + hum + "," + temp + "," + pres + "," + ori + "," + comp + "\n") f.close() print("sensor data writen to file")
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 json import os from sense_hat import SenseHat sense = SenseHat() sense.set_imu_config(True, True, True) # Python SenseHat API documentation: # https://pythonhosted.org/sense-hat/api # Roll is: angular x. # Pitch is: angular y. # Yaw is: angular z. data_imu = {} data_imu["gyro"] = sense.get_orientation_degrees( ) # Gets orientation (3-ang-axis) in [deg]. data_imu["acc"] = sense.get_accelerometer( ) # Gets orientation from the accelerometer (3-ang-axis) in [deg]. data_imu["gyror"] = sense.get_gyroscope_raw( ) # Gets angular velocity (3-axis) in [rad/s]. data_imu["mag"] = sense.get_compass() # Gets orientation to North in [deg]. data_imu["magr"] = sense.get_compass_raw( ) # Gets magnetic field (3-axis) in [µT]. data_imu["accr"] = sense.get_accelerometer_raw( ) # Gets acceleration (3-axis) in [g]'s. data_env = {} data_env["temph"] = sense.get_temperature_from_humidity( ) # Gets temperature from humidity sensor in [ºC]. data_env["tempp"] = sense.get_temperature_from_pressure( ) # Gets temperature from pressure sensor in [ºC].
class SenseHATPlugin(EPLPluginBase): def __init__(self, init): super(SenseHATPlugin, self).__init__(init) self.getLogger().info("SenseHATPlugin initialised with config: %s" % self.getConfig()) self.sense = SenseHat() @EPLAction("action<integer >") def setRotation(self, angle=None): """ Orientation of the message/image @param message: Orientation of the message, The supported values of orientation are 0, 90, 180, and 270.. """ if angle == None: angle = 90 self.sense.set_rotation(angle) @EPLAction("action<integer, integer, integer, integer, integer >") def setPixel(self, x, y, r, g, b): self.sense.set_pixel(x, y, r, g, b) @EPLAction("action< sequence <sequence <integer> > >") def setPixels(self, pixels): if not isinstance(pixels, list): return self.sense.set_pixels(pixels) @EPLAction("action< >") def clear(self): self.sense.clear() @EPLAction("action< integer, integer, integer >") def Clear(self, r, g, b): self.sense.clear(r, g, b) @EPLAction("action<string >") def showMessage(self, message): self.sense.show_message(message) @EPLAction("action<string, float, sequence<integer>, sequence<integer> >") def displayMessage(self, message, scrollingSpeed=None, textColor=None, backgroundColor=None): """ This method will scrolls a text message across the LED Matrix. @param message: The Scrolling message to be displayed. @param scrollingSpeed: Scrolling message speed. @param textColor: The text color of the scrolling message i.e [R G B] . @param backgroundColor: The background color of the scrolling message [R G B]. """ if scrollingSpeed == None: scrollingSpeed = 0.1 if (textColor == None) or (not isinstance(textColor, list)): textColor = [255, 255, 255] if (backgroundColor == None) or (not isinstance(backgroundColor, list)): backgroundColor = [0, 0, 0] self.sense.show_message(message, scroll_speed=scrollingSpeed, text_colour=textColor, back_colour=backgroundColor) @EPLAction("action<string >") def showLetter(self, letter): if not isinstance(letter, basestring): return self.sense.show_letter(letter) @EPLAction("action<boolean >") def lowLight(self, islowLight): self.sense.low_light = islowLight @EPLAction("action<string >") def loadImage(self, imagePath): self.sense.load_image(imagePath) ########################################################################################################## # Environmental sensors ########################################################################################################## @EPLAction("action<> returns float") def getHumidity(self): humidity = self.sense.get_humidity() return round(humidity, 2) @EPLAction("action<> returns float") def getTemperature(self): temp = self.sense.get_temperature() return round(temp, 2) @EPLAction("action<> returns float") def getTemperatureFromHumidity(self): temp = self.sense.get_temperature_from_humidity() return round(temp, 2) @EPLAction("action<> returns float") def getTemperatureFromPressure(self): temp = self.sense.get_temperature_from_pressure() return round(temp, 2) @EPLAction("action<> returns float") def getPressure(self): pressure = self.sense.get_pressure() return round(pressure, 2) ########################################################################################################## # Joystick ########################################################################################################## @EPLAction("action<boolean > returns com.apamax.sensehat.InputEvent") def waitForEvent(self, emptyBuffer=False): jevent = self.sense.stick.wait_for_event(emptybuffer=emptyBuffer) evtInstance = Event( 'com.apamax.sensehat.InputEvent', { "actionValue": jevent.action, "directionValue": jevent.direction, "timestamp": jevent.timestamp }) return evtInstance @EPLAction("action<> returns sequence<com.apamax.sensehat.InputEvent >") def getEvents(self): events = list() for event in self.sense.stick.get_events(): evtInstance = Event( 'com.apamax.sensehat.InputEvent', { "actionValue": jevent.action, "directionValue": jevent.direction, "timestamp": jevent.timestamp }) events.append(evtInstance) return events def pushed_up(event): if event.action == ACTION_RELEASED: joyevt = Event('com.apamax.sensehat.JoystickControl', {"controlType": 1}) Correlator.sendTo("sensedhat_data", joyevt) def pushed_down(event): if event.action == ACTION_RELEASED: joyevt = Event('com.apamax.sensehat.JoystickControl', {"controlType": 2}) Correlator.sendTo("sensedhat_data", joyevt) def pushed_left(event): if event.action == ACTION_RELEASED: joyevt = Event('com.apamax.sensehat.JoystickControl', {"controlType": 3}) Correlator.sendTo("sensedhat_data", joyevt) def pushed_right(event): if event.action == ACTION_RELEASED: joyevt = Event('com.apamax.sensehat.JoystickControl', {"controlType": 4}) Correlator.sendTo("sensedhat_data", joyevt) def pushed_in(event): if event.action == ACTION_RELEASED: self.sense.show_message(str(round(sense.temp, 1)), 0.05, b) ########################################################################################################## # IMU Sensor ########################################################################################################## @EPLAction("action<boolean, boolean, boolean >") def setIMUConfig(self, compassEnabled, gyroscopeEnabled, accelerometerEnabled): ''' Enables and disables the gyroscope, accelerometer and/or magnetometer contribution to the get orientation functions @param compassEnabled : enable compass @param gyroscopeEnabled : enable gyroscope @param accelerometerEnabled : enable accelerometer ''' self.sense.set_imu_config(compassEnabled, gyroscopeEnabled, accelerometerEnabled) @EPLAction("action< > returns com.apamax.sensehat.IMUData") def getOrientationRadians(self): ''' Gets the current orientation in radians using the aircraft principal axes of pitch, roll and yaw @return event with pitch, roll and yaw values. Values are floats representing the angle of the axis in radians ''' pitch, roll, yaw = self.sense.get_orientation_radians().values() evtInstance = Event('com.apamax.sensehat.IMUData', { "pitch": pitch, "roll": roll, "yaw": yaw }) return evtInstance @EPLAction("action< > returns com.apamax.sensehat.IMUData") def getOrientationDegrees(self): ''' Gets the current orientation in degrees using the aircraft principal axes of pitch, roll and yaw @return event with pitch, roll and yaw values. Values are Floats representing the angle of the axis in degrees ''' pitch, roll, yaw = self.sense.get_orientation_degrees().values() evtInstance = Event('com.apamax.sensehat.IMUData', { "pitch": pitch, "roll": roll, "yaw": yaw }) return evtInstance @EPLAction("action< > returns com.apamax.sensehat.IMUData") def getOrientation(self): ''' @return event with pitch, roll and yaw representing the angle of the axis in degrees ''' pitch, roll, yaw = self.sense.get_orientation().values() evtInstance = Event('com.apamax.sensehat.IMUData', { "pitch": pitch, "roll": roll, "yaw": yaw }) return evtInstance @EPLAction("action< > returns float") def getCompass(self): ''' Calls set_imu_config internally in Python core to disable the gyroscope and accelerometer then gets the direction of North from the magnetometer in degrees @return The direction of North ''' return self.sense.get_compass() @EPLAction("action< > returns com.apamax.sensehat.IMUDataRaw") def getCompassRaw(self): ''' Gets the raw x, y and z axis magnetometer data @return event representing the magnetic intensity of the axis in microteslas (uT) ''' x, y, z = self.sense.get_compass_raw().values() evtInstance = Event('com.apamax.sensehat.IMUDataRaw', { "x": x, "y": y, "z": z }) return evtInstance @EPLAction("action< > returns com.apamax.sensehat.IMUData") def getGyroscope(self): ''' Calls set_imu_config internally in Python core to disable the magnetometer and accelerometer then gets the current orientation from the gyroscope only @return event with pitch, roll and yaw representing the angle of the axis in degrees ''' pitch, roll, yaw = self.sense.get_gyroscope().values() evtInstance = Event('com.apamax.sensehat.IMUData', { "pitch": pitch, "roll": roll, "yaw": yaw }) return evtInstance @EPLAction("action< > returns com.apamax.sensehat.IMUDataRaw") def getGyroscopeRaw(self): ''' Gets the raw x, y and z axis gyroscope data @return event representing the rotational intensity of the axis in radians per second ''' x, y, z = sense.get_gyroscope_raw().values() evtInstance = Event('com.apamax.sensehat.IMUDataRaw', { "x": x, "y": y, "z": z }) return evtInstance @EPLAction("action< > returns com.apamax.sensehat.IMUData") def getAccelerometer(self): ''' Calls set_imu_config in Python core to disable the magnetometer and gyroscope then gets the current orientation from the accelerometer only @return Object representing the angle of the axis in degrees ''' pitch, roll, yaw = self.sense.get_accelerometer().values() evtInstance = Event('com.apamax.sensehat.IMUData', { "pitch": pitch, "roll": roll, "yaw": yaw }) return evtInstance @EPLAction("action< > returns com.apamax.sensehat.IMUDataRaw") def getAccelerometerRaw(self): ''' Gets the raw x, y and z axis accelerometer data @return event representing the acceleration intensity of the axis in Gs ''' x, y, z = sense.get_accelerometer_raw().values() evtInstance = Event('com.apamax.sensehat.IMUDataRaw', { "x": x, "y": y, "z": z }) return evtInstance
from sense_hat import SenseHat import time sense = SenseHat() sense.clear() pitch = sense.get_orientation_degrees()['pitch'] current_position = [4, 4] while True: orientation = sense.get_orientation_degrees() new_pitch = orientation['pitch'] diff = pitch - new_pitch if (abs(diff) > 30): if diff > 0: adding_value = 1 else: adding_value = -1 current_position[0] = current_position[0] + adding_value if current_position[0] < 0: current_position[0] = 0 elif current_position[0] > 7: current_position[0] = 7 sense.clear() sense.set_pixel(current_position[0], current_position[1], (255, 0, 0)) pitch = new_pitch #time.sleep(0.5)
#!/usr/bin/env python from sense_hat import SenseHat sense = SenseHat() while True: o = sense.get_orientation_degrees() print("{roll:16.9f} {pitch:16.9f} {yaw:16.9f}".format(**o))
# Shift-lefting orientation angle def shiftAngle(angle): return (angle - 180) if angle > 180 else (angle + 180) def pixelRow(angle): normalized_angle = math.floor(rows * shiftAngle(angle) / 360) return [ colours_per_row[i] if i == normalized_angle else black for i in range(rows) ] # Exit handling def exit_handler(signal, frame): sense.clear() sys.exit(0) signal.signal(signal.SIGTERM, exit_handler) while True: roll = sense.get_orientation_degrees()['roll'] pixels = [] for num in range(rows): pixels = pixels + pixelRow(roll) sense.set_pixels(pixels)
textSpeed = 0.05 #sense.show_message(initMessage, scroll_speed = textSpeed, text_colour = foreColor, back_colour = backColor) # display splash sense.load_image("kao.png") time.sleep(2) while True: # 磁気 magnetoRaw = sense.get_compass_raw() # 加速度 acceleroRaw = sense.get_accelerometer_raw() # 方位角 houikaku = sense.get_compass() # 傾き katamuki = sense.get_orientation_degrees() print("magneto x: {x:.6g}, y: {y:.6g}, z: {z:.6g}".format(**magnetoRaw)) print("accelero x: {x:.6g}, y: {y:.6g}, z: {z:.6g}".format(**acceleroRaw)) print("houikaku: {:.6g}".format(houikaku)) print("katamuki: p: {pitch}, r: {roll}, y: {yaw}".format(**katamuki)) houi = houi_hantei(houikaku) sense.show_letter(houi, text_colour=foreColor, back_colour=backColor) houi_image = "" if houi == "N": houi_image = "kita.png" elif houi == "E": houi_image = "higashi.png" elif houi == "S":
# N.B.: The temperature is read from the pressure and/or humidity sensor pressure = sense.get_pressure() temperature = sense.get_temperature() humidity = sense.get_humidity() # Print the results result = "Pre: %.1f Temp: %.1f Hum: %.1f" % (pressure, temperature, humidity) sense.show_message(result) # # Sense the Inertial Measurement Unit # # Read the orientation of the module # N.B.: This is read from the accelerometer orientation = sense.get_orientation_degrees() # radians version exists too print("Pitch {0} Roll {1} Yaw {2}".format(orientation["pitch"], orientation["roll"], orientation["yaw"])) # Get the accelerator information in G's accel = sense.get_accelerometer_raw() sense.show_message("x: %.1f y: %.1f z: %1.f" % (accel["x"], accel["y"], accel["z"])) # Show the orientation graphically sense.show_letter("J") x = round(accel["x"], 0) while x < 1: accel = sense.get_accelerometer_raw() x = round(accel["x"], 0) y = round(accel["y"], 0) z = round(accel["z"], 0)
client.connect(mqtt_broker) logging.info("Connected to broker") # get the data from sense hat hat = SenseHat() hat.clear() logging.info("Starting sensor value sending loop") try: while True: temperature = hat.get_temperature() client.publish(topic_root + "temperature", temperature) humidity = hat.get_humidity() client.publish(topic_root + "humidity", humidity) pressure = hat.get_pressure() client.publish(topic_root + "pressure", pressure) orientation = hat.get_orientation_degrees() client.publish(topic_root + "orientation/pitch", orientation['pitch']) client.publish(topic_root + "orientation/roll", orientation['roll']) client.publish(topic_root + "orientation/yaw", orientation['yaw']) compass = hat.get_compass() client.publish(topic_root + "compass", compass) time.sleep(.200) except KeyboardInterrupt: pass
oldZAceleration=0 m_x_old=0 m_y_old=0 m_z_old=0 oldYaw=0 oldNorma=0 oldNormaW=0 while True: temperature = sense.get_temperature() pressure = sense.get_pressure() humidity = sense.get_humidity() pitch, roll, yaw = sense.get_orientation_degrees().values() x, y, z = sense.get_accelerometer_raw().values() m_x, m_y, m_z = sense.get_compass_raw().values() # Rotation pitch = round(pitch, 3) roll = round(roll, 3) yaw = round(yaw, 3) # Enviromental temperature = round(temperature, 1) pressure = round(pressure, 1) humidity = round(humidity, 1)
class Game(): def __init__(self): self.sense = SenseHat() self.configure_stick() self.last_orientation = self.sense.get_orientation_degrees() while True: for i in range(5): self.sense.load_image("img/heart8.png") time.sleep(0.5) self.sense.clear() time.sleep(0.5) self.sense.show_message("TE QUIERO") time.sleep(1) self.running = False # event = self.sense.stick.wait_for_event() def run(self): #self.init_message() self.start_game() def configure_stick(self, ): self.sense.stick.direction_any = self.launch_game def init_message(self): self.sense.show_message("Ready", scroll_speed=0.05) self.sense.show_message("Set", scroll_speed=0.05) self.sense.show_message("Go!!", scroll_speed=0.05) def start_game(self): ball = Ball(x=4, y=4, sense=self.sense) self.running = True while self.running: dx, dy = self.next_move() game_over = self.is_game_over(ball, dx, dy) print("Game over = {}".format(game_over)) if game_over: self.running = False print("Game over") self.sense.show_letter("X", text_colour=[255, 255, 0]) time.sleep(3) self.show_message("Game over") else: ball.move(dx, dy) time.sleep(ORIENTATION_REFRESH) def launch_game(self, event): if event.action != ACTION_RELEASED: self.run() def is_game_over(self, ball, dx, dy): died = False new_x = ball.x + dx new_y = ball.y + dy if new_x <= 7 and new_x >= 0 and new_y <= 7 and new_y >= 0: died = False else: died = True print("Died = {}".format(died)) return died def next_move(self, min_value=0, max_value=7): # x = getrandbits(1) * 2 - 1 # y = getrandbits(1) * 2 - 1 orientation = self.sense.get_orientation_degrees() difference = { 'd_pitch': orientation['pitch'] - self.last_orientation['pitch'], 'd_roll': orientation['roll'] - self.last_orientation['roll'], 'd_yaw': orientation['yaw'] - self.last_orientation['yaw'] } # sys.stdout.write("p: {pitch:.2f}, r: {roll:.2f}, y: {yaw:.2f}".format(**orientation)) if difference['d_pitch'] / 0.5 > 1: x = -1 elif difference['d_pitch'] / 0.5 < -1: x = 1 else: x = 0 if difference['d_roll'] / 0.5 > 1: y = 1 elif difference['d_roll'] / 0.5 < -1: y = -1 else: y = 0 sys.stdout.write( "droll: {:.2f}, dpitch: {:.2f} - X: {:.2f},Y: {:.2f}\n".format( difference['d_roll'], difference['d_pitch'], x, y)) self.last_orientation = orientation return x, y
def start(self): self.enable = True self.imuPub = rospy.Publisher(self.robot_host + '/imu', Imu, queue_size=10) self.imuRawPub = rospy.Publisher(self.robot_host + '/imu/raw', Imu, queue_size=10) self.accelerometerPub = rospy.Publisher(self.robot_host + '/imu/accelerometer', Accelerometer, queue_size=10) self.accelerometerPitchPub = rospy.Publisher( self.robot_host + '/imu/accelerometer/pitch', Pitch, queue_size=10) self.accelerometerRollPub = rospy.Publisher(self.robot_host + '/imu/accelerometer/roll', Roll, queue_size=10) self.accelerometerYawPub = rospy.Publisher(self.robot_host + '/imu/accelerometer/yaw', Yaw, queue_size=10) self.accelerometerRawPub = rospy.Publisher(self.robot_host + '/imu/accelerometer/raw', Accelerometer, queue_size=10) self.accelerometerRawXPub = rospy.Publisher(self.robot_host + '/imu/accelerometer/raw/x', Float64, queue_size=10) self.accelerometerRawYPub = rospy.Publisher(self.robot_host + '/imu/accelerometer/raw/y', Float64, queue_size=10) self.accelerometerRawZPub = rospy.Publisher(self.robot_host + '/imu/accelerometer/raw/z', Float64, queue_size=10) self.gyroscopePub = rospy.Publisher(self.robot_host + '/imu/gyroscope', Gyroscope, queue_size=10) self.gyroscopePitchPub = rospy.Publisher(self.robot_host + '/imu/gyroscope/pitch', Pitch, queue_size=10) self.gyroscopeRollPub = rospy.Publisher(self.robot_host + '/imu/gyroscope/roll', Roll, queue_size=10) self.gyroscopeYawPub = rospy.Publisher(self.robot_host + '/imu/gyroscope/yaw', Yaw, queue_size=10) self.gyroscopeRawPub = rospy.Publisher(self.robot_host + '/imu/gyroscope/raw', Gyroscope, queue_size=10) self.gyroscopeRawXPub = rospy.Publisher(self.robot_host + '/imu/gyroscope/raw/x', Float64, queue_size=10) self.gyroscopeRawYPub = rospy.Publisher(self.robot_host + '/imu/gyroscope/raw/y', Float64, queue_size=10) self.gyroscopeRawZPub = rospy.Publisher(self.robot_host + '/imu/gyroscope/raw/z', Float64, queue_size=10) self.magnetometerPub = rospy.Publisher(self.robot_host + '/imu/magnetometer', Magnetometer, queue_size=10) self.magnetometerRawPub = rospy.Publisher(self.robot_host + '/imu/magnetometer/raw', Orientation, queue_size=10) self.orientationPub = rospy.Publisher(self.robot_host + '/imu/orientation', Orientation, queue_size=10) self.orientationDegreePub = rospy.Publisher(self.robot_host + '/imu/orientation/degrees', Orientation, queue_size=10) self.orientationRadiansPub = rospy.Publisher( self.robot_host + '/imu/orientation/radians', Orientation, queue_size=10) self.orientationNorthPub = rospy.Publisher(self.robot_host + '/imu/orientation/north', Magnetometer, queue_size=10) sense = SenseHat() while not rospy.is_shutdown(): accel_only = sense.get_accelerometer() accel_raw = sense.get_accelerometer_raw() gyro_only = sense.get_gyroscope() gyro_raw = sense.get_gyroscope_raw() north = sense.get_compass() compass = sense.get_compass_raw() orientation = sense.get_orientation() orientation_deg = sense.get_orientation_degrees() orientation_rad = sense.get_orientation_radians() imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = "/base_link" imu_msg.linear_acceleration.x = accel_only['pitch'] imu_msg.linear_acceleration.y = accel_only['roll'] imu_msg.linear_acceleration.z = accel_only['yaw'] imu_msg.angular_velocity.x = gyro_only['pitch'] imu_msg.angular_velocity.y = gyro_only['roll'] imu_msg.angular_velocity.z = gyro_only['yaw'] imu_msg.orientation.x = orientation['pitch'] imu_msg.orientation.y = orientation['roll'] imu_msg.orientation.z = orientation['yaw'] imu_msg.orientation.w = 0 imu_msg.orientation_covariance = [ 99999.9, 0.0, 0.0, 0.0, 99999.9, 0.0, 0.0, 0.0, 99999.9 ] imu_msg.angular_velocity_covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] imu_msg.linear_acceleration_covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] imu_raw_msg = Imu() imu_raw_msg.header.stamp = rospy.Time.now() imu_raw_msg.header.frame_id = "/base_link" imu_raw_msg.linear_acceleration.x = accel_raw['x'] imu_raw_msg.linear_acceleration.y = accel_raw['y'] imu_raw_msg.linear_acceleration.z = accel_raw['z'] imu_raw_msg.angular_velocity.x = gyro_raw['x'] imu_raw_msg.angular_velocity.y = gyro_raw['y'] imu_raw_msg.angular_velocity.z = gyro_raw['z'] imu_raw_msg.orientation.x = compass['x'] imu_raw_msg.orientation.y = compass['y'] imu_raw_msg.orientation.z = compass['z'] imu_raw_msg.orientation.w = north imu_raw_msg.orientation_covariance = [ 99999.9, 0.0, 0.0, 0.0, 99999.9, 0.0, 0.0, 0.0, 99999.9 ] imu_raw_msg.angular_velocity_covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] imu_raw_msg.linear_acceleration_covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] accel_msg = Accelerometer() accel_msg.header.stamp = rospy.Time.now() accel_msg.header.frame_id = "/base_link" accel_msg.x = accel_only['pitch'] accel_msg.y = accel_only['roll'] accel_msg.z = accel_only['yaw'] accel_pitch_msg = Pitch() accel_pitch_msg.header.stamp = rospy.Time.now() accel_pitch_msg.header.frame_id = "/base_link" accel_pitch_msg.data = accel_only['pitch'] accel_roll_msg = Roll() accel_roll_msg.header.stamp = rospy.Time.now() accel_roll_msg.header.frame_id = "/base_link" accel_roll_msg.data = accel_only['roll'] accel_yaw_msg = Yaw() accel_yaw_msg.header.stamp = rospy.Time.now() accel_yaw_msg.header.frame_id = "/base_link" accel_yaw_msg.data = accel_only['yaw'] accel_raw_msg = Accelerometer() accel_raw_msg.header.stamp = rospy.Time.now() accel_raw_msg.header.frame_id = "/base_link" accel_raw_msg.x = accel_raw['x'] accel_raw_msg.y = accel_raw['y'] accel_raw_msg.z = accel_raw['z'] accel_raw_x_msg = Float64() accel_raw_x_msg.header.stamp = rospy.Time.now() accel_raw_x_msg.header.frame_id = "/base_link" accel_raw_x_msg.data = accel_raw['x'] accel_raw_y_msg = Float64() accel_raw_y_msg.header.stamp = rospy.Time.now() accel_raw_y_msg.header.frame_id = "/base_link" accel_raw_y_msg.data = accel_raw['y'] accel_raw_z_msg = Float64() accel_raw_z_msg.header.stamp = rospy.Time.now() accel_raw_z_msg.header.frame_id = "/base_link" accel_raw_z_msg.data = accel_raw['z'] gyro_msg = Gyroscope() gyro_msg.header.stamp = rospy.Time.now() gyro_msg.header.frame_id = "/base_link" gyro_msg.x = gyro_only['pitch'] gyro_msg.y = gyro_only['roll'] gyro_msg.z = gyro_only['yaw'] gyro_pitch_msg = Pitch() gyro_pitch_msg.header.stamp = rospy.Time.now() gyro_pitch_msg.header.frame_id = "/base_link" gyro_pitch_msg.data = gyro_only['pitch'] gyro_roll_msg = Roll() gyro_roll_msg.header.stamp = rospy.Time.now() gyro_roll_msg.header.frame_id = "/base_link" gyro_roll_msg.data = gyro_only['roll'] gyro_yaw_msg = Yaw() gyro_yaw_msg.header.stamp = rospy.Time.now() gyro_yaw_msg.header.frame_id = "/base_link" gyro_yaw_msg.data = gyro_only['yaw'] gyro_raw_msg = Gyroscope() gyro_raw_msg.header.stamp = rospy.Time.now() gyro_raw_msg.header.frame_id = "/base_link" gyro_raw_msg.x = gyro_raw['x'] gyro_raw_msg.y = gyro_raw['y'] gyro_raw_msg.z = gyro_raw['z'] gyro_raw_x_msg = Float64() gyro_raw_x_msg.header.stamp = rospy.Time.now() gyro_raw_x_msg.header.frame_id = "/base_link" gyro_raw_x_msg.data = gyro_raw['x'] gyro_raw_y_msg = Float64() gyro_raw_y_msg.header.stamp = rospy.Time.now() gyro_raw_y_msg.header.frame_id = "/base_link" gyro_raw_y_msg.data = gyro_raw['y'] gyro_raw_z_msg = Float64() gyro_raw_z_msg.header.stamp = rospy.Time.now() gyro_raw_z_msg.header.frame_id = "/base_link" gyro_raw_z_msg.data = gyro_raw['z'] north_msg = Magnetometer() north_msg.header.stamp = rospy.Time.now() north_msg.header.frame_id = "/base_link" north_msg.north = north compass_msg = Orientation() compass_msg.header.stamp = rospy.Time.now() compass_msg.header.stamp = rospy.Time.now() compass_msg.x = compass['x'] compass_msg.y = compass['y'] compass_msg.z = compass['z'] orientation_msg = Orientation() orientation_msg.header.stamp = rospy.Time.now() orientation_msg.header.stamp = rospy.Time.now() orientation_msg.x = orientation['pitch'] orientation_msg.y = orientation['roll'] orientation_msg.z = orientation['yaw'] orientation_degree_msg = Orientation() orientation_degree_msg.header.stamp = rospy.Time.now() orientation_degree_msg.header.stamp = rospy.Time.now() orientation_degree_msg.x = orientation_deg['pitch'] orientation_degree_msg.y = orientation_deg['roll'] orientation_degree_msg.z = orientation_deg['yaw'] orientation_rad_msg = Orientation() orientation_rad_msg.header.stamp = rospy.Time.now() orientation_rad_msg.header.stamp = rospy.Time.now() orientation_rad_msg.x = orientation_rad['pitch'] orientation_rad_msg.y = orientation_rad['roll'] orientation_rad_msg.z = orientation_rad['yaw'] rospy.loginfo( "imu/accelerometer: p: {pitch}, r: {roll}, y: {yaw}".format( **accel_only)) rospy.loginfo( "imu/accelerometer/raw: x: {x}, y: {y}, z: {z}".format( **accel_raw)) rospy.loginfo( "imu/gyroscope: p: {pitch}, r: {roll}, y: {yaw}".format( **gyro_only)) rospy.loginfo( "imu/gyroscope/raw: x: {x}, y: {y}, z: {z}".format(**gyro_raw)) rospy.loginfo("imu/magnetometer: North: %s" % north) rospy.loginfo( "imu/magnetometer/raw: x: {x}, y: {y}, z: {z}".format( **compass)) rospy.loginfo( "imu/orientation: p: {pitch}, r: {roll}, y: {yaw}".format( **orientation)) rospy.loginfo( "imu/orientation/degrees: p: {pitch}, r: {roll}, y: {yaw}". format(**orientation_deg)) rospy.loginfo( "imu/orientation/radians: p: {pitch}, r: {roll}, y: {yaw}". format(**orientation_rad)) rospy.loginfo("imu/orientation/north: North: %s" % north) self.imuPub.publish(imu_msg) self.imuRawPub.publish(imu_raw_msg) self.accelerometerPub.publish(accel_msg) self.accelerometerPitchPub.publish(accel_pitch_msg) self.accelerometerRollPub.publish(accel_roll_msg) self.accelerometerYawPub.publish(accel_yaw_msg) self.accelerometerRawPub.publish(accel_raw_msg) self.accelerometerRawXPub.publish(accel_raw_x_msg) self.accelerometerRawYPub.publish(accel_raw_y_msg) self.accelerometerRawZPub.publish(accel_raw_z_msg) self.gyroscopePub.publish(gyro_msg) self.gyroscopePitchPub.publish(gyro_pitch_msg) self.gyroscopeRollPub.publish(gyro_roll_msg) self.gyroscopeYawPub.publish(gyro_yaw_msg) self.gyroscopeRawPub.publish(gyro_raw_msg) self.gyroscopeRawXPub.publish(gyro_raw_x_msg) self.gyroscopeRawYPub.publish(gyro_raw_y_msg) self.gyroscopeRawZPub.publish(gyro_raw_z_msg) self.magnetometerPub.publish(north_msg) self.magnetometerRawPub.publish(compass_msg) self.orientationPub.publish(orientation_msg) self.orientationDegreePub.publish(orientation_degree_msg) self.orientationRadiansPub.publish(orientation_rad_msg) self.orientationNorthPub.publish(north_msg) self.rate.sleep()
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 CiosRaspberryHat: # # Constructor # # url string WebsocketのURL # channel_id string CiosのチャネルID # access_token string Ciosのアクセストークン # def __init__(self, url, channel_id, access_token): self.url = url self.channel = channel_id self.token = access_token self.ws = 0 self.sense = SenseHat() # SenseHat インスタンス作成 self.sense.set_imu_config(True, True, True) # 加速度センサーの有効化 self.connectCount = 0 self.screen = "top" # # connection # # WebSocketへのコネクションを貼る # def connection(self): try: ws_url = self.url + "?" + "channel_id=" + self.channel + "&access_token=" + self.token print("--------------- WebSocket Connection ---------------") print("ConnectionURL: " + ws_url) print("--------------- WebSocket Connection ---------------") self.sense.show_letter("C", text_colour=[0, 255, 255]) self.ws = create_connection(ws_url) except: print("Websocket Connection Error...") self.sense.show_letter("E", text_colour=[255, 0, 0]) self.errorReConnect() return "error" # # getSensorData # # SenseHatからのデータを取得、JSON整形を行う # return: Json String # def getSensorData(self): try: humidity = self.sense.get_humidity() temp = self.sense.get_temperature() pressure = self.sense.get_pressure() orientation = self.sense.get_orientation_degrees() compass = self.sense.get_compass_raw() gyroscope = self.sense.get_gyroscope_raw() accelerometer = self.sense.get_accelerometer_raw() message = { "humidity": humidity, "temperature": temp, "pressure": pressure, "degrees_p": orientation["pitch"], "degrees_r": orientation["roll"], "degrees_y": orientation["yaw"], "compass_x": compass["x"], "compass_y": compass["y"], "compass_z": compass["z"], "gyroscope_x": gyroscope["x"], "gyroscope_y": gyroscope["y"], "gyroscope_z": gyroscope["z"], "accelerometer_x": accelerometer["x"], "accelerometer_y": accelerometer["y"], "accelerometer_z": accelerometer["z"] } send_message = json.dumps(message) return send_message except: print("getSensorData Error...") self.ws.close() self.sense.show_letter("E", text_colour=[255, 0, 0]) self.errorReConnect() return "error" # # sendMessage # # message string 送信するメッセージ(Json形式) # def sendMessage(self, message): try: print("--------------- WebSocket Send Message ---------------") print(message) print("--------------- WebSocket Send Message ---------------") self.ws.send(message) except: print("Websocket Send Error...") self.ws.close() self.sense.show_letter("E", text_colour=[255, 0, 0]) self.errorReConnect() return "error" # # Error ReConnect WebSocket # # message string 送信するメッセージ(Json形式) # def errorReConnect(self): try: if self.connectCount > 0: # ErrorCount カウント数以上のエラーで停止 raise self.connectCount += 1 for v in range(self.connectCount): # Error数に応じて、待機時間を追加 print("Wait ::: " + str(self.connectCount - v) + " sec") time.sleep(1) self.connection() except: print("Websocket connection Error count : " + str(self.connectCount) + " Over") self.sense.show_letter("E", text_colour=[255, 0, 0]) time.sleep(0.5) self.sense.show_letter("E", text_colour=[0, 255, 0]) time.sleep(0.5) self.sense.show_letter("E", text_colour=[0, 0, 255]) time.sleep(0.5) self.sense.show_letter("E", text_colour=[255, 0, 0]) time.sleep(2) self.sense.clear() exit()
#!/usr/bin/python import sys import time from sense_hat import SenseHat sense = SenseHat() sense.set_imu_config(False, True, False) # gyroscope only sense.clear([0, 0, 0]) pitch_n = 0 roll_n = 0 x = 3 y = 3 orientation = sense.get_orientation_degrees() # read offsets pitch_ofs = 180 #int(orientation["pitch"]) roll_ofs = 180 #int(orientation["roll"]) while 1: orientation = sense.get_orientation_degrees() #print("p: {pitch}, r: {roll}, y: {yaw}".format(**orientation)) pitch = (orientation["pitch"] + 180) % 360 roll = (orientation["roll"] - 180) % 360 d_pitch = pitch_n - pitch d_roll = roll_n - roll pitch_n = pitch roll_n = roll
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):
async def handle(self, context: RequestContext, responder: BaseResponder): """ Message handler logic for basic messages. Args: context: request context responder: responder callback """ self._logger.debug(f"ReadSensorHandler called with context {context}") assert isinstance(context.message, ReadSensor) self._logger.info("Received read sensor: %s", context.message.sensors) sensors = context.message.sensors meta = {"sensors": sensors} conn_mgr = ConnectionManager(context) await conn_mgr.log_activity( context.connection_record, "read_sensor", context.connection_record.DIRECTION_RECEIVED, meta, ) await responder.send_webhook( "read_sensor", { "message_id": context.message._id, "sensors": sensors, "state": "received" }, ) sense = SenseHat() temperature = None humidity = None pressure = None orientation = None accelerometer = None compass = None gyroscope = None stick_events = None pixels = None if "temperature" in sensors: temperature = sense.get_temperature() if "humidity" in sensors: humidity = sense.get_humidity() if "pressure" in sensors: pressure = sense.get_pressure() if "orientation" in sensors: orientation = sense.get_orientation_degrees() if "accelerometer" in sensors: accelerometer = sense.get_accelerometer_raw() if "compass" in sensors: compass = sense.get_compass_raw() if "gyroscope" in sensors: gyroscope = sense.get_gyroscope_raw() if "stick_events" in sensors: stick_events = [] stick_event_objects = sense.stick.get_events() for event in stick_event_objects: event_dict = {} event_dict['timestamp'] = event.timestamp event_dict['direction'] = event.direction event_dict['action'] = event.action stick_events.append(event_dict) if "pixels" in sensors: pixels = sense.get_pixels() reply_msg = SensorValue(temperature=temperature, humidity=humidity, pressure=pressure, orientation=orientation, accelerometer=accelerometer, compass=compass, gyroscope=gyroscope, stick_events=stick_events, pixels=pixels) await responder.send_reply(reply_msg) await conn_mgr.log_activity( context.connection_record, "sensor_value", context.connection_record.DIRECTION_SENT, {"content": "reply"}, )