Diff: IMU6050/IMUWheelchair.cpp

Revision:

14:365c1c1bf6ee

Parent:

13:d66766523196

Child:

15:b7cac36c4cce

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