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