Revision 14:365c1c1bf6ee, committed 2019-07-04

Comitter:

sepham

Date:

Thu Jul 04 18:25:35 2019 +0000

Parent:

13:d66766523196

Commit message:

IMU tof more gyro and testing

Changed in this revision

--- a/IMU6050/IMUWheelchair.cpp	Wed Jul 03 22:09:33 2019 +0000
+++ b/IMU6050/IMUWheelchair.cpp	Thu Jul 04 18:25:35 2019 +0000
@@ -1,5 +1,5 @@
 #include "IMUWheelchair.h"
-static float total_yaw;
+float total_yaw;
 
 
 IMUWheelchair::IMUWheelchair(Serial* out, Timer* time)
@@ -20,6 +20,8 @@
 }
 
 void IMUWheelchair::setup() {
+    imu->setGyroRange(MPU6050_GYRO_RANGE_500);
+
     if(imu->testConnection()== 0)
         printf("not connected\r\n");
     else
@@ -27,6 +29,7 @@
     
     //timer
     t->start();
+    
 }
 
 //Get the x component of the angular acceleration from IMU. Stores the component
@@ -34,7 +37,7 @@
 //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];
+    return (double)accelD[0];
 }
 
 //Get the y component of the angular acceleration from IMU. Stores the component
@@ -42,7 +45,7 @@
 //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];   
+    return (double)accelD[1];   
 }
 
 //Get the z component of the angular acceleration from IMU. Stores the component
@@ -50,15 +53,16 @@
 //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];
+    return (double)accelD[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];
+
+    imu->getGyro(gyroD); //Change the values in gyroArray
+    return (double)gyroD[0];
     
 }
 
@@ -67,7 +71,7 @@
 //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];
+    return (double)gyroD[1];
     
 }
 
@@ -76,15 +80,20 @@
 //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];
+    return (double)gyroD[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);
+     printf("t->read(): %lf", t->read());
+     total_yaw = total_yaw - t->read()*gyroZ;
+     
+     printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
     }
     t->reset();
     if(total_yaw > 360)
@@ -98,15 +107,15 @@
 double IMUWheelchair::pitch()
  {
     imu->getAccelero(accelD);
-    float pitch = atan2 (-accelData[1] ,( sqrt (accelData[0] * accelData[0]) +(accelData[2]  *accelData[2])));
+    float pitch = atan2 (-accelD[1] ,( sqrt (accelD[0] * accelD[0]) +(accelD[2]  *accelD[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]))));
+    float roll = atan2(-accelD[0]  ,( sqrt((accelD[1] *accelD[1] ) + 
+ (accelD[2] * accelD[2]))));
     roll = roll*57.3;
     t->reset();
     return (double)roll;

--- a/main.cpp	Wed Jul 03 22:09:33 2019 +0000
+++ b/main.cpp	Thu Jul 04 18:25:35 2019 +0000
@@ -1,24 +1,30 @@
 #include <MPU6050.h>
 #include "IMUWheelchair.h"
-MPU6050 IMU(D14, D15);
+//MPU6050 IMU(D14, D15);
 Serial pc(USBTX, USBRX, 57600);                                     //Serial Monitor
 float dataGyro[3];
 float *dataG = dataGyro;
-
+Timer t;
 float dataYaw[3];
 float *dataY = dataYaw;
+IMUWheelchair IMU(&pc,&t);
 
 int main()
 {
-    IMU.setGyroRange(MPU6050_GYRO_RANGE_500);
-    pc.printf("Imu test connections %d\r\n", IMU.testConnection());
+    t.start();
+    //pc.printf("Imu test connections %d\r\n", IMU.testConnection());
     while(1){
         
-        IMU.getGyro(dataG);
+        //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]);
+         //IMU.gyro_x();
+       // IMU.getAcceleroAngle(dataY);
+       // pc.printf("x: %f, y: %f, z: %f\r\n", dataG[0]*180/3.1415926 + .79, dataG[1], dataG[2]);
+       // printf("%f, %f, %f\n", IMU.gyro_x(), IMU.gyro_y(), IMU.gyro_z()); 
+       // printf("%f\r\n ", IMU.accel_x());
+      
+       IMU.yaw();
+       // printf("%f\r\n ", IMU.yaw());
     }
 }