a
IMU6050/IMUWheelchair.cpp@13:d66766523196, 2019-07-03 (annotated)
- Committer:
- jahutchi
- Date:
- Wed Jul 03 22:09:33 2019 +0000
- Revision:
- 13:d66766523196
- Child:
- 14:365c1c1bf6ee
Functions are working but now figuring out the yaw
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jahutchi | 13:d66766523196 | 1 | #include "IMUWheelchair.h" |
jahutchi | 13:d66766523196 | 2 | static 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() { |
jahutchi | 13:d66766523196 | 23 | if(imu->testConnection()== 0) |
jahutchi | 13:d66766523196 | 24 | printf("not connected\r\n"); |
jahutchi | 13:d66766523196 | 25 | else |
jahutchi | 13:d66766523196 | 26 | printf("connected\r\n"); |
jahutchi | 13:d66766523196 | 27 | |
jahutchi | 13:d66766523196 | 28 | //timer |
jahutchi | 13:d66766523196 | 29 | t->start(); |
jahutchi | 13:d66766523196 | 30 | } |
jahutchi | 13:d66766523196 | 31 | |
jahutchi | 13:d66766523196 | 32 | //Get the x component of the angular acceleration from IMU. Stores the component |
jahutchi | 13:d66766523196 | 33 | //in a float array |
jahutchi | 13:d66766523196 | 34 | //Returns a double, the value of the x-acceleration (m/s^2) |
jahutchi | 13:d66766523196 | 35 | double IMUWheelchair::accel_x() { |
jahutchi | 13:d66766523196 | 36 | imu -> getAccelero(accelD); //Change the values in accelerationArray |
jahutchi | 13:d66766523196 | 37 | return (double)accelData[0]; |
jahutchi | 13:d66766523196 | 38 | } |
jahutchi | 13:d66766523196 | 39 | |
jahutchi | 13:d66766523196 | 40 | //Get the y component of the angular acceleration from IMU. Stores the component |
jahutchi | 13:d66766523196 | 41 | //in a float array |
jahutchi | 13:d66766523196 | 42 | //Returns a double, the value of the y-acceleration (m/s^2) |
jahutchi | 13:d66766523196 | 43 | double IMUWheelchair::accel_y() { |
jahutchi | 13:d66766523196 | 44 | imu -> getAccelero(accelD); //Change the values in accelerationArray |
jahutchi | 13:d66766523196 | 45 | return (double)accelData[1]; |
jahutchi | 13:d66766523196 | 46 | } |
jahutchi | 13:d66766523196 | 47 | |
jahutchi | 13:d66766523196 | 48 | //Get the z component of the angular acceleration from IMU. Stores the component |
jahutchi | 13:d66766523196 | 49 | //in a float array |
jahutchi | 13:d66766523196 | 50 | //Returns a double, the value of the z-acceleration (m/s^2) |
jahutchi | 13:d66766523196 | 51 | double IMUWheelchair::accel_z() { |
jahutchi | 13:d66766523196 | 52 | imu -> getAccelero(accelD); //Change the values in accelerationArray |
jahutchi | 13:d66766523196 | 53 | return (double)accelData[2]; |
jahutchi | 13:d66766523196 | 54 | } |
jahutchi | 13:d66766523196 | 55 | |
jahutchi | 13:d66766523196 | 56 | //Get the x component of the angular velocity from IMU's gyroscope. Stores the |
jahutchi | 13:d66766523196 | 57 | //component in a float array |
jahutchi | 13:d66766523196 | 58 | //Returns a double, the value of the x-angular velocity (rad/s) |
jahutchi | 13:d66766523196 | 59 | double IMUWheelchair::gyro_x() { |
jahutchi | 13:d66766523196 | 60 | imu -> getGyro(gyroD); //Change the values in gyroArray |
jahutchi | 13:d66766523196 | 61 | return (double)gyroData[0]; |
jahutchi | 13:d66766523196 | 62 | |
jahutchi | 13:d66766523196 | 63 | } |
jahutchi | 13:d66766523196 | 64 | |
jahutchi | 13:d66766523196 | 65 | //Get the y component of the angular velocity from IMU's gyroscope. Stores the |
jahutchi | 13:d66766523196 | 66 | //component in a float array |
jahutchi | 13:d66766523196 | 67 | //Returns a double, the value of the y-angular velocity (rad/s) |
jahutchi | 13:d66766523196 | 68 | double IMUWheelchair::gyro_y() { |
jahutchi | 13:d66766523196 | 69 | imu -> getGyro(gyroD); //Change the values in gyroArray |
jahutchi | 13:d66766523196 | 70 | return (double)gyroData[1]; |
jahutchi | 13:d66766523196 | 71 | |
jahutchi | 13:d66766523196 | 72 | } |
jahutchi | 13:d66766523196 | 73 | |
jahutchi | 13:d66766523196 | 74 | //Get the z component of the angular velocity from IMU's gyroscope. Stores the |
jahutchi | 13:d66766523196 | 75 | //component in a float array |
jahutchi | 13:d66766523196 | 76 | //Returns a double, the value of the z-angular velocity (rad/s) |
jahutchi | 13:d66766523196 | 77 | double IMUWheelchair::gyro_z() { |
jahutchi | 13:d66766523196 | 78 | imu -> getGyro(gyroD); //Change the values in gyroArray |
jahutchi | 13:d66766523196 | 79 | return (double)gyroData[2]; |
jahutchi | 13:d66766523196 | 80 | } |
jahutchi | 13:d66766523196 | 81 | |
jahutchi | 13:d66766523196 | 82 | //Get the yaw, or the angle turned at a certain time interval |
jahutchi | 13:d66766523196 | 83 | //Return double, the angle or yaw, (degree) |
jahutchi | 13:d66766523196 | 84 | double IMUWheelchair::yaw() { |
jahutchi | 13:d66766523196 | 85 | float gyroZ = IMUWheelchair::gyro_z(); |
jahutchi | 13:d66766523196 | 86 | if(abs(gyroZ) >= .3) { |
jahutchi | 13:d66766523196 | 87 | total_yaw = (total_yaw - t->read()*gyroZ); |
jahutchi | 13:d66766523196 | 88 | } |
jahutchi | 13:d66766523196 | 89 | t->reset(); |
jahutchi | 13:d66766523196 | 90 | if(total_yaw > 360) |
jahutchi | 13:d66766523196 | 91 | total_yaw -= 360; |
jahutchi | 13:d66766523196 | 92 | if(total_yaw < 0) |
jahutchi | 13:d66766523196 | 93 | total_yaw += 360; |
jahutchi | 13:d66766523196 | 94 | return (double)total_yaw; |
jahutchi | 13:d66766523196 | 95 | } |
jahutchi | 13:d66766523196 | 96 | |
jahutchi | 13:d66766523196 | 97 | |
jahutchi | 13:d66766523196 | 98 | double IMUWheelchair::pitch() |
jahutchi | 13:d66766523196 | 99 | { |
jahutchi | 13:d66766523196 | 100 | imu->getAccelero(accelD); |
jahutchi | 13:d66766523196 | 101 | float pitch = atan2 (-accelData[1] ,( sqrt (accelData[0] * accelData[0]) +(accelData[2] *accelData[2]))); |
jahutchi | 13:d66766523196 | 102 | pitch = pitch*57.3; |
jahutchi | 13:d66766523196 | 103 | return (double)pitch; |
jahutchi | 13:d66766523196 | 104 | } |
jahutchi | 13:d66766523196 | 105 | |
jahutchi | 13:d66766523196 | 106 | double IMUWheelchair::roll() { |
jahutchi | 13:d66766523196 | 107 | imu->getAccelero(accelD); |
jahutchi | 13:d66766523196 | 108 | float roll = atan2(-accelData[0] ,( sqrt((accelData[1] *accelData[1] ) + |
jahutchi | 13:d66766523196 | 109 | (accelData[2] * accelData[2])))); |
jahutchi | 13:d66766523196 | 110 | roll = roll*57.3; |
jahutchi | 13:d66766523196 | 111 | t->reset(); |
jahutchi | 13:d66766523196 | 112 | return (double)roll; |
jahutchi | 13:d66766523196 | 113 | } |