Diff: IMU6050/IMUWheelchair.cpp

Revision:

15:b7cac36c4cce

Parent:

14:365c1c1bf6ee

--- a/IMU6050/IMUWheelchair.cpp	Thu Jul 04 18:25:35 2019 +0000
+++ b/IMU6050/IMUWheelchair.cpp	Fri Jul 05 16:44:37 2019 +0000
@@ -8,6 +8,8 @@
     usb = out;
     t = time;
     start = false;
+    IMUWheelchair::setup();
+
 }
 
 IMUWheelchair::IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){
@@ -15,13 +17,13 @@
     t = time;
     imu = new MPU6050(sda_pin, scl_pin);
     IMUWheelchair::setup();
-    accelD = accelData;
-    gyroD = gyroData;
+
 }
 
 void IMUWheelchair::setup() {
-    imu->setGyroRange(MPU6050_GYRO_RANGE_500);
-
+    imu->setGyroRange(MPU6050_GYRO_RANGE_1000);
+    accelD = accelData;
+    gyroD = gyroData;
     if(imu->testConnection()== 0)
         printf("not connected\r\n");
     else
@@ -60,7 +62,6 @@
 //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)gyroD[0];
     
@@ -88,12 +89,12 @@
 //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) {
-     printf("t->read(): %lf", t->read());
-     total_yaw = total_yaw - t->read()*gyroZ;
-     
-     printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
+
+    float gyroZ = .4+(IMUWheelchair::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)