Library receiving values from MPU6050
- include "mbed.h"
- include "TI_MPU6050.h"
Example
include the mbed library with this snippet
TI_MPU6050 mpu6050; DigitalOut led1(LED1); int main() { mpu6050.setSleepEnabled(false); while(1) { if (mpu6050.isHorizontal()) { led1 = 1; } else { led1 = 0; } int verticalAngle = mpu6050.getVerticalAngle(); printf("verticalAngle is %d\n\r", verticalAngle); } }
IS_MPU6050.cpp@0:81e5563b1ea1, 2018-06-04 (annotated)
- Committer:
- tichise
- Date:
- Mon Jun 04 17:22:09 2018 +0000
- Revision:
- 0:81e5563b1ea1
new
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tichise | 0:81e5563b1ea1 | 1 | #include "IS_MPU6050.h" |
tichise | 0:81e5563b1ea1 | 2 | #include "mbed.h" |
tichise | 0:81e5563b1ea1 | 3 | |
tichise | 0:81e5563b1ea1 | 4 | IS_MPU6050::IS_MPU6050() |
tichise | 0:81e5563b1ea1 | 5 | { |
tichise | 0:81e5563b1ea1 | 6 | _mpu.initialize(); |
tichise | 0:81e5563b1ea1 | 7 | |
tichise | 0:81e5563b1ea1 | 8 | bool result = _mpu.testConnection(); |
tichise | 0:81e5563b1ea1 | 9 | |
tichise | 0:81e5563b1ea1 | 10 | if (result) { |
tichise | 0:81e5563b1ea1 | 11 | // printf("MPU6050 Test Connection passed\n\r"); |
tichise | 0:81e5563b1ea1 | 12 | } else { |
tichise | 0:81e5563b1ea1 | 13 | // printf("MPU6050 Test Connection failed\n\r"); |
tichise | 0:81e5563b1ea1 | 14 | } |
tichise | 0:81e5563b1ea1 | 15 | } |
tichise | 0:81e5563b1ea1 | 16 | |
tichise | 0:81e5563b1ea1 | 17 | int IS_MPU6050::getVerticalAngle() |
tichise | 0:81e5563b1ea1 | 18 | { |
tichise | 0:81e5563b1ea1 | 19 | int16_t ax, ay, az; |
tichise | 0:81e5563b1ea1 | 20 | int16_t gx, gy, gz; |
tichise | 0:81e5563b1ea1 | 21 | |
tichise | 0:81e5563b1ea1 | 22 | int i = 0; |
tichise | 0:81e5563b1ea1 | 23 | long x = 0; |
tichise | 0:81e5563b1ea1 | 24 | long y = 0; |
tichise | 0:81e5563b1ea1 | 25 | long z = 0; |
tichise | 0:81e5563b1ea1 | 26 | |
tichise | 0:81e5563b1ea1 | 27 | int repeatCount = 100; |
tichise | 0:81e5563b1ea1 | 28 | |
tichise | 0:81e5563b1ea1 | 29 | // 各データを100回読み込んで平均化 |
tichise | 0:81e5563b1ea1 | 30 | for (i = 0; i < repeatCount; ++i) { |
tichise | 0:81e5563b1ea1 | 31 | _mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
tichise | 0:81e5563b1ea1 | 32 | |
tichise | 0:81e5563b1ea1 | 33 | // printf("x:%d; y:%d; z:%d;\n\r",ax,ay,az); |
tichise | 0:81e5563b1ea1 | 34 | |
tichise | 0:81e5563b1ea1 | 35 | x = x + ax; // X軸を読み込む |
tichise | 0:81e5563b1ea1 | 36 | y = y + ay; // Y軸を読み込む |
tichise | 0:81e5563b1ea1 | 37 | z = z + az; // Y軸を読み込む |
tichise | 0:81e5563b1ea1 | 38 | } |
tichise | 0:81e5563b1ea1 | 39 | |
tichise | 0:81e5563b1ea1 | 40 | double avX = x / repeatCount; |
tichise | 0:81e5563b1ea1 | 41 | double avY = y / repeatCount; |
tichise | 0:81e5563b1ea1 | 42 | |
tichise | 0:81e5563b1ea1 | 43 | int verticalAngle = atan2(avX - 507, avY - 520) / 3.14159 * 180.0; |
tichise | 0:81e5563b1ea1 | 44 | |
tichise | 0:81e5563b1ea1 | 45 | return verticalAngle; |
tichise | 0:81e5563b1ea1 | 46 | } |
tichise | 0:81e5563b1ea1 | 47 | |
tichise | 0:81e5563b1ea1 | 48 | bool IS_MPU6050::isHorizontal() |
tichise | 0:81e5563b1ea1 | 49 | { |
tichise | 0:81e5563b1ea1 | 50 | int verticalAngle = getVerticalAngle(); |
tichise | 0:81e5563b1ea1 | 51 | |
tichise | 0:81e5563b1ea1 | 52 | int baseAngle = 90; |
tichise | 0:81e5563b1ea1 | 53 | bool isHorizontal; |
tichise | 0:81e5563b1ea1 | 54 | |
tichise | 0:81e5563b1ea1 | 55 | if (verticalAngle < (baseAngle - 20)) { |
tichise | 0:81e5563b1ea1 | 56 | // printf("vertical Angle : %d;\n\r", verticalAngle); |
tichise | 0:81e5563b1ea1 | 57 | |
tichise | 0:81e5563b1ea1 | 58 | isHorizontal = false; |
tichise | 0:81e5563b1ea1 | 59 | } else if ((baseAngle + 20) < verticalAngle) { |
tichise | 0:81e5563b1ea1 | 60 | // printf("vertical Angle : %d;\n\r", verticalAngle); |
tichise | 0:81e5563b1ea1 | 61 | |
tichise | 0:81e5563b1ea1 | 62 | isHorizontal = false; |
tichise | 0:81e5563b1ea1 | 63 | } else { |
tichise | 0:81e5563b1ea1 | 64 | isHorizontal = true; |
tichise | 0:81e5563b1ea1 | 65 | } |
tichise | 0:81e5563b1ea1 | 66 | |
tichise | 0:81e5563b1ea1 | 67 | return isHorizontal; |
tichise | 0:81e5563b1ea1 | 68 | } |
tichise | 0:81e5563b1ea1 | 69 | |
tichise | 0:81e5563b1ea1 | 70 | void IS_MPU6050::setSleepEnabled(bool enabled) |
tichise | 0:81e5563b1ea1 | 71 | { |
tichise | 0:81e5563b1ea1 | 72 | _mpu.setSleepEnabled(enabled); |
tichise | 0:81e5563b1ea1 | 73 | } |