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