class SenseHatCSV: ETX = [255, 0, 0] OFF = [0, 0, 0] # off BTX = [0, 255, 0] # on led_tx_on = [ BTX, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF ] led_tx_off = [ ETX, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF ] RADIANS = "RADIANS" ACCEL = "ACCEL" GYRO = "GYRO" def __init__(self): self.sense = SenseHat() self.sense.set_imu_config(True, True, True) def get_record_from_radians(self): imu_input = self.sense.get_orientation_radians() data = { 'xRoll': imu_input['pitch'], 'yPitch': imu_input['roll'], 'zYaw': imu_input['yaw'], 'timestamp': round(time.time() * 1000), } return data
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}]
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)) p = orad['pitch'] r = orad['roll'] y = orad['yaw'] drawDot(-p*5.0 + 3.5, r*5.0 + 3.5) # print (r) # sleep(1) #for a in range (0, 20): # sh.set_pixel (a, a, red) # sleep(1) # sh.clear() # drawDot (float(a+0.5),float(a+0.5)) # drawDot (a*0.1 + 3, a*0.15 + 3) # sleep(1)
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 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()
def main(): sensehat = SenseHat() sensehat.set_imu_config(True, True, True) velocityMatrix = [0.0, 0.0, 0.0] resultMatrixKeys = ['x', 'y', 'z'] velocityMatrix = dict(zip(resultMatrixKeys, velocityMatrix)) displacementX = 0 displacementY = 0 displacementZ = 0 sensehat.show_message("calibrating", 0.1, [100, 0, 0]) sensehat.show_letter('!', [100, 0, 0]) gCoeff = calculateGCoeff(sensehat) sensehat.show_message("Complete", 0.1, [0, 100, 0]) flag = True while flag: starttime = time.time() '''get raw data''' accel_raw = sensehat.get_accelerometer_raw() orient_rad = sensehat.get_orientation_radians() orient_deg = sensehat.get_orientation_degrees() external_temp = sensehat.get_temperature() pressure = sensehat.get_pressure() eulermatrixtrans = createTransformMatrix(orient_rad) accel_procc = removeGravity(accel_raw, eulermatrixtrans, gCoeff) accel_real = matrixMultiply3x9(accel_procc, eulermatrixtrans) endtime = time.time() '''calculate velocity change and displacement''' total_time = starttime - endtime '''if motion is detected we output distance moved''' if abs(accel_procc[0]) > 0.03 or abs(accel_procc[1]) > 0.03 or abs( accel_procc[2]) > 0.03: print( ("acceleration x = {0},y={1},z={2}, degrees to north = {3},{4}" .format(accel_real['x'], accel_real['y'], accel_real['z'], orient_rad, orient_deg))) config = { "apiKey": "AIzaSyA20qu9ddnRJPAQgGpn9ySQLuqjLH2WWPI", "authDomain": "firefightingmonitoringsystem.firebaseapp.com", "databaseURL": "https://firefightingmonitoringsystem.firebaseio.com/", "storageBucket": "firefightingmonitoringsystem.appspot.com" } fireBase = pyrebase.initialize_app(config) db = firebase.database() '''Need to add acceleration data''' data = { "external_temp": external_temp, "pressure": pressure, } results = db.push(data) print(results) return
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
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) sin_y = math.sin(yaw_total) cos_y = math.cos(yaw_total) sin_p = math.sin(pitch) cos_p = math.cos(pitch)
#print(sense.compass_raw) sense.set_imu_config(False, True, False) #只启用陀螺仪 roll, pitch, yaw = sense.get_orientation().values() #获取当前俯仰轴,滚转轴,偏航轴的角度信息 roll = round(roll, 2) pitch = round(pitch, 2) yaw = round(yaw, 2) print("Pitch Axis = {} , Roll Axis = {} , Yaw Axis = {}".format( pitch, roll, yaw)) orientation = sense.get_orientation() #获取当前俯仰轴,滚转轴,偏航轴的角度信息。 print("pitch = {}, roll = {}, yell = {}".format( round(orientation["pitch"], 2), round(orientation["roll"], 2), round(orientation["yaw"], 2))) orientation = sense.get_orientation_radians() #获取当前俯仰轴,滚转轴,偏航轴的弧度信息。 print("pitch = {}, roll = {}, yell = {}".format( round(orientation["pitch"], 2), round(orientation["roll"], 2), round(orientation["yaw"], 2))) #gyro_only = sense.get_gyroscope()#仅从陀螺仪获取当前方向 #print("p: {pitch}, r: {roll}, y: {yaw}".format(**gyro_only)) #print(sense.gyro) #print(sense.gyroscope) #raw = sense.get_gyroscope_raw()#获取原始的x,y和z轴陀螺仪数据 #print("x: {x}, y: {y}, z: {z}".format(**raw)) #print(sense.gyro_raw) #print(sense.gyroscope_raw) sense.set_imu_config(False, False, True) #只启用加速度计
sensehat.clear( sens_defn.V ) time.sleep( .1 ) sensehat.clear( ) time.sleep( .1 ) ### sensor print( sensehat.gamma ) print( "Humidity : %.4f %%rH" % sensehat.get_humidity( ) ) print( "Temp main: %.4f C" % sensehat.get_temperature( ) ) print( "Temp humd: %.4f C" % sensehat.get_temperature_from_humidity( ) ) print( "Temp prss: %.4f C" % sensehat.get_temperature_from_pressure( ) ) print( "Pressure : %.4f mb" % sensehat.get_pressure( ) ) # millibar sensehat.set_imu_config( True , True , True ) print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_orientation_radians( ) ) ) print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_orientation_degrees( ) ) ) print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_orientation( ) ) ) print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_gyroscope( ) ) ) print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_accelerometer( ) ) ) print( "North: %s" % sensehat.get_compass( ) ) print( sensehat.get_compass_raw( ) ) print( "x: {x} , y: {y} , z: {z}".format( **sensehat.get_compass_raw( ) ) ) print( "x: %.4f" % sensehat.get_compass_raw( )['x'] + " , y: %.4f" % sensehat.get_compass_raw( )['y'] + " , Z: %.4f" % sensehat.get_compass_raw( )['z'] ) print( "x: %.4f" % sensehat.get_gyroscope_raw( )['x'] + " , y: %.4f" % sensehat.get_gyroscope_raw( )['y'] + " , Z: %.4f" % sensehat.get_gyroscope_raw( )['z'] )
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
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
class OrientationSensor: def __init__(self): self.start = True self.sensehat = SenseHat() self.accel_procc = 0 def calculateGCoeff(self): g = [] samples = 100 for i in range(samples): accel_raw = self.sensehat.get_accelerometer_raw() orient_rad = self.sensehat.get_orientation_radians() eulermatrixtrans = self.createTransformMatrix(orient_rad) gX = accel_raw['x'] / eulermatrixtrans[0][2] gY = accel_raw['y'] / eulermatrixtrans[1][2] gZ = accel_raw['z'] / eulermatrixtrans[2][2] gVal = (gX + gY + gZ) / 3 g.append(gVal) sum = 0.0 for j in range(samples): sum += g[j] return sum / 100.0 def removeGravity(self, accel_raw, eulertransmatrix, g): rotated_gravity = [0.0, 0.0, 0.0, 0.0] rotated_gravity[0] = g * eulertransmatrix[0][2] rotated_gravity[1] = g * eulertransmatrix[1][2] rotated_gravity[2] = g * eulertransmatrix[2][2] ''' we can then calculate the impact of gravity with these quations''' x = accel_raw['x'] - rotated_gravity[0] y = accel_raw['y'] - rotated_gravity[1] z = accel_raw['z'] - rotated_gravity[2] return x, y, z def createTransformMatrix(self, orient_rad): '''We find the orientation of our device so that the orientation can match''' yaw = orient_rad['yaw'] pitch = orient_rad['pitch'] roll = orient_rad['roll'] sinRoll = math.sin(roll) cosRoll = math.cos(roll) sinPitch = math.sin(pitch) cosPitch = math.cos(pitch) sinYaw = math.sin(yaw) cosYaw = math.cos(yaw) eulervalues = { 'sinRoll': sinRoll, 'sinPitch': sinPitch, 'sinYaw': sinYaw, 'cosRoll': cosRoll, 'cosPitch': cosPitch, 'cosYaw': cosYaw, } '''we transform our euler matrix so we can use it to normalize our accelerations''' eulermatrixtrans = [] row3 = [ cosYaw * sinPitch * cosRoll + sinYaw * sinRoll, sinYaw * sinPitch * cosRoll - cosYaw * sinRoll, cosPitch * cosRoll ] row2 = [ cosYaw * sinPitch * sinRoll - sinYaw * cosRoll, sinYaw * sinPitch * sinRoll + cosYaw * cosRoll, cosPitch * sinRoll ] row1 = [cosYaw * sinPitch, sinYaw * cosPitch, sinPitch * (-1)] eulermatrixtrans.append(row1) eulermatrixtrans.append(row2) eulermatrixtrans.append(row3) return eulermatrixtrans def matrixMultiply3x9(self, firstMatrix, secondMatrix): resultMatrix = [0.0, 0.0, 0.0] resultMatrixKeys = ['x', 'y', 'z'] resultMatrix[0] = firstMatrix[0] * secondMatrix[0][0] + firstMatrix[ 1] * secondMatrix[1][0] + firstMatrix[2] * secondMatrix[2][0] resultMatrix[1] = firstMatrix[0] * secondMatrix[1][0] + firstMatrix[ 1] * secondMatrix[1][1] + firstMatrix[2] * secondMatrix[2][1] resultMatrix[2] = firstMatrix[0] * secondMatrix[2][0] + firstMatrix[ 1] * secondMatrix[1][2] + firstMatrix[2] * secondMatrix[2][2] return dict(zip(resultMatrixKeys, resultMatrix)) def mainOrientationLoop(self): self.sensehat.set_imu_config(True, True, True) velocityMatrix = [0.0, 0.0, 0.0] resultMatrixKeys = ['x', 'y', 'z'] velocityMatrix = dict(zip(resultMatrixKeys, velocityMatrix)) displacementX = 0 displacementY = 0 displacementZ = 0 gCoeff = self.calculateGCoeff() flag = True while not self.thread.stopped: starttime = time.time() '''get raw data''' accel_raw = self.sensehat.get_accelerometer_raw() orient_rad = self.sensehat.get_orientation_radians() orient_deg = self.sensehat.get_orientation_degrees() external_temp = self.sensehat.get_temperature() pressure = self.sensehat.get_pressure() eulermatrixtrans = self.createTransformMatrix(orient_rad) self.accel_procc = self.removeGravity(accel_raw, eulermatrixtrans, gCoeff) '''endtime = time.time()''' '''calculate velocity change and displacement''' '''total_time = starttime - endtime''' '''if motion is detected we output distance moved''' '''if abs(accel_procc[0]) > 0.03 or abs(accel_procc[1]) > 0.03 or abs(accel_procc[2]) > 0.03 : print(("acceleration x = {0},y={1},z={2}, degrees to north = {3},{4}" .format(accel_procc[0] ,accel_procc[1] ,accel_procc[2], orient_rad,orient_deg ))) ''' return def startAsyncOrientation(self): self.thread = threading.Thread(target=self.mainOrientationLoop) self.thread.stopped = False self.thread.start() return def stopAsyncOrientation(self): self.thread.stopped = true self.accel_procc = 0 return
class SenseHatREST: ETX = [255, 0, 0] OFF = [0, 0, 0] # off BTX = [0, 255, 0] # on led_tx_on = [ BTX, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF ] led_tx_off = [ ETX, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF ] RADIANS = "RADIANS" ACCEL = "ACCEL" GYRO = "GYRO" def __init__(self): self.sense = SenseHat() self.sense.set_imu_config(True, True, True) def get_record_from_radians(self): imu_input = self.sense.get_orientation_radians() data = { 'xRoll': imu_input['pitch'], 'yPitch': imu_input['roll'], 'zYaw': imu_input['yaw'], 'timestamp': round(time.time() * 1000), } return data def make_post_call(self, data, config): data['experimentId'] = config.get('experiment_id'), data['type'] = config.get('sensor_type'), data['sensorLocation'] = config.get('sensor_location'), data['subject'] = config.get('subject'), data['exercise'] = config.get('exercise'), data['isRaw'] = config.get('False'), data['timestamp'] = round(time.time() * 1000), data['deviceId'] = config.get('inputDeviceId') sense.set_pixels(led_tx_on) requests.post(config.get('restEndpoint'), json=data) sense.set_pixels(led_tx_off)
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
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 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
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="CS294.obj", name="model", x=0, y=-1, z=40, sx=7.5, sy=7.5, sz=7.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 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
#!/usr/bin/env python import rospy import time import math from sense_hat import SenseHat from std_msgs.msg import Header from tf.transformations import quaternion_from_euler from sensor_msgs.msg import Imu, Temperature, RelativeHumidity, FluidPressure sense = SenseHat() sense.set_imu_config(True, True, True) #compass, gyro, accele calibration = sense.get_orientation_radians() start_roll = calibration['yaw'] start_pitch = calibration['roll'] start_yaw = calibration['pitch '] start_time = int(time.time()) * 1000 def talker(): temp_pub = rospy.Publisher('rov/int_temperature', Temperature, queue_size = 1) #Publisher for the different sensors: Temperature, Humidity, Pressure, pressure_pub = rospy.Publisher('rov/int_pressure', FluidPressure, queue_size = 1) humidity_pub = rospy.Publisher('rov/int_humidity', RelativeHumidity, queue_size = 1) imu_pub = rospy.Publisher("rov/imu", Imu, queue_size = 1) #Imu publisher rate = rospy.Rate(60) while not rospy.is_shutdown(): global calibration, start_roll, start_pitch, start_yaw, start_time header = Header()
acc_vec = Vector3() acceleration = sense.get_accelerometer_raw() acc_vec.x = acceleration['x'] acc_vec.y = acceleration['y'] acc_vec.z = acceleration['z'] pub_accelerometer.publish(acc_vec) gyro_vec = Vector3() gyroscope = sense.get_gyroscope() gyro_vec.x = gyroscope['roll'] gyro_vec.y = gyroscope['pitch'] gyro_vec.z = gyroscope['yaw'] pub_gyroscope.publish(gyro_vec) ori_rad_pose = Quaternion() orientation_rad = sense.get_orientation_radians() quat = quaternion_from_euler(orientation_rad['roll'], orientation_rad['pitch'], orientation_rad['yaw']) ori_rad_pose.x = quat[0] ori_rad_pose.y = quat[1] ori_rad_pose.z = quat[2] ori_rad_pose.w = quat[3] pub_orientation_quat.publish(ori_rad_pose) pub_humidity.publish(sense.get_humidity()) pub_temperature.publish(sense.get_temperature()) pub_pressure.publish(sense.get_pressure()) for event in sense.stick.get_events(): if event.action == "pressed":
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