a
Diff: IMU6050/IMUWheelchair.cpp
- Revision:
- 14:365c1c1bf6ee
- Parent:
- 13:d66766523196
- Child:
- 15:b7cac36c4cce
diff -r d66766523196 -r 365c1c1bf6ee IMU6050/IMUWheelchair.cpp --- 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;