Diff: chair_imu.cpp

Revision:

5:e0ccaab3959a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chair_imu.cpp	Tue Jul 17 07:19:04 2018 +0000
@@ -0,0 +1,121 @@
+#include "chair_imu.h"
+
+Serial pc(USBTX, USBRX);
+Timer t;
+
+chair_imu::chair_imu(){
+  imu = new BNO055(SDA, SCL);
+}
+
+chair_imu::chair_imu(PinName sda_pin, PinName scl_pin){
+  imu = new BNO055(sda_pin, scl_pin);
+}
+
+void chair_imu::setup(){
+  imu->reset();
+  pc.printf("Bosch Sensortec BNO055 test program on \r\n");
+  while (imu->check() == 0) {
+    pc.printf("Bosch BNO055 is NOT available!!\r\n");
+    wait(.5);
+  }
+  pc.printf("Bosch Sensortec BNO055 available \r\n");
+  imu->set_accel_units(MPERSPERS);
+  imu->setmode(OPERATION_MODE_AMG);
+  imu->get_calib();
+  imu->write_calibration_data();
+  imu->set_angle_units(DEGREES);
+  imu->setmode(OPERATION_MODE_AMG); //put into while loop 
+  t.start();
+}
+
+double chair_imu::accel_x(){
+  imu->get_accel();
+  return (double)imu->accel.x;
+}
+
+double chair_imu::accel_y(){
+  imu->get_accel();
+  return (double)imu->accel.y;
+}
+
+double chair_imu::accel_z(){
+  imu->get_accel();
+  return (double)imu->accel.z;
+}
+
+double chair_imu::gyro_x(){
+  imu->get_gyro();
+  return (double)imu->gyro.x;
+}
+
+double chair_imu::gyro_y(){
+  imu->get_gyro();
+  return (double)imu->gyro.y;
+}
+
+double chair_imu::gyro_z(){
+  imu->get_gyro();
+  return (double)imu->gyro.z;
+}
+
+double chair_imu::angle_north(){
+  imu->get_mag();
+  float x = imu->mag.x;
+  float y = imu->mag.y;
+  
+  float result = x/y;
+  
+  float angleToNorth;
+  if(imu->mag.y>0)
+    angleToNorth = 90.0 - atan(result)*180/PI;
+  else if(imu->mag.y<0)
+    angleToNorth = 270.0 - atan(result)*180/PI;
+  else if(y == 0 && x <= 0)
+    angleToNorth = 180;
+  else if(y == 0 && x > 0)
+    angleToNorth = 0;
+  
+  return (double)angleToNorth;
+}
+
+double chair_imu::roll(){
+  imu->get_accel();
+  imu->get_gyro();
+  
+  float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) + 
+  (imu->accel.z * imu->accel.z))));
+  roll = roll*57.3;
+  
+  t.reset();
+
+  return (double)roll;
+}
+
+double chair_imu::pitch(){
+  imu->get_accel();
+  imu->get_gyro();
+  
+  float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) + 
+  (imu->accel.z * imu->accel.z))));
+  pitch = pitch*57.3;
+  
+  t.reset();
+
+  return (double)pitch;
+}
+
+double chair_imu::yaw(){
+  imu->get_gyro();
+  float yaw = (yaw - t.read()*imu->gyro.z);
+  
+  if(yaw > 360)
+    yaw -= 360;
+  if(yaw < 0)
+    yaw += 360;
+    
+  t.reset();
+
+  return (double)yaw;
+}
+
+