Diff: BNO080Wheelchair.cpp

Revision:

11:dbe6d8d0ceb1

Parent:

10:9275e6f7bf1b

Child:

12:f013530d8358

--- a/BNO080Wheelchair.cpp	Tue Jul 30 18:33:45 2019 +0000
+++ b/BNO080Wheelchair.cpp	Fri Aug 02 23:34:30 2019 +0000
@@ -5,67 +5,87 @@
 BNO080Wheelchair::BNO080Wheelchair(Serial *debugPort, PinName sdaPin, 
                                  PinName sclPin, PinName intPin, PinName rstPin,
                                  uint8_t i2cAddress, int i2cPortpeed) {
-    imu = new BNO080(usb, 
-            SDA, 
-            SCL, 
-            INT_PIN,         
-            RST_PIN,
-            0x4a, 
-            100000);
+    imu = new BNO080(debugPort, sdaPin, sclPin, intPin,rstPin,i2cAddress, i2cPortpeed);
     //setUp
     
 }
 //Check if all the
 void BNO080Wheelchair::setup() {
     imu -> begin();
+    //Tell the IMU to report every 100ms
+    imu -> enableReport(BNO080::TOTAL_ACCELERATION, 200);
+    imu -> enableReport(BNO080::LINEAR_ACCELERATION, 200);
+    imu -> enableReport(BNO080::GRAVITY_ACCELERATION, 200);
+    imu -> enableReport(BNO080::GYROSCOPE, 200);
+    imu -> enableReport(BNO080::MAG_FIELD, 200);    
+//    imu -> enableReport(BNO080::MAG_FIELD_UNCALIBRATED, 100);    
+//    imu -> enableReport(BNO080::ROTATION, 100);
+//    imu -> enableReport(BNO080::GEOMAGNETIC_ROTATION, 100);    
+//    imu -> enableReport(BNO080::GAME_ROTATION, 100);    
+//    imu -> enableReport(BNO080::TAP_DETECTOR, 100);   
+//    imu -> enableReport(BNO080::STABILITY_CLASSIFIER, 100);    
+//    imu -> enableReport(BNO080::STEP_DETECTOR, 100);    
+//    imu -> enableReport(BNO080::STEP_COUNTER, 100);
+//    imu -> enableReport(BNO080::SIGNIFICANT_MOTION, 100);    
+//    imu -> enableReport(BNO080::SHAKE_DETECTOR, 100);    
+
 }
 
+//Get the x component of the angular velocity from IMU. Stores the component
+//in a float array
 //Returns a double, the value of the x-acceleration (m/s^2)
-double BNO080Wheelchair::gyro_x() {  
-    imu -> updateData();            // hasNewData()?
-    return (double)gyroRotation[0];
+double BNO080Wheelchair::gyro_x() {
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> gyroRotation[0];
 }
 
 //Get the y component of the angular velocity from IMU. Stores the component
 //in a float array
 //Returns a double, the value of the y-acceleration (m/s^2)
 double BNO080Wheelchair::gyro_y() {
-    imu -> updateData();            // hasNewData()?
-    return (double)gyroRotation[1];
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> gyroRotation[1];
 }
 
 //Get the z component of the angular velocity from IMU. Stores the component
 //in a float array
 //Returns a double, the value of the z-acceleration (m/s^2)
 double BNO080Wheelchair::gyro_z() {
-    imu -> updateData();            // hasNewData()?
-    return (double)gyroRotation[2];
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> gyroRotation[2];
 }
 
 //Get the x component of the linear acceleration from IMU. Stores the component
 //in a float array
-//Returns a double, the value of the z-acceleration (m/s^2)
+//Returns a double, the value of the x-acceleration (m/s^2)
 double BNO080Wheelchair::accel_x() {
-    imu -> updateData();            // hasNewData()?
-    return (double)linearAcceleration[0];
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> totalAcceleration[0];
 }
 
 //Get the y component of the linear acceleration from IMU. Stores the component
 //in a float array
-//Returns a double, the value of the z-acceleration (m/s^2)
+//Returns a double, the value of the y-acceleration (m/s^2)
 double BNO080Wheelchair::accel_y() {
-    imu -> updateData();            // hasNewData()?
-    return (double)linearAcceleration[1];
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> totalAcceleration[1];
 }
 
 //Get the z component of the linear acceleration from IMU. Stores the component
 //in a float array
 //Returns a double, the value of the z-acceleration (m/s^2)
 double BNO080Wheelchair::accel_z() {
-    imu -> updateData();            // hasNewData()?
-    return (double)linearAcceleration[2];
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> totalAcceleration[2];
 }
 
+//Get yaw
 double BNO080Wheelchair::yaw() {
 
     float gyroZ = .4+(BNO080Wheelchair::gyro_x())*180/3.141593;
@@ -82,23 +102,93 @@
     return (double)total_yaw;
 }
 
-double BNO080Wheelchair::compass() {
+//Get x component of magnetic field vector
+double BNO080Wheelchair::mag_x() {
+    wait(1);
     imu -> updateData();
-    
-    float Pi = 3.14159;
-  
-    // Calculate the angle of the vector y,x
-    double heading = (atan2(magField[1],magField[0]) * 180) / Pi;
-  
-    // Normalize to 0-360
-    if (heading < 0)
-    {
-        heading = 360 + heading;
-    } 
-       
-    return heading;
+    return (double)imu -> magField[0];
+}
+
+//Get y component of magnetic field vector
+double BNO080Wheelchair::mag_y() {
+    wait(1);
+    imu -> updateData();
+    return (double)imu -> magField[1];
+}
+
+//Get z component of magnetic field vector
+double BNO080Wheelchair::mag_z() {
+    wait(1);
+    imu -> updateData();
+    return (double)imu -> magField[2];
 }
 
+//Check if IMU is pointing in one of the 4 cardinal directions (NSWE)
+char BNO080Wheelchair::compass() {
+    imu -> updateData();
+    double x = imu -> magField[0];
+    double y = imu -> magField[1];
+    
+    if ( (x>10) && (y<-5) && (y>-10) ) {
+        return 'N'; //Facing NORTH
+    }
+    
+    else if ( (x<-8) && (y>-5) ) {
+        return 'S'; //Facing SOUTH
+    }
+    
+    else if ( (x>-3) && (x<4) && (y<-15) ) {
+        return 'W'; //Facing WEST
+    }
 
+    else if ( (x>4) && (x<8) && (y>1) ) {
+        return 'E'; //Facing EAST
+    }
+    
+    else if ( (x==0) && (y==0) ) {
+        return 'O'; //IMU reports not enabled correctly, zeros received
+    }   
+
+    else {
+        return 'I'; //Direction indeterminate
+    }
+}
 
+//Get the rotation of the IMU (from magnetic north) in radians
+Quaternion BNO080Wheelchair::rotation() {
+    wait(0.05);
+    //printf("Update Data GYRO X: %d \n", imu -> updateData());            // hasNewData()?
+    imu -> updateData();
+    //wait(0.05);
+    return imu -> rotationVector;
+}
 
+/*
+//Returns Qw component of rotation vector
+double BNO080Wheelchair::rot_w() {
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> rotationVector[0];
+}
+
+//Returns Qx component of rotation vector
+double BNO080Wheelchair::rot_x() {
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> rotationVector[1];
+}
+
+//Returns Qy component of rotation vector
+double BNO080Wheelchair::rot_y() {
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> rotationVector[2];
+}
+
+//Returns Qz component of rotation vector
+double BNO080Wheelchair::rot_z() {
+    wait(0.05);
+    imu -> updateData();
+    return (double)imu -> rotationVector[2];
+}
+*/
\ No newline at end of file