Diff: chair_imu.cpp

Revision:

6:8cd00c26bb47

Parent:

5:e0ccaab3959a

--- a/chair_imu.cpp	Tue Jul 17 07:19:04 2018 +0000
+++ b/chair_imu.cpp	Tue Jul 17 19:19:26 2018 +0000
@@ -1,121 +1,133 @@
 #include "chair_imu.h"
-
+ 
 Serial pc(USBTX, USBRX);
 Timer t;
 
+// Default Initialization
 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;
+    imu = new BNO055(SDA, SCL);
 }
 
-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;
+// Initialize Pins 
+chair_imu::chair_imu(PinName SDA_Pin, PinName SCL_Pin){
+    imu = new BNO055(SDA_Pin, SCL_Pin);
 }
+ 
+// Check to make sure BNO055 is properly working
+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");
 
-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;
+    //Set the SI units & start the timer
+    imu->set_accel_units(MPERSPERS);
+    imu->setmode(OPERATION_MODE_AMG);
+    imu->set_angle_units(DEGREES);
+    imu->get_calib();
+    imu->write_calibration_data();
+    imu->setmode(OPERATION_MODE_AMG); //put into while loop 
+    t.start();
+}
+ 
+// Receive acceleration data (in meters per second)
+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::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::accel_z(){
+    imu->get_accel();
+    return (double)imu->accel.z;
 }
-
-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;
+ 
+ 
+// Receive Gyroscope data (in degrees)
+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;
 }
 
 
+// Receive angle relative to North
+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;
+}
+ 
+// Rotation about the X-axis
+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;         // Scaling factor   
+
+    t.reset();
+ 
+    return (double)roll;
+}
+
+// Rotation about the Y-axis
+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;       // Scaling Factor
+
+    t.reset();
+
+    return (double)pitch;
+}
+ 
+// Rotation about the Z-axis
+double chair_imu::yaw(){
+    imu->get_gyro();
+    float yaw = (yaw - t.read() * imu->gyro.z);
+
+    // Return a yaw value between 0 and 360 degrees
+    if(yaw > 360)
+    yaw -= 360;
+    if(yaw < 0)
+    yaw += 360;
+
+    t.reset();
+
+    return (double)yaw;
+}
\ No newline at end of file