Advertisement
Guest User

Untitled

a guest
Oct 31st, 2018
217
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import sys
  2. sys.path.append('.')
  3. import RTIMU
  4. import time
  5.  
  6. SETTINGS_FILE = "RTIMULib"
  7. s = RTIMU.Settings(SETTINGS_FILE)
  8. imu = RTIMU.RTIMU(s)
  9.  
  10. if (not imu.IMUInit()):
  11.     print('IMU init failed.')
  12.     sys.exit(1)
  13.  
  14. imu.setSlerpPower(0.02)
  15. imu.setGyroEnable(True)
  16. imu.setAccelEnable(True)
  17. imu.setCompassEnable(True)
  18.  
  19. poll_interval = imu.IMUGetPollInterval()
  20. print('poll interval: ', poll_interval)
  21.  
  22. gyro_meas = []
  23. fusion_meas = []
  24.  
  25. try:
  26.     while True:
  27.  
  28.         if imu.IMURead():
  29.             data = imu.getIMUData()
  30.             fusion_pose = data["fusionPose"]
  31.             Gyro = data["gyro"]
  32.             print('\ngyro: {:.4f}, {:.4f}, {:.4f}'.format(Gyro[0], Gyro[1], Gyro[2]))
  33.             print('fusion: {:.4f}, {:.4f}, {:.4f}'.format(fusion_pose[0], fusion_pose[1], fusion_pose[2]))
  34.  
  35.             time.sleep(poll_interval*1.0/1000.0)
  36.  
  37. except:
  38.     pass
Advertisement
Advertisement
Advertisement
RAW Paste Data Copied
Advertisement