Diff: BNO080Wheelchair.cpp

Revision:

10:9275e6f7bf1b

Parent:

8:217f510db255

Child:

11:dbe6d8d0ceb1

--- a/BNO080Wheelchair.cpp	Mon Jul 29 18:24:40 2019 +0000
+++ b/BNO080Wheelchair.cpp	Tue Jul 30 18:33:45 2019 +0000
@@ -1,14 +1,104 @@
-#include "BNO80Wheelchair.h"
+#include "BNO080Wheelchair.h"
+float total_yaw;
 
-//The constructor for the BNO80 imu. Needs 7 parameters
-BNO80Wheelchair::BNO80Wheelchair(Serial *debugPort, PinName sdaPin, 
+//The constructor for the BNO080 imu. Needs 7 parameters
+BNO080Wheelchair::BNO080Wheelchair(Serial *debugPort, PinName sdaPin, 
                                  PinName sclPin, PinName intPin, PinName rstPin,
                                  uint8_t i2cAddress, int i2cPortpeed) {
-    imu = new BNO80();
+    imu = new BNO080(usb, 
+            SDA, 
+            SCL, 
+            INT_PIN,         
+            RST_PIN,
+            0x4a, 
+            100000);
     //setUp
     
 }
 //Check if all the
-void BNO80Wheelchair::setup() {
+void BNO080Wheelchair::setup() {
     imu -> begin();
-}
\ No newline at end of file
+}
+
+//Returns a double, the value of the x-acceleration (m/s^2)
+double BNO080Wheelchair::gyro_x() {  
+    imu -> updateData();            // hasNewData()?
+    return (double)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];
+}
+
+//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];
+}
+
+//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)
+double BNO080Wheelchair::accel_x() {
+    imu -> updateData();            // hasNewData()?
+    return (double)linearAcceleration[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)
+double BNO080Wheelchair::accel_y() {
+    imu -> updateData();            // hasNewData()?
+    return (double)linearAcceleration[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];
+}
+
+double BNO080Wheelchair::yaw() {
+
+    float gyroZ = .4+(BNO080Wheelchair::gyro_x())*180/3.141593;
+    if(abs(gyroZ) >= .5) {
+     //printf("t->read(): %lf, gyroscope %lf, change %lf\r\n", t->read(), gyroZ, t->read()*gyroZ*2.25);
+        total_yaw = total_yaw - t->read()*gyroZ;
+     //printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
+    }
+    t->reset();
+    if(total_yaw > 360)
+        total_yaw -= 360;
+    if(total_yaw < 0)
+        total_yaw += 360;
+    return (double)total_yaw;
+}
+
+double BNO080Wheelchair::compass() {
+    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;
+}
+
+
+
+