Header file and functions

Files at this revision

API Documentation at this revision

Comitter:
jahutchi
Date:
Wed Jul 03 22:09:33 2019 +0000
Parent:
12:6efce6d008f8
Commit message:
Functions are working but now figuring out the yaw

Changed in this revision

IMU6050/IMU6050.cpp Show diff for this revision Revisions of this file
IMU6050/IMU6050.h Show diff for this revision Revisions of this file
IMU6050/IMUWheelchair.cpp Show annotated file Show diff for this revision Revisions of this file
IMU6050/IMUWheelchair.h Show annotated file Show diff for this revision Revisions of this file
MPU6050.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 6efce6d008f8 -r d66766523196 IMU6050/IMU6050.cpp
--- a/IMU6050/IMU6050.cpp	Wed Jul 03 17:56:22 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,79 +0,0 @@
-#include "IMU6050.h"
-IMU6050::IMU6050(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){
-    accelD = accelData;
-    gyroD = gyroData;
-}
-
-void IMU6050::setup() {
-    
-}
-
-//Get the x component of the angular acceleration from IMU. Stores the component
-//in a float array
-//Returns a double, the value of the x-acceleration (m/s^2)
-double IMU6050::accel_x() {  
-    imu -> getAccelero(accelD); //Change the values in accelerationArray
-    return (double)accelData[0];
-}
-
-//Get the y component of the angular acceleration from IMU. Stores the component
-//in a float array
-//Returns a double, the value of the y-acceleration (m/s^2)
-double IMU6050::accel_y() {
-    imu -> getAccelero(accelD); //Change the values in accelerationArray
-    return (double)accelData[1];   
-}
-
-//Get the z component of the angular acceleration from IMU. Stores the component
-//in a float array
-//Returns a double, the value of the z-acceleration (m/s^2)
-double IMU6050::accel_z() {
-    imu -> getAccelero(accelD); //Change the values in accelerationArray
-    return (double)accelData[2];
-}
-
-//Get the x component of the angular velocity from IMU's gyroscope. Stores the 
-//component in a float array
-//Returns a double, the value of the x-angular velocity (rad/s)
-double IMU6050::gyro_x() {
-    imu -> getGyro(gyroD); //Change the values in gyroArray
-    return (double)gyroData[0];
-    
-}
-
-//Get the y component of the angular velocity from IMU's gyroscope. Stores the 
-//component in a float array
-//Returns a double, the value of the y-angular velocity (rad/s)
-double IMU6050::gyro_y() {
-    imu -> getGyro(gyroD); //Change the values in gyroArray
-    return (double)gyroData[1];
-    
-}
-
-//Get the z component of the angular velocity from IMU's gyroscope. Stores the 
-//component in a float array
-//Returns a double, the value of the z-angular velocity (rad/s)
-double IMU6050::gyro_z() {
-    imu -> getGyro(gyroD); //Change the values in gyroArray
-    return (double)gyroData[2];
-}
-
-//Get the yaw, or the angle turned at a certain time interval
-//Return double, the angle or yaw, (degree)
-
-/*
-double IMU6050::yaw() {
-    
-    imu -> computeAngle();
-    return
-}
-
-double IMU6050::pitch() {
-    
-}
-
-double IMU6050::roll() {
-    
-}
-
-*/
\ No newline at end of file
diff -r 6efce6d008f8 -r d66766523196 IMU6050/IMU6050.h
--- a/IMU6050/IMU6050.h	Wed Jul 03 17:56:22 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,74 +0,0 @@
-#ifndef IMU6050_H
-#define IMU6050_H
-
-#include "filter.h"
-//#include  "mbed.h"
-#include  "math.h"
-#include  <MPU6050.h>
-
-#define PI 3.141593
-
-/*#define SDA D14
-#define SCL D15*/
-#define SDA PB_9
-#define SCL PB_8
-#define SAMPLEFREQ 50
-#define CAL_TIME 3
-
-class IMU6050 {
-    public:
-        //The constructor for this class
-        IMU6050(Serial* out, Timer* time);
-        IMU6050(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time);
-        
-        //Set up the IMU, check if it connects
-        void setup();
-        
-        //Get the x-component of the angular acceleration
-        double accel_x();
-        
-        //Get the y-component of the angular acceleration
-        double accel_y();
-        
-        //Get the z-component of the angular acceleration
-        double accel_z();
-        
-        //Get the x-component of gyro, angular velocity
-        double gyro_x();
-        
-        //Get the y-component of gyro, angular velocity
-        double gyro_y();
-        
-        //Get the z-component of gyro, angular velocity
-        double gyro_z();
-        
-        //Magnometer to find angle relative to North to compare to gyroscope
-        //double angle_north();
-        
-        //Get the YAW, or angle (theta), direction facing
-        double yaw();
-        
-        //Get the pitch, (Up and down component)
-        double pitch();
-        
-        //Get the roll, the tilt
-        double roll();
-  
-        MPU6050* imu; //The IMU we're testing from, MPU6050  
-        
-    private:
-        Serial* usb; //the connection port
-        Timer* t;//to calculate the time
-        float accelData[3];   // stores the angular acceleration component
-        float gyroData[3];    //stores the gyro data x,y,z
-        float* accelD;        //Pointer that points to either accelData 
-        float* gyroD;         //Ptr to the gyroData array
-        
-        float angleData[3]; //Contains the pitch, roll, yaw angle
-        float* angleD;//Ptr to angleData array
-        
-        void calibrate_yaw();
-    
-};
-
-#endif
\ No newline at end of file
diff -r 6efce6d008f8 -r d66766523196 IMU6050/IMUWheelchair.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU6050/IMUWheelchair.cpp	Wed Jul 03 22:09:33 2019 +0000
@@ -0,0 +1,113 @@
+#include "IMUWheelchair.h"
+static float total_yaw;
+
+
+IMUWheelchair::IMUWheelchair(Serial* out, Timer* time)
+{
+    imu = new MPU6050(SDA, SCL);
+    usb = out;
+    t = time;
+    start = false;
+}
+
+IMUWheelchair::IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){
+    usb = out;
+    t = time;
+    imu = new MPU6050(sda_pin, scl_pin);
+    IMUWheelchair::setup();
+    accelD = accelData;
+    gyroD = gyroData;
+}
+
+void IMUWheelchair::setup() {
+    if(imu->testConnection()== 0)
+        printf("not connected\r\n");
+    else
+        printf("connected\r\n");
+    
+    //timer
+    t->start();
+}
+
+//Get the x component of the angular acceleration from IMU. Stores the component
+//in a float array
+//Returns a double, the value of the x-acceleration (m/s^2)
+double IMUWheelchair::accel_x() {  
+    imu -> getAccelero(accelD); //Change the values in accelerationArray
+    return (double)accelData[0];
+}
+
+//Get the y component of the angular acceleration from IMU. Stores the component
+//in a float array
+//Returns a double, the value of the y-acceleration (m/s^2)
+double IMUWheelchair::accel_y() {
+    imu -> getAccelero(accelD); //Change the values in accelerationArray
+    return (double)accelData[1];   
+}
+
+//Get the z component of the angular acceleration from IMU. Stores the component
+//in a float array
+//Returns a double, the value of the z-acceleration (m/s^2)
+double IMUWheelchair::accel_z() {
+    imu -> getAccelero(accelD); //Change the values in accelerationArray
+    return (double)accelData[2];
+}
+
+//Get the x component of the angular velocity from IMU's gyroscope. Stores the 
+//component in a float array
+//Returns a double, the value of the x-angular velocity (rad/s)
+double IMUWheelchair::gyro_x() {
+    imu -> getGyro(gyroD); //Change the values in gyroArray
+    return (double)gyroData[0];
+    
+}
+
+//Get the y component of the angular velocity from IMU's gyroscope. Stores the 
+//component in a float array
+//Returns a double, the value of the y-angular velocity (rad/s)
+double IMUWheelchair::gyro_y() {
+    imu -> getGyro(gyroD); //Change the values in gyroArray
+    return (double)gyroData[1];
+    
+}
+
+//Get the z component of the angular velocity from IMU's gyroscope. Stores the 
+//component in a float array
+//Returns a double, the value of the z-angular velocity (rad/s)
+double IMUWheelchair::gyro_z() {
+    imu -> getGyro(gyroD); //Change the values in gyroArray
+    return (double)gyroData[2];
+}
+
+//Get the yaw, or the angle turned at a certain time interval
+//Return double, the angle or yaw, (degree)
+double IMUWheelchair::yaw() {
+    float gyroZ = IMUWheelchair::gyro_z();
+    if(abs(gyroZ) >= .3) {
+     total_yaw = (total_yaw - t->read()*gyroZ);
+    }
+    t->reset();
+    if(total_yaw > 360)
+        total_yaw -= 360;
+    if(total_yaw < 0)
+        total_yaw += 360;
+    return (double)total_yaw;
+}
+
+
+double IMUWheelchair::pitch()
+ {
+    imu->getAccelero(accelD);
+    float pitch = atan2 (-accelData[1] ,( sqrt (accelData[0] * accelData[0]) +(accelData[2]  *accelData[2])));
+    pitch = pitch*57.3;
+    return (double)pitch;
+}
+
+double IMUWheelchair::roll() {
+    imu->getAccelero(accelD);
+    float roll = atan2(-accelData[0]  ,( sqrt((accelData[1] *accelData[1] ) + 
+ (accelData[2] * accelData[2]))));
+    roll = roll*57.3;
+    t->reset();
+    return (double)roll;
+}
diff -r 6efce6d008f8 -r d66766523196 IMU6050/IMUWheelchair.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU6050/IMUWheelchair.h	Wed Jul 03 22:09:33 2019 +0000
@@ -0,0 +1,76 @@
+#ifndef IMUWheelchair_H
+#define IMUWheelchair_H
+
+#include "filter.h"
+//#include  "mbed.h"
+#include  "math.h"
+#include  <MPU6050.h>
+
+#define PI 3.141593
+
+/*#define SDA D14
+#define SCL D15*/
+#define SDA PB_9
+#define SCL PB_8
+#define SAMPLEFREQ 50
+#define CAL_TIME 3
+
+class IMUWheelchair {
+    public:
+        //The constructor for this class
+        IMUWheelchair(Serial* out, Timer* time);
+        IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time);
+        
+        //Set up the IMU, check if it connects
+        void setup();
+        
+        //Get the x-component of the angular acceleration
+        double accel_x();
+        
+        //Get the y-component of the angular acceleration
+        double accel_y();
+        
+        //Get the z-component of the angular acceleration
+        double accel_z();
+        
+        //Get the x-component of gyro, angular velocity
+        double gyro_x();
+        
+        //Get the y-component of gyro, angular velocity
+        double gyro_y();
+        
+        //Get the z-component of gyro, angular velocity
+        double gyro_z();
+        
+        //Magnometer to find angle relative to North to compare to gyroscope
+        //double angle_north();
+        
+        //Get the YAW, or angle (theta), direction facing
+        double yaw();
+        
+        //Get the pitch, (Up and down component)
+        double pitch();
+        
+        //Get the roll, the tilt
+        double roll();
+  
+        MPU6050* imu; //The IMU we're testing from, MPU6050  
+        
+    private:
+        Serial* usb; //the connection port
+        Timer* t;//to calculate the time
+        float accelData[3];   // stores the angular acceleration component
+        float gyroData[3];    //stores the gyro data x,y,z
+        float* accelD;        //Pointer that points to either accelData 
+        float* gyroD;         //Ptr to the gyroData array
+        
+        float angleData[3]; //Contains the pitch, roll, yaw angle
+        float* angleD;//Ptr to angleData array
+        
+        void calibrate_yaw();
+        
+        bool start;
+    
+};
+
+#endif
\ No newline at end of file
diff -r 6efce6d008f8 -r d66766523196 MPU6050.cpp
--- a/MPU6050.cpp	Wed Jul 03 17:56:22 2019 +0000
+++ b/MPU6050.cpp	Wed Jul 03 22:09:33 2019 +0000
@@ -207,7 +207,7 @@
         data[2]=(float)temp[2] / 3752.9;
         }
     if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
-        data[0]=(float)temp[0] / 1879.3;;
+        data[0]=(float)temp[0] / 1879.3;
         data[1]=(float)temp[1] / 1879.3;
         data[2]=(float)temp[2] / 1879.3;
         }
@@ -306,7 +306,7 @@
     this->write(MPU6050_RA_INT_ENABLE, temp);
 }
 
-///function for disbling interrupts on MPU6050 INT pin, when the data is ready to take
+///function for disabling interrupts on MPU6050 INT pin, when the data is ready to take
 void MPU6050::disableInt ( void ){
     char temp;
     temp = this->read(MPU6050_RA_INT_ENABLE);
diff -r 6efce6d008f8 -r d66766523196 main.cpp
--- a/main.cpp	Wed Jul 03 17:56:22 2019 +0000
+++ b/main.cpp	Wed Jul 03 22:09:33 2019 +0000
@@ -1,17 +1,24 @@
 #include <MPU6050.h>
-#include "IMU6050.h"
+#include "IMUWheelchair.h"
 MPU6050 IMU(D14, D15);
 Serial pc(USBTX, USBRX, 57600);                                     //Serial Monitor
-float dataA[3];
-float* data = dataA;
+float dataGyro[3];
+float *dataG = dataGyro;
+
+float dataYaw[3];
+float *dataY = dataYaw;
+
 int main()
 {
     IMU.setGyroRange(MPU6050_GYRO_RANGE_500);
     pc.printf("Imu test connections %d\r\n", IMU.testConnection());
     while(1){
         
-        IMU.getGyro(data);
-        pc.printf("x: %f, y: %f, z: %f\r\n", dataA[0]*180/3.14, dataA[1]*180/3.14, dataA[2]*180/3.14);
+        IMU.getGyro(dataG);
+       // pc.printf("x: %f, y: %f, z: %f\r\n", dataGyro[0]*180/3.14, dataGyro[1]*180/3.14, dataGyro[2]*180/3.14);
         wait(.2);
+        
+        IMU.getAcceleroAngle(dataY);
+        pc.printf("x: %f, y: %f, z: %f\r\n", dataG[0], dataG[1], dataG[2]);
     }
 }