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)
Esempio n. 4
0
    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()
Esempio n. 5
0
File: roll.py Progetto: aminsaied/pi
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
Esempio n. 8
0
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)
Esempio n. 9
0
    #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)  #只启用加速度计
Esempio n. 10
0
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
Esempio n. 12
0
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
Esempio n. 13
0
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
Esempio n. 14
0
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
Esempio n. 16
0
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
Esempio n. 17
0
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
Esempio n. 18
0
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
Esempio n. 19
0
#!/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()
Esempio n. 20
0
            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":
Esempio n. 21
0
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