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