<s> from smbus import SMBus <EOL> from bitstring import BitArray <EOL> import math <EOL> import threading <EOL> import time <EOL> from KalmanFilter import * <EOL> class IMU ( object ) : <EOL> DELAY_TIME = <NUM_LIT> <EOL> GYRO_NOISE = <NUM_LIT> <EOL> BIAS_NOISE = <NUM_LIT> <EOL> ACCEL_NOISE = <NUM_LIT> <EOL> def __init__ ( self , i2cBus = <NUM_LIT:1> ) : <EOL> self . gyro = Gyroscope ( i2cBus , Gyroscope . DPS2000 ) <EOL> self . accel = Accelerometer ( i2cBus , Accelerometer . SCALE_A_8G ) <EOL> initOrientation = self . accel . getOrientation ( ) <EOL> self . roll = initOrientation . roll <EOL> self . pitch = initOrientation . pitch <EOL> self . yaw = initOrientation . yaw <EOL> self . kalmanFilter = KalmanFilter ( IMU . GYRO_NOISE , IMU . BIAS_NOISE , IMU . ACCEL_NOISE ) <EOL> self . thread = threading . Thread ( target = self . updateOrientation ) <EOL> self . thread . daemon = True <EOL> self . thread . start ( ) <EOL> def zero ( self ) : <EOL> self . gyro . zero ( ) <EOL> self . accel . zero ( ) <EOL> def updateOrientation ( self ) : <EOL> self . elapsedTime = IMU . DELAY_TIME <EOL> while True : <EOL> startTime = time . time ( ) <EOL> gyroX , gyroY , gyroZ = self . gyro . getRawData ( ) <EOL> accelOrientation = self . accel . getOrientation ( ) <EOL> self . roll = self . kalmanFilter . filterX ( accelOrientation . roll , gyroX , self . elapsedTime ) <EOL> self . pitch = self . kalmanFilter . filterY ( accelOrientation . pitch , gyroY , self . elapsedTime ) <EOL> time . sleep ( max ( <NUM_LIT:0> , IMU . DELAY_TIME - ( time . time ( ) - startTime ) ) ) <EOL> self . elapsedTime = time . time ( ) - startTime <EOL> def getOrientation ( self ) : <EOL> return Orientation ( self . roll , self . pitch , self . yaw ) <EOL> class Orientation ( object ) : <EOL> def __init__ ( self , roll , pitch , yaw ) : <EOL> self . roll = roll <EOL> self . pitch = pitch <EOL> self . yaw = yaw <EOL> class Accelerometer ( object ) : <EOL> LSM_ACC_ADDR = <NUM_LIT> <EOL> CTRL_REG1_A = <NUM_LIT> <EOL> CTRL_REG4_A = <NUM_LIT> <EOL> OUT_X_L_A = <NUM_LIT> <EOL> OUT_X_H_A = <NUM_LIT> <EOL> OUT_Y_L_A = <NUM_LIT> <EOL> OUT_Y_H_A = <NUM_LIT> <EOL> OUT_Z_L_A = <NUM_LIT> <EOL> OUT_Z_H_A = <NUM_LIT> <EOL> POWER_ON = <NUM_LIT> <EOL> SCALE_A_2G = <NUM_LIT> <EOL> SCALE_A_4G = <NUM_LIT> <EOL> SCALE_A_8G = <NUM_LIT> <EOL> SCALE_A_16G = <NUM_LIT> <EOL> LSM_MAG_ADDR = <NUM_LIT> <EOL> CRA_REG_M = <NUM_LIT> <EOL> CRB_REG_M = <NUM_LIT> <EOL> MR_REG_M = <NUM_LIT> <EOL> OUT_X_L_M = <NUM_LIT> <EOL> OUT_X_H_M = <NUM_LIT> <EOL> OUT_Y_L_M = <NUM_LIT> <EOL> OUT_Y_H_M = <NUM_LIT> <EOL> OUT_Z_L_M = <NUM_LIT> <EOL> OUT_Z_H_M = <NUM_LIT> <EOL> DATA_RATE = <NUM_LIT> <EOL> CONV_MODE = <NUM_LIT> <EOL> SCALE_M_13G = <NUM_LIT> <EOL> SCALE_M_19G = <NUM_LIT> <EOL> SCALE_M_25G = <NUM_LIT> <EOL> SCALE_M_40G = <NUM_LIT> <EOL> SCALE_M_47G = <NUM_LIT> <EOL> SCALE_M_56G = <NUM_LIT> <EOL> SCALE_M_81G = <NUM_LIT> <EOL> ZERO_X = - <NUM_LIT> <EOL> ZERO_Y = <NUM_LIT> <EOL> ZERO_Z = - <NUM_LIT> <EOL> def __init__ ( self , i2cBus , gaussScale ) : <EOL> self . bus = SMBus ( i2cBus ) <EOL> self . bus . write_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . CTRL_REG1_A , Accelerometer . POWER_ON ) <EOL> self . bus . write_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . CTRL_REG4_A , gaussScale ) <EOL> if gaussScale == Accelerometer . SCALE_A_2G : <EOL> self . scale = ( - <NUM_LIT> / <NUM_LIT> ) <EOL> if gaussScale == Accelerometer . SCALE_A_4G : <EOL> self . scale = ( - <NUM_LIT> / <NUM_LIT> ) <EOL> if gaussScale == Accelerometer . SCALE_A_8G : <EOL> self . scale = ( - <NUM_LIT> / <NUM_LIT> ) <EOL> if gaussScale == Accelerometer . SCALE_A_16G : <EOL> self . scale = ( - <NUM_LIT> / <NUM_LIT> ) <EOL> def zero ( self ) : <EOL> count = <NUM_LIT:0> <EOL> xtotal = <NUM_LIT:0> <EOL> ytotal = <NUM_LIT:0> <EOL> ztotal = <NUM_LIT:0> <EOL> print '<STR_LIT>' , self . ZERO_X , self . ZERO_Y , self . ZERO_Z <EOL> while count < <NUM_LIT:100> : <EOL> x , y , z = self . getRawData ( ) <EOL> orientation = self . getOrientation ( ) <EOL> count = <NUM_LIT:1> + count <EOL> print ( '<STR_LIT>' ) . format ( x , y , z ) <EOL> print ( '<STR_LIT>' ) . format ( orientation . pitch , orientation . roll ) <EOL> xtotal = x / self . scale - self . ZERO_X + xtotal <EOL> ytotal = y / self . scale - self . ZERO_Y + ytotal <EOL> ztotal = z / self . scale - self . ZERO_Z + ztotal <EOL> time . sleep ( <NUM_LIT> ) <EOL> self . ZERO_X = - <NUM_LIT:1> * xtotal / <NUM_LIT> <EOL> self . ZERO_Y = - <NUM_LIT:1> * ytotal / <NUM_LIT> <EOL> print '<STR_LIT>' , self . ZERO_X , self . ZERO_Y , self . ZERO_Z <EOL> def getRawData ( self ) : <EOL> x = <NUM_LIT> * self . bus . read_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . OUT_X_H_A ) + self . bus . read_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . OUT_X_L_A ) <EOL> if x >= <NUM_LIT> : <EOL> x = BitArray ( bin ( x ) ) . int <EOL> y = <NUM_LIT> * self . bus . read_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . OUT_Y_H_A ) + self . bus . read_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . OUT_Y_L_A ) <EOL> if y >= <NUM_LIT> : <EOL> y = BitArray ( bin ( y ) ) . int <EOL> z = <NUM_LIT> * self . bus . read_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . OUT_Z_H_A ) + self . bus . read_byte_data ( Accelerometer . LSM_ACC_ADDR , Accelerometer . OUT_Z_L_A ) <EOL> if z >= <NUM_LIT> : <EOL> z = BitArray ( bin ( z ) ) . int <EOL> return self . scale * ( x + self . ZERO_X ) , self . scale * ( y + self . ZERO_Y ) , self . scale * ( z - self . ZERO_Z ) <EOL> def getOrientation ( self ) : <EOL> x , y , z = self . getRawData ( ) <EOL> roll = math . atan2 ( y , z ) + math . pi <EOL> pitch = math . atan2 ( x , z ) + math . pi <EOL> if roll > math . pi : <EOL> roll = roll - <NUM_LIT:2> * math . pi <EOL> if pitch > math . pi : <EOL> pitch = pitch - <NUM_LIT:2> * math . pi <EOL> return Orientation ( math . degrees ( roll ) , math . degrees ( pitch ) , <NUM_LIT:0> ) <EOL> class Gyroscope ( object ) : <EOL> DPS250 = <NUM_LIT> <EOL> DPS500 = <NUM_LIT> <EOL> DPS2000 = <NUM_LIT> <EOL> L3GADDR = <NUM_LIT> <EOL> CTREG1 = <NUM_LIT> <EOL> CTREG4 = <NUM_LIT> <EOL> ON = <NUM_LIT> <EOL> XOUTLOW = <NUM_LIT> <EOL> XOUTHIGH = <NUM_LIT> <EOL> YOUTLOW = <NUM_LIT> <EOL> YOUTHIGH = <NUM_LIT> <EOL> ZOUTLOW = <NUM_LIT> <EOL> ZOUTHIGH = <NUM_LIT> <EOL> ZERO_X = - <NUM_LIT> <EOL> ZERO_Y = <NUM_LIT> <EOL> ZERO_Z = - <NUM_LIT> <EOL> def __init__ ( self , i2cBus , dps ) : <EOL> self . bus = SMBus ( i2cBus ) <EOL> self . bus . write_byte_data ( Gyroscope . L3GADDR , Gyroscope . CTREG1 , Gyroscope . ON ) <EOL> self . bus . write_byte_data ( Gyroscope . L3GADDR , Gyroscope . CTREG4 , dps ) <EOL> if dps == Gyroscope . DPS250 : <EOL> self . scale = <NUM_LIT> / <NUM_LIT> <EOL> if dps == Gyroscope . DPS500 : <EOL> self . scale = <NUM_LIT> / <NUM_LIT> <EOL> if dps == Gyroscope . DPS2000 : <EOL> self . scale = <NUM_LIT> / <NUM_LIT> <EOL> def zero ( self ) : <EOL> count = <NUM_LIT:0> <EOL> xtotal = <NUM_LIT:0> <EOL> ytotal = <NUM_LIT:0> <EOL> ztotal = <NUM_LIT:0> <EOL> print '<STR_LIT>' , self . ZERO_X , self . ZERO_Y , self . ZERO_Z <EOL> while count < <NUM_LIT:100> : <EOL> x , y , z = self . getRawData ( ) <EOL> count = <NUM_LIT:1> + count <EOL> print ( '<STR_LIT>' ) . format ( x / self . scale , y / self . scale , z / self . scale ) <EOL> xtotal = x / self . scale - self . ZERO_X + xtotal <EOL> ytotal = y / self . scale - self . ZERO_Y + ytotal <EOL> ztotal = z / self . scale - self . ZERO_Z + ztotal <EOL> time . sleep ( <NUM_LIT> ) <EOL> print xtotal / <NUM_LIT:100> , ytotal / <NUM_LIT:100> , ztotal / <NUM_LIT:100> <EOL> self . ZERO_X = - <NUM_LIT:1> * xtotal / <NUM_LIT:100> <EOL> self . ZERO_Y = - <NUM_LIT:1> * ytotal / <NUM_LIT:100> <EOL> self . ZERO_Z = - <NUM_LIT:1> * ztotal / <NUM_LIT:100> <EOL> print '<STR_LIT>' , self . ZERO_X , self . ZERO_Y , self . ZERO_Z <EOL> def getRawData ( self ) : <EOL> x = <NUM_LIT> * self . bus . read_byte_data ( Gyroscope . L3GADDR , Gyroscope . XOUTHIGH ) + self . bus . read_byte_data ( Gyroscope . L3GADDR , Gyroscope . XOUTLOW ) <EOL> if x >= <NUM_LIT> : <EOL> x = BitArray ( bin ( x ) ) . int <EOL> y = <NUM_LIT> * self . bus . read_byte_data ( Gyroscope . L3GADDR , Gyroscope . YOUTHIGH ) + self . bus . read_byte_data ( Gyroscope . L3GADDR , Gyroscope . YOUTLOW ) <EOL> if y >= <NUM_LIT> : <EOL> y = BitArray ( bin ( y ) ) . int <EOL> z = <NUM_LIT> * self . bus . read_byte_data ( Gyroscope . L3GADDR , Gyroscope . ZOUTHIGH ) + self . bus . read_byte_data ( Gyroscope . L3GADDR , Gyroscope . ZOUTLOW ) <EOL> if z >= <NUM_LIT> : <EOL> z = BitArray ( bin ( z ) ) . int <EOL> return self . scale * ( x + Gyroscope . ZERO_X ) , self . scale * ( y + Gyroscope . ZERO_Y ) , self . scale * ( z + Gyroscope . ZERO_Z ) </s>
