Revision 13:d66766523196, committed 2019-07-03

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

--- 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

--- 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

--- /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;
+}

--- /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

--- 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);

--- 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]);
     }
 }