def main():
try:
#Create your Phidget channels
accelerometer0 = Accelerometer()
gyroscope0 = Gyroscope()
magnetometer0 = Magnetometer()
#Set addressing parameters to specify which channel to open (if any)
#Assign any event handlers you need before calling open so that no events are missed.
accelerometer0.setOnAccelerationChangeHandler(onAccelerationChange)
gyroscope0.setOnAngularRateUpdateHandler(onAngularRateUpdate)
magnetometer0.setOnMagneticFieldChangeHandler(onMagneticFieldChange)
#Open your Phidgets and wait for attachment
accelerometer0.openWaitForAttachment(5000)
gyroscope0.openWaitForAttachment(5000)
magnetometer0.openWaitForAttachment(5000)
#Do stuff with your Phidgets here or in your event handlers.
time.sleep(5)
#Close your Phidgets once the program is done.
accelerometer0.close()
gyroscope0.close()
magnetometer0.close()
except PhidgetException as ex:
#We will catch Phidget Exceptions here, and print the error informaiton.
traceback.print_exc()
print("")
print("PhidgetException " + str(ex.code) + " (" + ex.description +
"): " + ex.details)
def __init__(self):
self.kalmanX = KalmanAngle()
self.kalmanY = KalmanAngle()
self.RestrictPitch = True #Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
self.radToDeg = 57.2957786
self.kalAngleX = 0
self.kalAngleY = 0
try:
self.accel = Accelerometer() #ch.getAcceleration()
self.gyro = Gyroscope() #ch.getAngularRate()
except Exception as e:
print(
"Cannot initalize, try checking you have the Phidget22 library installed."
)
print("Error:", e)
sys.exit()
def plugin_reconfigure(handle, new_config):
""" Reconfigures the plugin
Args:
handle: handle returned by the plugin initialisation call
new_config: JSON object representing the new configuration category for the category
Returns:
new_handle: new handle to be used in the future calls
"""
_LOGGER.info("Old config for wind_turbine plugin {} \n new config {}".format(handle, new_config))
# Shutdown sensors
try:
handle['humidity'].close()
handle['temperature'].close()
handle['current'].close()
handle['encoder'].close()
handle['accelerometer'].close()
handle['gyroscope'].close()
handle['magnetometer'].close()
except Exception as ex:
_LOGGER.exception("wind_turbine exception: {}".format(str(ex)))
raise ex
time.sleep(5)
new_handle = copy.deepcopy(new_config)
try:
# check if temp/humidity sensor is enabled. If so restart it
if new_handle['tempHumEnable']['value'] == 'true':
new_handle['humidity'] = HumiditySensor()
new_handle['humidity'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['humidity'].setHubPort(int(new_handle['tempHumPort']['value']))
new_handle['humidity'].setIsHubPortDevice(False)
new_handle['humidity'].setChannel(0)
new_handle['humidity'].openWaitForAttachment(5000)
try:
new_handle['humidity'].getHumidity()
except Exception:
pass
new_handle['temperature'] = TemperatureSensor()
new_handle['temperature'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['temperature'].setHubPort(int(new_handle['tempHumPort']['value']))
new_handle['temperature'].setIsHubPortDevice(False)
new_handle['temperature'].setChannel(0)
new_handle['temperature'].openWaitForAttachment(5000)
try:
new_handle['temperature'].getTemperature()
except Exception:
pass
# check if current sensor is enabled, if so restart it
if new_handle['currentEnable']['value'] == 'true':
new_handle['current'] = CurrentInput()
new_handle['current'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['current'].setHubPort(int(new_handle['currentPort']['value']))
new_handle['current'].setIsHubPortDevice(False)
new_handle['current'].setChannel(0)
new_handle['current'].openWaitForAttachment(5000)
try:
new_handle['current'].getCurrent()
except Exception:
pass
# check if encoder sensor is enabled
if new_handle['encoderEnable']['value'] == 'true':
new_handle['encoder'] = Encoder()
new_handle['encoder'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['encoder'].setHubPort(int(new_handle['encoderPort']['value']))
new_handle['encoder'].setIsHubPortDevice(False)
new_handle['encoder'].setChannel(0)
new_handle['encoder'].openWaitForAttachment(5000)
new_handle['encoder'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['encoder'].getPosition()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if accelerometer is enabled
if new_handle['accelerometerEnable']['value'] == 'true':
new_handle['accelerometer'] = Accelerometer()
new_handle['accelerometer'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['accelerometer'].setHubPort(int(new_handle['spatialPort']['value']))
new_handle['accelerometer'].setIsHubPortDevice(False)
new_handle['accelerometer'].setChannel(0)
new_handle['accelerometer'].openWaitForAttachment(5000)
new_handle['accelerometer'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['accelerometer'].getAcceleration()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if gyroscope is enabled
if new_handle['gyroscopeEnable']['value'] == 'true':
new_handle['gyroscope'] = Gyroscope()
new_handle['gyroscope'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['gyroscope'].setHubPort(int(new_handle['spatialPort']['value']))
new_handle['gyroscope'].setIsHubPortDevice(False)
new_handle['gyroscope'].setChannel(0)
new_handle['gyroscope'].openWaitForAttachment(5000)
new_handle['gyroscope'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['gyroscope'].getAngularRate()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if magnetometer enable is enabled
if new_handle['magnetometerEnable']['value'] == 'true':
new_handle['magnetometer'] = Magnetometer()
new_handle['magnetometer'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['magnetometer'].setHubPort(int(new_handle['spatialPort']['value']))
new_handle['magnetometer'].setIsHubPortDevice(False)
new_handle['magnetometer'].setChannel(0)
new_handle['magnetometer'].openWaitForAttachment(5000)
new_handle['magnetometer'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['magnetometer'].getMagneticField()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if hub has changed, if so init restart
if new_handle['hubSN']['value'] != handle['hubSN']['value']:
new_handle['restart'] = 'yes'
else:
new_handle['restart'] = 'no'
except Exception as ex:
_LOGGER.exception("wind_turbine exception: {}".format(str(ex)))
raise ex
# counter to know when to run process
new_handle['tempHumCount'] = 0
new_handle['currentCount'] = 0
new_handle['encoderCount'] = 0
new_handle['accelerometerCount'] = 0
new_handle['gyroscopeCount'] = 0
new_handle['magnetometerCount'] = 0
# counter of last encoder value
new_handle['encoderPreviousValue'] = handle['encoderPreviousValue']
return new_handle
def plugin_init(config):
""" Initialise the plugin.
Args:
config: JSON configuration document for the South plugin configuration category
Returns:
data: JSON object to be used in future calls to the plugin
Raises:
"""
try:
data = copy.deepcopy(config)
if data['tempHumEnable']['value'] == 'true':
data['humidity'] = HumiditySensor()
data['humidity'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['humidity'].setHubPort(int(data['tempHumPort']['value']))
data['humidity'].setIsHubPortDevice(False)
data['humidity'].setChannel(0)
data['humidity'].openWaitForAttachment(5000)
try:
data['humidity'].getHumidity()
except Exception:
pass
data['temperature'] = TemperatureSensor()
data['temperature'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['temperature'].setHubPort(int(data['tempHumPort']['value']))
data['temperature'].setIsHubPortDevice(False)
data['temperature'].setChannel(0)
data['temperature'].openWaitForAttachment(5000)
try:
data['temperature'].getTemperature()
except Exception:
pass
if data['currentEnable']['value'] == 'true':
data['current'] = CurrentInput()
data['current'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['current'].setHubPort(int(data['currentPort']['value']))
data['current'].setIsHubPortDevice(False)
data['current'].setChannel(0)
data['current'].openWaitForAttachment(5000)
try:
data['current'].getCurrent()
except Exception:
pass
if data['encoderEnable']['value'] == 'true':
data['encoder'] = Encoder()
data['encoder'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['encoder'].setHubPort(int(data['encoderPort']['value']))
data['encoder'].setIsHubPortDevice(False)
data['encoder'].setChannel(0)
data['encoder'].openWaitForAttachment(5000)
data['encoder'].setDataInterval(20)
i = 0
while i < 120:
try:
data['encoder'].getPosition()
except Exception:
time.sleep(1)
i += 1
else:
break
if data['accelerometerEnable']['value'] == 'true':
data['accelerometer'] = Accelerometer()
data['accelerometer'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['accelerometer'].setHubPort(int(data['spatialPort']['value']))
data['accelerometer'].setIsHubPortDevice(False)
data['accelerometer'].setChannel(0)
data['accelerometer'].openWaitForAttachment(5000)
data['accelerometer'].setDataInterval(20)
i = 0
while i < 120:
try:
data['accelerometer'].getAcceleration()
except Exception:
time.sleep(1)
i += 1
else:
break
if data['gyroscopeEnable']['value'] == 'true':
data['gyroscope'] = Gyroscope()
data['gyroscope'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['gyroscope'].setHubPort(int(data['spatialPort']['value']))
data['gyroscope'].setIsHubPortDevice(False)
data['gyroscope'].setChannel(0)
data['gyroscope'].openWaitForAttachment(5000)
data['gyroscope'].setDataInterval(20)
i = 0
while i < 120:
try:
data['gyroscope'].getAngularRate()
except Exception:
time.sleep(1)
i += 1
else:
break
if data['magnetometerEnable']['value'] == 'true':
data['magnetometer'] = Magnetometer()
data['magnetometer'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['magnetometer'].setHubPort(int(data['spatialPort']['value']))
data['magnetometer'].setIsHubPortDevice(False)
data['magnetometer'].setChannel(0)
data['magnetometer'].openWaitForAttachment(5000)
data['magnetometer'].setDataInterval(20)
i = 0
while i < 120:
try:
data['magnetometer'].getMagneticField()
except Exception:
time.sleep(1)
i += 1
else:
break
except Exception as ex:
_LOGGER.exception("wind_turbine exception: {}".format(str(ex)))
raise ex
# counter to know when to run process
data['tempHumCount'] = 0
data['currentCount'] = 0
data['encoderCount'] = 0
data['accelerometerCount'] = 0
data['gyroscopeCount'] = 0
data['magnetometerCount'] = 0
# counter of last encoder value
data['encoderPreviousValue'] = 0
data['encoderPreviousTime'] = 0
return data
i = 0
while i < 120:
try:
print(accelerometer.getAcceleration())
except Exception as e:
time.sleep(1)
i+=1
else:
break
if i == 120:
# exit
pass
print(accelerometer.getAcceleration())
"""
gyroscope = Gyroscope()
gyroscope.setDeviceSerialNumber(538854)
gyroscope.setHubPort(2)
gyroscope.setIsHubPortDevice(False)
gyroscope.setChannel(0)
gyroscope.openWaitForAttachment(5000)
i = 0
while i < 120:
try:
print(gyroscope.getAngularRate())
except Exception as e:
time.sleep(1)
i+=1
else:
break
#########################################################################
# Event Manager Definitions
#########################################################################
t = []
t_gyro = []
ac0 = []
ac1 = []
ac2 = []
gr0 = []
gr1 = []
gr2 = []
try:
ch = Accelerometer()
Gyro = Gyroscope()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
########################### Accelerometer Setup
def AccelerometerAttached(e):
try:
attached = e
print("\nAttach Event Detected (Information Below)")
print("===========================================")
print("Library Version: %s" % attached.getLibraryVersion())
print("Serial Number: %d" % attached.getDeviceSerialNumber())
import rospy
from sensor_msgs.msg import Imu # ROS msg type for IMU
from Phidget22.Devices.Gyroscope import *
from Phidget22.Devices.Accelerometer import *
from Phidget22.PhidgetException import *
from Phidget22.Phidget import *
from Phidget22.Net import *
rospy.init_node('imu')
pub = rospy.Publisher('imu_data', Imu, queue_size = 10)
if __name__ == __main__:
# Setting up Phidget object
try:
ch = Gyroscope()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
try:
ch.open()
except PhidgetException as e:
print (“Phidget Exception %i: %s” % (e.code, e.details))
# Opening ROS publisher
rospy.init('imu_data', anonymous=True)
r = rospy.Rate(20)
while not rospy.is_shutdown():
pub.publish()