![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Added for gyro and testing
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
IMU6050/IMUWheelchair.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
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;
diff -r d66766523196 -r 365c1c1bf6ee main.cpp --- 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()); } }