a
IMU6050/IMUWheelchair.cpp@14:365c1c1bf6ee, 2019-07-04 (annotated)
- Committer:
- sepham
- Date:
- Thu Jul 04 18:25:35 2019 +0000
- Revision:
- 14:365c1c1bf6ee
- Parent:
- 13:d66766523196
- Child:
- 15:b7cac36c4cce
IMU tof more gyro and testing
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jahutchi | 13:d66766523196 | 1 | #include "IMUWheelchair.h" |
sepham | 14:365c1c1bf6ee | 2 | float total_yaw; |
jahutchi | 13:d66766523196 | 3 | |
jahutchi | 13:d66766523196 | 4 | |
jahutchi | 13:d66766523196 | 5 | IMUWheelchair::IMUWheelchair(Serial* out, Timer* time) |
jahutchi | 13:d66766523196 | 6 | { |
jahutchi | 13:d66766523196 | 7 | imu = new MPU6050(SDA, SCL); |
jahutchi | 13:d66766523196 | 8 | usb = out; |
jahutchi | 13:d66766523196 | 9 | t = time; |
jahutchi | 13:d66766523196 | 10 | start = false; |
jahutchi | 13:d66766523196 | 11 | } |
jahutchi | 13:d66766523196 | 12 | |
jahutchi | 13:d66766523196 | 13 | IMUWheelchair::IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){ |
jahutchi | 13:d66766523196 | 14 | usb = out; |
jahutchi | 13:d66766523196 | 15 | t = time; |
jahutchi | 13:d66766523196 | 16 | imu = new MPU6050(sda_pin, scl_pin); |
jahutchi | 13:d66766523196 | 17 | IMUWheelchair::setup(); |
jahutchi | 13:d66766523196 | 18 | accelD = accelData; |
jahutchi | 13:d66766523196 | 19 | gyroD = gyroData; |
jahutchi | 13:d66766523196 | 20 | } |
jahutchi | 13:d66766523196 | 21 | |
jahutchi | 13:d66766523196 | 22 | void IMUWheelchair::setup() { |
sepham | 14:365c1c1bf6ee | 23 | imu->setGyroRange(MPU6050_GYRO_RANGE_500); |
sepham | 14:365c1c1bf6ee | 24 | |
jahutchi | 13:d66766523196 | 25 | if(imu->testConnection()== 0) |
jahutchi | 13:d66766523196 | 26 | printf("not connected\r\n"); |
jahutchi | 13:d66766523196 | 27 | else |
jahutchi | 13:d66766523196 | 28 | printf("connected\r\n"); |
jahutchi | 13:d66766523196 | 29 | |
jahutchi | 13:d66766523196 | 30 | //timer |
jahutchi | 13:d66766523196 | 31 | t->start(); |
sepham | 14:365c1c1bf6ee | 32 | |
jahutchi | 13:d66766523196 | 33 | } |
jahutchi | 13:d66766523196 | 34 | |
jahutchi | 13:d66766523196 | 35 | //Get the x component of the angular acceleration from IMU. Stores the component |
jahutchi | 13:d66766523196 | 36 | //in a float array |
jahutchi | 13:d66766523196 | 37 | //Returns a double, the value of the x-acceleration (m/s^2) |
jahutchi | 13:d66766523196 | 38 | double IMUWheelchair::accel_x() { |
jahutchi | 13:d66766523196 | 39 | imu -> getAccelero(accelD); //Change the values in accelerationArray |
sepham | 14:365c1c1bf6ee | 40 | return (double)accelD[0]; |
jahutchi | 13:d66766523196 | 41 | } |
jahutchi | 13:d66766523196 | 42 | |
jahutchi | 13:d66766523196 | 43 | //Get the y component of the angular acceleration from IMU. Stores the component |
jahutchi | 13:d66766523196 | 44 | //in a float array |
jahutchi | 13:d66766523196 | 45 | //Returns a double, the value of the y-acceleration (m/s^2) |
jahutchi | 13:d66766523196 | 46 | double IMUWheelchair::accel_y() { |
jahutchi | 13:d66766523196 | 47 | imu -> getAccelero(accelD); //Change the values in accelerationArray |
sepham | 14:365c1c1bf6ee | 48 | return (double)accelD[1]; |
jahutchi | 13:d66766523196 | 49 | } |
jahutchi | 13:d66766523196 | 50 | |
jahutchi | 13:d66766523196 | 51 | //Get the z component of the angular acceleration from IMU. Stores the component |
jahutchi | 13:d66766523196 | 52 | //in a float array |
jahutchi | 13:d66766523196 | 53 | //Returns a double, the value of the z-acceleration (m/s^2) |
jahutchi | 13:d66766523196 | 54 | double IMUWheelchair::accel_z() { |
jahutchi | 13:d66766523196 | 55 | imu -> getAccelero(accelD); //Change the values in accelerationArray |
sepham | 14:365c1c1bf6ee | 56 | return (double)accelD[2]; |
jahutchi | 13:d66766523196 | 57 | } |
jahutchi | 13:d66766523196 | 58 | |
jahutchi | 13:d66766523196 | 59 | //Get the x component of the angular velocity from IMU's gyroscope. Stores the |
jahutchi | 13:d66766523196 | 60 | //component in a float array |
jahutchi | 13:d66766523196 | 61 | //Returns a double, the value of the x-angular velocity (rad/s) |
jahutchi | 13:d66766523196 | 62 | double IMUWheelchair::gyro_x() { |
sepham | 14:365c1c1bf6ee | 63 | |
sepham | 14:365c1c1bf6ee | 64 | imu->getGyro(gyroD); //Change the values in gyroArray |
sepham | 14:365c1c1bf6ee | 65 | return (double)gyroD[0]; |
jahutchi | 13:d66766523196 | 66 | |
jahutchi | 13:d66766523196 | 67 | } |
jahutchi | 13:d66766523196 | 68 | |
jahutchi | 13:d66766523196 | 69 | //Get the y component of the angular velocity from IMU's gyroscope. Stores the |
jahutchi | 13:d66766523196 | 70 | //component in a float array |
jahutchi | 13:d66766523196 | 71 | //Returns a double, the value of the y-angular velocity (rad/s) |
jahutchi | 13:d66766523196 | 72 | double IMUWheelchair::gyro_y() { |
jahutchi | 13:d66766523196 | 73 | imu -> getGyro(gyroD); //Change the values in gyroArray |
sepham | 14:365c1c1bf6ee | 74 | return (double)gyroD[1]; |
jahutchi | 13:d66766523196 | 75 | |
jahutchi | 13:d66766523196 | 76 | } |
jahutchi | 13:d66766523196 | 77 | |
jahutchi | 13:d66766523196 | 78 | //Get the z component of the angular velocity from IMU's gyroscope. Stores the |
jahutchi | 13:d66766523196 | 79 | //component in a float array |
jahutchi | 13:d66766523196 | 80 | //Returns a double, the value of the z-angular velocity (rad/s) |
jahutchi | 13:d66766523196 | 81 | double IMUWheelchair::gyro_z() { |
jahutchi | 13:d66766523196 | 82 | imu -> getGyro(gyroD); //Change the values in gyroArray |
sepham | 14:365c1c1bf6ee | 83 | return (double)gyroD[2]; |
jahutchi | 13:d66766523196 | 84 | } |
jahutchi | 13:d66766523196 | 85 | |
sepham | 14:365c1c1bf6ee | 86 | |
sepham | 14:365c1c1bf6ee | 87 | |
jahutchi | 13:d66766523196 | 88 | //Get the yaw, or the angle turned at a certain time interval |
jahutchi | 13:d66766523196 | 89 | //Return double, the angle or yaw, (degree) |
jahutchi | 13:d66766523196 | 90 | double IMUWheelchair::yaw() { |
jahutchi | 13:d66766523196 | 91 | float gyroZ = IMUWheelchair::gyro_z(); |
jahutchi | 13:d66766523196 | 92 | if(abs(gyroZ) >= .3) { |
sepham | 14:365c1c1bf6ee | 93 | printf("t->read(): %lf", t->read()); |
sepham | 14:365c1c1bf6ee | 94 | total_yaw = total_yaw - t->read()*gyroZ; |
sepham | 14:365c1c1bf6ee | 95 | |
sepham | 14:365c1c1bf6ee | 96 | printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ); |
jahutchi | 13:d66766523196 | 97 | } |
jahutchi | 13:d66766523196 | 98 | t->reset(); |
jahutchi | 13:d66766523196 | 99 | if(total_yaw > 360) |
jahutchi | 13:d66766523196 | 100 | total_yaw -= 360; |
jahutchi | 13:d66766523196 | 101 | if(total_yaw < 0) |
jahutchi | 13:d66766523196 | 102 | total_yaw += 360; |
jahutchi | 13:d66766523196 | 103 | return (double)total_yaw; |
jahutchi | 13:d66766523196 | 104 | } |
jahutchi | 13:d66766523196 | 105 | |
jahutchi | 13:d66766523196 | 106 | |
jahutchi | 13:d66766523196 | 107 | double IMUWheelchair::pitch() |
jahutchi | 13:d66766523196 | 108 | { |
jahutchi | 13:d66766523196 | 109 | imu->getAccelero(accelD); |
sepham | 14:365c1c1bf6ee | 110 | float pitch = atan2 (-accelD[1] ,( sqrt (accelD[0] * accelD[0]) +(accelD[2] *accelD[2]))); |
jahutchi | 13:d66766523196 | 111 | pitch = pitch*57.3; |
jahutchi | 13:d66766523196 | 112 | return (double)pitch; |
jahutchi | 13:d66766523196 | 113 | } |
jahutchi | 13:d66766523196 | 114 | |
jahutchi | 13:d66766523196 | 115 | double IMUWheelchair::roll() { |
jahutchi | 13:d66766523196 | 116 | imu->getAccelero(accelD); |
sepham | 14:365c1c1bf6ee | 117 | float roll = atan2(-accelD[0] ,( sqrt((accelD[1] *accelD[1] ) + |
sepham | 14:365c1c1bf6ee | 118 | (accelD[2] * accelD[2])))); |
jahutchi | 13:d66766523196 | 119 | roll = roll*57.3; |
jahutchi | 13:d66766523196 | 120 | t->reset(); |
jahutchi | 13:d66766523196 | 121 | return (double)roll; |
jahutchi | 13:d66766523196 | 122 | } |