class Controller(object):
def __init__(self, drone):
self.sense = SenseHat()
self.drone = drone
self.landed = True
#self.sense.set_imu_config(False, True, False)
self.sense.stick.direction_up = self.pushed_up
self.sense.stick.direction_middle = self.pushed_middle
self.sense.stick.direction_down = self.pushed_down
def pushed_middle(self, event):
if (event.action != ACTION_RELEASED):
#print("Midten er trykket")
self.sense.set_pixels(poin_hal)
self.drone.stop()
def pushed_up(self, event):
if (event.action == ACTION_HELD and self.landed):
#print("Op er trykket")
self.sense.set_pixels(poin_up)
self.drone.takeOff()
self.landed = False
def pushed_down(self, event):
if (event.action == ACTION_HELD and not self.landed):
self.sense.set_pixels(poin_down)
self.drone.land()
self.landed = True
def check_roll(self):
roll = self.sense.get_gyroscope()['roll']
if (roll >= 45.0 and roll < 90.0):
self.drone.ascend_alt(20)
#time.sleep(0.5)
elif (roll <= 315.0 and roll > 270.0):
self.drone.descend_alt(20)
#time.sleep(0.5)
def check_pitch(self):
pitch = self.sense.get_gyroscope()['pitch']
if (pitch >= 45.0 and pitch < 90.0):
self.drone.ccw(5)
#time.sleep(0.5)
elif (pitch <= 315.0 and pitch > 270.0):
self.drone.cw(5)
def _init_(self):
device_sensor = SenseHat()
self.device_id = 4
self.device_lon = random.uniform(-76.8, -77.2)
self.device_lat = random.uniform(38.75, 39.0)
self.device_last_time = time.time()
self.device_last_temp = device_sensor.get_gyroscope()
self.device_last_humidity = device_sensor.get_accelerometer()
self.device_last_mag = device_sensor.get_compass()
self.device_last_pressure = device_sensor.get_pressure()
return self
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'
def sense():
from sense_hat import SenseHat
import time
sense = SenseHat()
last_pitch = 0
last_roll = 0
last_yaw = 0
pitch_queue = collections.deque(maxlen=MAX_LEN)
roll_queue = collections.deque(maxlen=MAX_LEN)
yaw_queue = collections.deque(maxlen=MAX_LEN)
count = 0
canStart = False
time_drives = 0
count_drives = 0
while 1:
gyro = sense.get_gyroscope()
logger.log("p: {pitch}, r: {roll}, y: {yaw}".format(**gyro))
pitch_queue.append(gyro['pitch'] - last_pitch)
roll_queue.append(gyro['roll'] - last_roll)
yaw_queue.append(gyro['yaw'] - last_yaw)
last_pitch = gyro['pitch']
last_roll = gyro['roll']
last_yaw = gyro['yaw']
if count > MAX_LEN * 2:
canStart = True
count += 1
if canStart:
pitch_mean = get_mean(pitch_queue)
roll_mean = get_mean(roll_queue)
yaw_mean = get_mean(yaw_queue)
if pitch_mean > 0.7 or roll_mean > 0.5 or yaw_mean > 0.5:
print('driving')
time_drives = time.time()
count_drives += 1
else:
if count_drives > 10 and (time.time() - time_drives) > 15:
count_drives = 0
logger.log("not driving for a while")
return True
else:
print("not driving...")
time.sleep(0.4)
def sensehat(sensor):
from sense_hat import SenseHat
sense = SenseHat()
if (sensor == "mv_sensors"):
while True:
if (magnetometer == True):
sense.set_imu_config(True, False, False)
north = sense.get_compass()
print("direction {0:.1f}".format(north))
if (gyroscope == True):
sense.set_imu_config(False, True, False)
gyro_only = sense.get_gyroscope()
print("pitch: {pitch}".format(**sense.gyro))
print("yaw: {yaw}".format(**sense.gyro))
print("roll: {roll}".format(**sense.gyro))
if (accelerometer == True):
sense.set_imu_config(False, False, True)
raw = sense.get_accelerometer_raw()
print("ac_x: {x}".format(**raw))
print("ac_y: {y}".format(**raw))
print("ac_z: {z}".format(**raw))
elif (sensor == "temperature"):
for x in range(0, 3):
t1 = sense.get_temperature_from_humidity()
t2 = sense.get_temperature_from_pressure()
t = (t1 + t2) / 2
t_cpu = get_cpu_temp()
# calculates the real temperature compesating CPU heating
t_corr = t - ((t_cpu - t) / 1.6)
t_corr = get_smooth(t_corr)
mesurement = t_corr
elif (sensor == "humidity"):
#Approximating from Buck's formula Ha = Hm*(2.5-0.029*Tm)
temp = sense.get_temperature()
humidity = sense.get_humidity()
calchum = humidity * (2.5 - 0.029 * temp)
mesurement = calchum
elif (sensor == "pressure"):
mesurement = sense.get_pressure()
return mesurement
from sense_hat import SenseHat
import paho.mqtt.client as mqtt #importamos el cliente
import time
import json
sense = SenseHat()
#variables de entorno
humidity = sense.get_humidity()
temp = sense.get_temperature()
p_bar = sense.get_pressure()
#variables inerciales
orientation = sense.get_orientation_degrees()
compass = sense.get_compass()
giro = sense.get_gyroscope()
acce = sense.get_accelerometer()
#conversion de datos/ algunas funciones retornan un diccionario
# es necesario convertir a str para poder enviarlas por mqtt
giro_s = json.dumps(giro)
orientation_s = json.dumps(orientation)
acce_s = json.dumps(acce)
#print(type(giro_s))
#Enviroment variables
print("Variables ambientales:\nHumity: %s\ntemperature: %s\npressure: %s\n-----------------" %(humidity,temp,p_bar))
#inercial sensors
print("inercial:\norientation:%s\ncompass:%s\ngyroscope:%s\naccelerometer%s\n-------------" %(orientation,compass,giro,acce))
class pollingThread(QThread):
def __init__(self):
super().__init__()
def run(self):
self.db = QtSql.QSqlDatabase.addDatabase('QMYSQL')
self.db.setHostName("3.34.124.67")
self.db.setDatabaseName("16_3")
self.db.setUserName("16_3")
self.db.setPassword("1234")
ok = self.db.open()
print(ok)
if ok == False:
return
self.mh = Raspi_MotorHAT(addr=0x6f)
self.dcMotor = self.mh.getMotor(2)
self.speed = 100
self.adjust = 0
self.moveTime = 0
self.dcMotor.setSpeed(self.speed)
self.query = QtSql.QSqlQuery()
self.servo = self.mh._pwm
self.servo.setPWMFreq(60)
self.sense = SenseHat()
gyro = self.sense.get_gyroscope()
self.prev_roll = gyro["roll"]
accel = self.sense.get_accelerometer_raw()
self.init_y = accel["y"]
while True:
time.sleep(0.1)
self.getQuery()
self.setQuery()
def setQuery(self):
pressure = self.sense.get_pressure()
temp = self.sense.get_temperature()
humidity = self.sense.get_humidity()
gyro = self.sense.get_gyroscope()
accel = self.sense.get_accelerometer_raw()
p = round((pressure - 1000.0) / 100.0, 3)
t = round(temp / 100.0, 3)
h = round(humidity / 100.0, 3)
roll = gyro["roll"]
y = accel["y"]
self.speed = int(100 + (100 * (self.init_y - y)))
roll_diff = self.prev_roll - roll
if (roll_diff < -90):
roll_diff += 360
if (roll_diff > 90):
roll_diff -= 360
if (roll_diff >= 10 or roll_diff <= -10):
self.adjust = int(100 * (roll_diff / 180))
self.dcMotor.setSpeed(self.speed + self.adjust)
self.prev_roll = roll
self.query.prepare(
"insert into SENSING1 (TIME, NUM1, NUM2, NUM3, META_STRING, IS_FINISH) values (:time, :num1, :num2, :num3, :meta, :finish)"
)
time = QDateTime().currentDateTime()
met = str(round(gyro["pitch"], 3)) + "|" + str(round(
gyro["roll"], 3)) + "|" + str(round(gyro["yaw"], 3))
self.query.bindValue(":time", time)
self.query.bindValue(":num1", p)
self.query.bindValue(":num2", t)
self.query.bindValue(":num3", h)
self.query.bindValue(":meta", met)
self.query.bindValue(":finish", 0)
self.query.exec()
a = int((p * 1271) % 256)
b = int((t * 1271) % 256)
c = int((h * 1271) % 256)
self.sense.clear(a, b, c)
def getQuery(self):
query = QtSql.QSqlQuery(
"select * from COMMAND1 order by TIME desc limit 1")
query.next()
cmdTime = query.record().value(0)
cmdType = query.record().value(1)
cmdArg = query.record().value(2)
is_finish = query.record().value(3)
if is_finish == 0:
print(cmdTime.toString(), cmdType, cmdArg)
args = cmdArg.split()
if len(args) == 2 and args[1] == "sec":
self.moveTime = int(args[0])
query = QtSql.QSqlQuery(
"update COMMAND1 set IS_FINISH = 1 where IS_FINISH = 0")
if cmdType == "go":
self.go()
if cmdType == "back":
self.back()
if cmdType == "left":
self.left()
if cmdType == "right":
self.right()
if cmdType == "mid":
self.middle()
if cmdType == "front" and cmdArg == "press":
self.middle()
self.forward()
if cmdType == "leftside" and cmdArg == "press":
self.left()
self.forward()
if cmdType == "rightside" and cmdArg == "press":
self.right()
self.forward()
if cmdType == "front" and cmdArg == "release":
self.stop()
if cmdType == "leftside" and cmdArg == "release":
self.stop()
if cmdType == "rightside" and cmdArg == "release":
self.stop()
def stop(self):
print("MOTOR STOP")
self.dcMotor.run(Raspi_MotorHAT.RELEASE)
def forward(self):
print("FORWARD")
self.dcMotor.run(Raspi_MotorHAT.FORWARD)
def go(self):
print("GO")
self.dcMotor.run(Raspi_MotorHAT.FORWARD)
time.sleep(self.moveTime)
self.dcMotor.run(Raspi_MotorHAT.RELEASE)
def back(self):
print("BACK")
self.dcMotor.run(Raspi_MotorHAT.BACKWARD)
time.sleep(self.moveTime)
self.dcMotor.run(Raspi_MotorHAT.RELEASE)
def steer(self, angle=0):
if angle <= -60:
angle = -60
if angle >= 60:
angle = 60
pulse_time = 200 + (614 - 200) // 180 * (angle + 90)
self.servo.setPWM(0, 0, pulse_time)
def left(self):
print("LEFT")
self.steer(-30)
def right(self):
print("RIGHT")
self.steer(30)
def middle(self):
print("MIDDLE")
self.steer(0)
print("1.Temperature")
print("2.Humidity")
print("3.Barometric Pressure")
option = int(input("Please choose an option: "))
if option == 1:
sense = SenseHat()
temp = sense.get_temperature()
print("Temperature: %s C" % temp)
temp = round(temp)
temp = str(temp)
sense.show_message(temp, text_colour=[255, 0, 0])
if option == 2:
humidity = sense.get_humidity()
print("Humidity: %s %%rH" % humidity)
humidity = str(humidity)
sense.show_message(humidity, text_colour=[255, 0, 0])
if option == 3:
pressure = sense.get_pressure()
print("Pressure: %s Millibars" % pressure)
pressure = str(pressure)
sense.show_message(pressure, text_colour=[255, 0, 0])
if option == 4:
while True:
gyro_only = sense.get_gyroscope()
print("p: {pitch}, r: {roll}, y: {yaw}".format(**gyro_only))
membar.min = 0.0
membar.max = 100.0
yawbar = HatBar(2, sense, [64, 64, 64])
rollbar = HatBar(3, sense, [64, 64, 64])
pitchbar = HatBar(4, sense, [64, 64, 64])
tempbar = HatBar(5, sense, [128, 128, 0])
humbar = HatBar(6, sense, [64, 0, 64])
prbar = HatBar(7, sense, [0, 128, 128])
while(True):
cpubar.draw(getCPU())
membar.draw(getRAM())
orientation = sense.get_gyroscope()
yawbar.draw(orientation['yaw'])
rollbar.draw(orientation['roll'])
pitchbar.draw(orientation['pitch'])
tempbar.draw(sense.get_temperature())
humbar.draw(sense.get_pressure())
prbar.draw(sense.get_humidity())
print(cpubar.curr, membar.curr, tempbar.curr, humbar.curr, prbar.curr, sep="|")
# Print the actual values to stdout
# print(cpubar.curr, membar.curr, sep="|")
# Wait
from sense_hat import SenseHat
from time import sleep
sense = SenseHat()
r = (255, 0, 0)
g = (0, 255, 0)
b = (0, 0, 255)
sense.set_imu_config(False, True, False)
raw = sense.get_gyroscope()
print("p: {pitch}, r: {roll}, y: {yaw}".format(**raw))
new_raw = {}
for item in raw.keys():
new_raw[item] = round(raw[item], 1)
X = [255, 0, 0] # Red
O = [0, 0, 0] # White
question_mark = [
O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O,
O, O, X, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O,
O, O, O, O, O, O, O, O, O, O, O, O, O, O
]
def printBall(r, c):
ball_position = r * 8 + c
for key, value in enumerate(question_mark):
# you need to turn off the magnetometer and accelerometer
sense.set_imu_config(False, True, False)
while True:
# Obtain gyroscope raw xyz axis data
# raw = sense.gyro_raw
# raw = sense.gyroscope_raw
# raw = sense.get_gyroscope_raw()
# Read radians value of imu Chip,roll->x, pitch->y, yaw->z
# Need to match the set_imu_config settings, the content displayed
# gyr_radians = sense.orientation_radians
# gyr_radians = sense.get_orientation_radians()
# Read the angle of the imu chip, set_imu_config modify the settings
# gyr_degrees = sense.orientation
# gyr_degrees = sense.get_orientation()
# The angle of the read-only gyroscope, starting at each start position (0,0,0)
# gyr_degrees = sense.gyro
# gyr_degrees = sense.gyroscope
gyr_degrees = sense.get_gyroscope()
# Print data
# print("x: {x}, y: {y}, z: {z}".format(**raw))
print("p_deg: {pitch}, r_deg: {roll}, y_deg: {yaw}".format(**gyr_degrees))
# print("p_rad: {pitch}, r_rad: {roll}, y_rad: {yaw}".format(**gyr_radians))
# Delay 0.2 seconds to avoid sending too fast
time.sleep(0.2)
sense = SenseHat()
sense.clear((0, 255, 0))
sense.low_light = True
GAIN = 2
client_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
#client_socket.settimeout(1.0)
addr = ("X.X.X.X", 12001)
values = [0]*4
while True:
# start = time.time()
# Read all the ADC channel values in a list.
for i in range(4):
values[i] = (adc.read_adc(i, gain=GAIN))
gyro = dict.values(sense.get_gyroscope())
accelero = dict.values(sense.get_accelerometer())
compass = (sense.get_compass())
# end = time.time()
message = "%i %i %i %i %.2f %.2f %.2f %.2f %.2f %.2f %.2f" %(values[0], values[1], values[2], values[3], gyro[0], accelero[0], gyro[1], accelero[1], gyro[2], accelero[2], compass)
client_socket.sendto(message, addr)
# print(end - start)
#time.sleep(0.5)
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'] )
print( "x: %.4f" % sensehat.get_accelerometer_raw( )['x']
+ " , y: %.4f" % sensehat.get_accelerometer_raw( )['y']
+ " , Z: %.4f" % sensehat.get_accelerometer_raw( )['z'] )
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
def update_gyro(self):
device_sensor = SenseHat()
self.device_last_humidity = device_sensor.get_gyroscope()
self.device_last_time = time.time()
from sense_hat import SenseHat
sense = SenseHat()
gyro_only = sense.get_gyroscope()
while(True):
print("p: {pitch}, r: {roll}, y: {yaw}".format(**gyro_only))
# alternatives
#print(sense.gyro)
#print(sense.gyroscope)
import time
import datetime
from sense_hat import SenseHat
sense = SenseHat()
red = (255, 0, 0)
blue = (0, 0, 255)
while True:
temp = sense.get_temperature()
humidity = sense.get_humidity()
pressure = sense.get_pressure()
gyro = sense.get_gyroscope()
rawMag = sense.get_compass_raw()
timestamp = time.ctime()
accel_only = sense.get_accelerometer()
north = sense.get_compass()
print("____________________________________________________________________")
print("Temperature: %s C" % temp)
print("Humidity: %s %%rH" % humidity)
print("Pressure: %s Millibars" % pressure)
print("North: %s" % north)
print("Magnetometer x: {x}, y: {y}, z: {z}".format(**rawMag))
print("Gyro: p: {pitch}, r: {roll}, y: {yaw}".format(**gyro))
print("Accel: p: {pitch}, r: {roll}, y: {yaw}".format(**accel_only))
log_record = "%s : temp=%s humidity=%s pressure=%s gyro=%s accel=%s mag=%s" % (
timestamp,
def setSmiley(color=(255, 0, 0)):
X = color
O = (0, 0, 0)
pixels = [
O, O, O, O, O, O, O, O, O, X, X, O, O, X, X, O, O, X, X, O, O, X, X, O,
O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, O, X, O, O, O, O, X, O,
O, O, X, X, X, X, O, O, O, O, O, O, O, O, O, O
]
sense.set_pixels(pixels)
r = 255
g = 0
b = 0
print(sense.get_gyroscope()["yaw"])
while (True):
rotation = sense.get_gyroscope()["yaw"]
if 45 <= rotation < 135:
sense.set_rotation(180)
elif 135 <= rotation < 225:
sense.set_rotation(90)
elif 225 <= rotation < 315:
sense.set_rotation(0)
elif 315 <= rotation <= 360 or 0 <= rotation < 45:
sense.set_rotation(270)
if r == 255 and g < 255 and b == 0:
g += 5
elif r > 0 and g == 255 and b == 0:
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'],
orientation['pitch'],
orientation['yaw'],
compass,
def sense():
from sense_hat import SenseHat
sense = SenseHat()
while (1):
gyro_only = sense.get_gyroscope()
print("p: {pitch}, r: {roll}, y: {yaw}".format(**gyro_only))
hdlr.setFormatter(formatter)
logger.addHandler(hdlr)
logger.setLevel(logging.INFO)
sense.set_imu_config(True, True, True)
sense.set_pixel(0, 0, [255, 0, 0])
while (MeasureIsRunning is False):
# Use joystick for starting Measurement
for event in sense.stick.get_events():
if (event.action == 'pressed' and event.direction == 'middle'):
MeasureIsRunning = True
while (MeasureIsRunning is True):
sense.set_pixel(0, 0, [0, 255, 0])
dictValues = sense.get_gyroscope()
strMessage = "Gyr:"
strMessage += " Pitch: {0:.{digits}f}°".format(dictValues.get('pitch'),
digits=DIGITS)
strMessage += " Roll: {0:.{digits}f}°".format(dictValues.get('roll'),
digits=DIGITS)
strMessage += " Yaw: {0:.{digits}f}°".format(dictValues.get('yaw'),
digits=DIGITS)
logger.info(strMessage)
print(strMessage)
# Use joystick again for stopping Measurement
for event in sense.stick.get_events():
if (event.action == 'pressed' and event.direction == 'middle'):
MeasureIsRunning = False
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()
humidity = fedora.get_humidity()
temp = fedora.get_temperature()
temp_from_humidity = fedora.get_temperature_from_humidity()
temp_from_pressure = fedora.get_temperature_from_pressure()
pressure = fedora.get_pressure()
#get orientation
orientation = fedora.get_orientation()
orientation_pitch = orientation.get('pitch')
orientation_roll = orientation.get('roll')
orientation_yaw = orientation.get('yaw')
#get compass
compass = fedora.get_compass()
#get gyroscope
gyro = fedora.get_gyroscope()
gyro_pitch = gyro.get('pitch')
gyro_roll = gyro.get('roll')
gyro_yaw = gyro.get('yaw')
#get accelerometer
accelerometer = fedora.get_accelerometer()
accelerometer_pitch = accelerometer.get('pitch')
accelerometer_roll = accelerometer.get('roll')
accelerometer_yaw = accelerometer.get('yaw')
#write to csv
with open("atmosphere.csv", 'a') as atm:
data_writer = csv.DictWriter(atm, fieldnames=fields)
data_writer.writerow({
'humidity': humidity,
b = blue
speed = 0.06
while 1:
message = "Michigan"
sense.show_message(message, speed, text_colour=yellow, back_colour=blue)
t = round(sense.get_temperature(),1)
p = round(sense.get_pressure(),1)
h = round(sense.get_humidity(),1)
msg = "Temperature: {0}, Pressure: {1}, Humidity: {2}".format(t,p,h)
print(msg)
#o = sense.get_orientation()
#print("pitch: {pitch}, Yaw: {yaw}, Roll: {roll}".format(**o))
print(sense.get_gyroscope())
north = sense.get_compass()
print("North: %s" % north)
sleep(0.5)
message = "Go Big Blue"
sense.show_message(message, speed, text_colour=yellow, back_colour=blue)
sleep(0.5)
TheM = [
y,y,b,b,b,b,y,y,
b,y,y,b,b,y,y,b,
b,y,b,y,y,b,y,b,
class InputModule(AbstractInput):
""" A sensor support class that measures """
def __init__(self, input_dev, testing=False):
super(InputModule, self).__init__(input_dev,
testing=testing,
name=__name__)
self.sensor = None
if not testing:
self.initialize_input()
def initialize_input(self):
""" Initialize the Sense HAT sensor class """
from sense_hat import SenseHat
self.sensor = SenseHat()
def get_measurement(self):
""" Get measurements and store in the database """
if not self.sensor:
self.logger.error("Input not set up")
return
self.return_dict = copy.deepcopy(measurements_dict)
if self.is_enabled(0):
try:
self.value_set(0, self.sensor.get_temperature())
except Exception as e:
self.logger.error(
"Temperature (temperature sensor) read failure: {}".format(
e))
if self.is_enabled(1):
try:
self.value_set(1, self.sensor.get_temperature_from_humidity())
except Exception as e:
self.logger.error(
"Temperature (humidity sensor) read failure: {}".format(e))
if self.is_enabled(2):
try:
self.value_set(2, self.sensor.get_temperature_from_pressure())
except Exception as e:
self.logger.error(
"Temperature (pressure sensor) read failure: {}".format(e))
if self.is_enabled(3):
try:
self.value_set(3, self.sensor.get_humidity())
except Exception as e:
self.logger.error("Humidity read failure: {}".format(e))
if self.is_enabled(4):
try:
self.value_set(4, self.sensor.get_pressure())
except Exception as e:
self.logger.error("Pressure read failure: {}".format(e))
if self.is_enabled(5):
try:
self.value_set(5, self.sensor.get_compass())
except Exception as e:
self.logger.error("Compass read failure: {}".format(e))
if self.is_enabled(6) or self.is_enabled(7) or self.is_enabled(8):
magnetism = self.sensor.get_compass_raw()
if self.is_enabled(6):
try:
self.value_set(6, magnetism["x"])
except Exception as e:
self.logger.error(
"Compass raw x read failure: {}".format(e))
if self.is_enabled(7):
try:
self.value_set(7, magnetism["y"])
except Exception as e:
self.logger.error(
"Compass raw y read failure: {}".format(e))
if self.is_enabled(8):
try:
self.value_set(8, magnetism["z"])
except Exception as e:
self.logger.error(
"Compass raw z read failure: {}".format(e))
if self.is_enabled(9) or self.is_enabled(10) or self.is_enabled(11):
gyroscope = self.sensor.get_gyroscope()
if self.is_enabled(9):
try:
self.value_set(9, gyroscope["pitch"])
except Exception as e:
self.logger.error(
"Gyroscope pitch read failure: {}".format(e))
if self.is_enabled(10):
try:
self.value_set(10, gyroscope["roll"])
except Exception as e:
self.logger.error(
"Gyroscope roll read failure: {}".format(e))
if self.is_enabled(11):
try:
self.value_set(11, gyroscope["yaw"])
except Exception as e:
self.logger.error(
"Gyroscope yaw read failure: {}".format(e))
if self.is_enabled(12) or self.is_enabled(13) or self.is_enabled(14):
acceleration = self.sensor.get_accelerometer_raw()
if self.is_enabled(12):
try:
self.value_set(12, acceleration["x"])
except Exception as e:
self.logger.error(
"Acceleration x read failure: {}".format(e))
if self.is_enabled(13):
try:
self.value_set(13, acceleration["y"])
except Exception as e:
self.logger.error(
"Acceleration y read failure: {}".format(e))
if self.is_enabled(14):
try:
self.value_set(14, acceleration["z"])
except Exception as e:
self.logger.error(
"Acceleration z read failure: {}".format(e))
return self.return_dict
t = round(t, 3)
p = round(p, 3)
h = round(h, 3)
msg = "Temp is %s, Pres is %s, Humidity is %s" % (t, p, h)
print(msg)
#sense.show_message(msg,scroll_speed = 0.05)
#x,y,z = sense.get_accelerometer_raw().values()
x, y, z = sense.get_accelerometer().values()
x = round(x, 3)
y = round(y, 3)
z = round(z, 3)
msg = "X is %s, Y is %s, Z is %s" % (x, y, z)
print(msg)
#sense.show_message(msg,scroll_speed = 0.05)
#gyro_x,gyro_y,gyro_z = sense.get_gyroscope_raw().values()
gyro_x, gyro_y, gyro_z = sense.get_gyroscope().values()
msg = "Gyro X is %s, Gyro Y is %s, Gyro Z is %s" % (gyro_x, gyro_y, gyro_z)
print(msg)
print("Read from module")
#sensor_readings = get_sense_data()
sensor_readings = sensor_data.get_sense_data()
#print(sensor_readings)
queue_size=10)
try:
rospy.loginfo(rospy.get_caller_id() + " Starting...")
while not rospy.is_shutdown():
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)
text_colour=[255, 0, 0])
mdm_connected = False
mdm_rsrp = mdm_rssi = 0
# Try to make the IP traffic use the cellular modem which should which show up as eth1
subprocess.call("sudo route add default eth1", shell=True)
else:
mdm_rsrp = mdm_rssi = 0
mdm_connected = True
# Read PI HAT Sensor data to send to the Flow program via an http get request
tempstr = str(round(sense.temp))
humiditystr = str(round(sense.humidity))
pressurestr = str(round(sense.pressure))
accelZ = accelY = accelX = 0
for i in range(1, NUM_POSITION_READINGS_AVG + 1):
orientation = sense.get_gyroscope()
accelZ += -180 + orientation["pitch"]
accelY += -180 + orientation["roll"]
accelX += -180 + orientation["yaw"]
accelXstr = str(round(accelX / i))
accelYstr = str(round(accelY / i))
accelZstr = str(round(accelZ / i))
#print accelXstr, accelYstr, accelZstr
# Button time!
button1 = button2 = button3 = button4 = button5 = "0"
buttons = sense.stick.get_events()
for button_events in buttons:
#print button_events.action
#print button_events.direction
if button_events.action == "pressed":
