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()
class PhidgetKalman(object): #Read the gyro and acceleromater values from MPU6050 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() # return self.accel, self.gyro def start(self): try: self.accel.openWaitForAttachment(1000) self.gyro.openWaitForAttachment(1000) except Exception as e: print( "Issue with attaching to IMU. The IMU maybe connected to something else right now.." ) print("Error:", e) sys.exit() def stop(self): self.accel.close() self.gyro.close() print("Stopping") def get_angles(self, measure_time=5): #Keep going time.sleep(1) accX, accY, accZ = self.accel.getAcceleration() if (self.RestrictPitch): roll = math.atan2(accY, accZ) * self.radToDeg pitch = math.atan(-accX / math.sqrt((accY**2) + (accZ**2))) * self.radToDeg else: roll = math.atan(accY / math.sqrt((accX**2) + (accZ**2))) * self.radToDeg pitch = math.atan2(-accX, accZ) * self.radToDeg # print(roll) self.kalmanX.setAngle(roll) self.kalmanX.setAngle(pitch) gyroXAngle = roll gyroYAngle = pitch compAngleX = roll compAngleY = pitch timer = time.time() compare_timer = time.time() flag = 0 try: while int(time.time()) < int(measure_time) + int( compare_timer ): #Should only run for about 5 seconds <- Could make lower #Read Accelerometer raw value accX, accY, accZ = self.accel.getAcceleration() #Read Gyroscope raw value gyroX, gyroY, gyroZ = self.gyro.getAngularRate() dt = time.time() - timer timer = time.time() if (self.RestrictPitch): roll = math.atan2(accY, accZ) * self.radToDeg pitch = math.atan( -accX / math.sqrt((accY**2) + (accZ**2))) * self.radToDeg else: roll = math.atan( accY / math.sqrt((accX**2) + (accZ**2))) * self.radToDeg pitch = math.atan2(-accX, accZ) * self.radToDeg gyroXRate = gyroX / 131 gyroYRate = gyroY / 131 if (self.RestrictPitch): if ((roll < -90 and self.kalAngleX > 90) or (roll > 90 and self.kalAngleX < -90)): self.kalmanX.setAngle(roll) complAngleX = roll self.kalAngleX = roll gyroXAngle = roll else: self.kalAngleX = self.kalmanX.getAngle( roll, gyroXRate, dt) if (abs(self.kalAngleX) > 90): gyroYRate = -gyroYRate self.kalAngleY = self.kalmanX.getAngle( pitch, gyroYRate, dt) else: if ((pitch < -90 and self.kalAngleY > 90) or (pitch > 90 and self.kalAngleY < -90)): self.kalmanX.setAngle(pitch) complAngleY = pitch self.kalAngleY = pitch gyroYAngle = pitch else: self.kalAngleY = self.kalmanX.getAngle( pitch, gyroYRate, dt) if (abs(self.kalAngleY) > 90): gyroXRate = -gyroXRate self.kalAngleX = self.kalmanX.getAngle( roll, gyroXRate, dt) #angle = (rate of change of angle) * change in time gyroXAngle = gyroXRate * dt gyroYAngle = gyroYAngle * dt #compAngle = constant * (old_compAngle + angle_obtained_from_gyro) + constant * angle_obtained from accelerometer compAngleX = 0.93 * (compAngleX + gyroXRate * dt) + 0.07 * roll compAngleY = 0.93 * (compAngleY + gyroYRate * dt) + 0.07 * pitch if ((gyroXAngle < -180) or (gyroXAngle > 180)): gyroXAngle = self.kalAngleX if ((gyroYAngle < -180) or (gyroYAngle > 180)): gyroYAngle = self.kalAngleY self.X_angle = self.kalAngleX self.Y_angle = self.kalAngleY #print("Angle X: " + str(self.kalAngleX)+" " +"Angle Y: " + str(self.kalAngleY)) #print(str(roll)+" "+str(gyroXAngle)+" "+str(compAngleX)+" "+str(self.kalAngleX)+" "+str(pitch)+" "+str(gyroYAngle)+" "+str(compAngleY)+" "+str(self.kalAngleY)) time.sleep(0.005) except KeyboardInterrupt: print("test") return self.X_angle, self.Y_angle
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()