Christian Benitez
/
IMU
Cleaned up
Fork of LMU by
main.cpp@1:4fb66612cb9c, 2018-08-03 (annotated)
- Committer:
- cpbenite
- Date:
- Fri Aug 03 22:00:54 2018 +0000
- Revision:
- 1:4fb66612cb9c
- Parent:
- 0:c9ec02922858
Cleaned up code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jvfausto | 0:c9ec02922858 | 1 | #include "mbed.h" |
jvfausto | 0:c9ec02922858 | 2 | #include "math.h" |
jvfausto | 0:c9ec02922858 | 3 | #include "BNO055.h" |
jvfausto | 0:c9ec02922858 | 4 | |
jvfausto | 0:c9ec02922858 | 5 | #define PI 3.141593 |
jvfausto | 0:c9ec02922858 | 6 | |
jvfausto | 0:c9ec02922858 | 7 | Serial pc(USBTX,USBRX); |
cpbenite | 1:4fb66612cb9c | 8 | BNO055 imu(D14, D15); // SDA, SCL |
jvfausto | 0:c9ec02922858 | 9 | |
jvfausto | 0:c9ec02922858 | 10 | double y; |
jvfausto | 0:c9ec02922858 | 11 | double x; |
jvfausto | 0:c9ec02922858 | 12 | double z; |
jvfausto | 0:c9ec02922858 | 13 | double angleToNorth; |
jvfausto | 0:c9ec02922858 | 14 | double result; |
jvfausto | 0:c9ec02922858 | 15 | // BNO055_ID_INF_TypeDef bno055_id_inf; |
jvfausto | 0:c9ec02922858 | 16 | // BNO055_EULER_TypeDef euler_angles; |
cpbenite | 1:4fb66612cb9c | 17 | |
cpbenite | 1:4fb66612cb9c | 18 | double getAngleToNorth (double x, double y, double result) { |
cpbenite | 1:4fb66612cb9c | 19 | if (y > 0) { |
cpbenite | 1:4fb66612cb9c | 20 | return 90.0 - atan(result) * 180/PI; |
cpbenite | 1:4fb66612cb9c | 21 | } |
cpbenite | 1:4fb66612cb9c | 22 | else if (y < 0) { |
cpbenite | 1:4fb66612cb9c | 23 | return 270.0 - atan(result) * 180/PI; |
cpbenite | 1:4fb66612cb9c | 24 | } |
cpbenite | 1:4fb66612cb9c | 25 | else if (y == 0 && x <= 0) { |
cpbenite | 1:4fb66612cb9c | 26 | return 180; |
cpbenite | 1:4fb66612cb9c | 27 | } |
cpbenite | 1:4fb66612cb9c | 28 | else if (y == 0 && x > 0) { |
cpbenite | 1:4fb66612cb9c | 29 | return 0; |
cpbenite | 1:4fb66612cb9c | 30 | } |
cpbenite | 1:4fb66612cb9c | 31 | } |
jvfausto | 0:c9ec02922858 | 32 | |
cpbenite | 1:4fb66612cb9c | 33 | void getValues() { |
cpbenite | 1:4fb66612cb9c | 34 | imu.get_accel(); |
cpbenite | 1:4fb66612cb9c | 35 | imu.get_gyro(); |
cpbenite | 1:4fb66612cb9c | 36 | imu.get_mag(); |
cpbenite | 1:4fb66612cb9c | 37 | } |
cpbenite | 1:4fb66612cb9c | 38 | |
cpbenite | 1:4fb66612cb9c | 39 | void printInfo() { |
cpbenite | 1:4fb66612cb9c | 40 | //pc.printf("BNO055 found\r\n\r\n"); |
jvfausto | 0:c9ec02922858 | 41 | pc.printf("Chip ID: %0z\r\n",imu.ID.id); |
jvfausto | 0:c9ec02922858 | 42 | pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel); |
jvfausto | 0:c9ec02922858 | 43 | pc.printf("Gyroscope ID: %0z\r\n",imu.ID.gyro); |
jvfausto | 0:c9ec02922858 | 44 | pc.printf("Magnetometer ID: %0z\r\n\r\n",imu.ID.mag); |
jvfausto | 0:c9ec02922858 | 45 | pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]); |
jvfausto | 0:c9ec02922858 | 46 | pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload); |
cpbenite | 1:4fb66612cb9c | 47 | } |
cpbenite | 1:4fb66612cb9c | 48 | |
cpbenite | 1:4fb66612cb9c | 49 | void setup() { |
cpbenite | 1:4fb66612cb9c | 50 | imu.reset(); |
jvfausto | 0:c9ec02922858 | 51 | imu.set_accel_units(MPERSPERS); |
jvfausto | 0:c9ec02922858 | 52 | imu.setmode(OPERATION_MODE_AMG); |
jvfausto | 0:c9ec02922858 | 53 | imu.read_calibration_data(); |
jvfausto | 0:c9ec02922858 | 54 | imu.write_calibration_data(); |
jvfausto | 0:c9ec02922858 | 55 | imu.set_angle_units(DEGREES); |
cpbenite | 1:4fb66612cb9c | 56 | } |
cpbenite | 1:4fb66612cb9c | 57 | |
cpbenite | 1:4fb66612cb9c | 58 | void printAccel() { |
cpbenite | 1:4fb66612cb9c | 59 | pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y + .25, imu.accel.z); |
cpbenite | 1:4fb66612cb9c | 60 | } |
cpbenite | 1:4fb66612cb9c | 61 | |
cpbenite | 1:4fb66612cb9c | 62 | void printGyro() { |
cpbenite | 1:4fb66612cb9c | 63 | pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z); |
cpbenite | 1:4fb66612cb9c | 64 | } |
cpbenite | 1:4fb66612cb9c | 65 | |
cpbenite | 1:4fb66612cb9c | 66 | void printMag() { |
cpbenite | 1:4fb66612cb9c | 67 | pc.printf("magnometer { x: %d\t y: %d\t z: %d }\r\n", imu.mag.x, imu.mag.y, imu.mag.z); |
cpbenite | 1:4fb66612cb9c | 68 | } |
cpbenite | 1:4fb66612cb9c | 69 | |
cpbenite | 1:4fb66612cb9c | 70 | void printValues() { |
cpbenite | 1:4fb66612cb9c | 71 | printAccel(); |
cpbenite | 1:4fb66612cb9c | 72 | printGyro(); |
cpbenite | 1:4fb66612cb9c | 73 | printMag(); |
cpbenite | 1:4fb66612cb9c | 74 | } |
cpbenite | 1:4fb66612cb9c | 75 | |
cpbenite | 1:4fb66612cb9c | 76 | void checkAvail() { |
cpbenite | 1:4fb66612cb9c | 77 | while (imu.check() == 0){ |
cpbenite | 1:4fb66612cb9c | 78 | pc.printf("Bosch BNO055 is NOT available!!\r\n"); |
cpbenite | 1:4fb66612cb9c | 79 | wait_ms(200); |
cpbenite | 1:4fb66612cb9c | 80 | } |
cpbenite | 1:4fb66612cb9c | 81 | |
cpbenite | 1:4fb66612cb9c | 82 | pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n"); |
cpbenite | 1:4fb66612cb9c | 83 | } |
cpbenite | 1:4fb66612cb9c | 84 | |
cpbenite | 1:4fb66612cb9c | 85 | int main() { |
cpbenite | 1:4fb66612cb9c | 86 | |
cpbenite | 1:4fb66612cb9c | 87 | setup(); |
cpbenite | 1:4fb66612cb9c | 88 | //pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n"); |
cpbenite | 1:4fb66612cb9c | 89 | checkAvail(); |
cpbenite | 1:4fb66612cb9c | 90 | printInfo(); |
cpbenite | 1:4fb66612cb9c | 91 | |
cpbenite | 1:4fb66612cb9c | 92 | while (1) { |
cpbenite | 1:4fb66612cb9c | 93 | getValues(); |
cpbenite | 1:4fb66612cb9c | 94 | printMag(); |
jvfausto | 0:c9ec02922858 | 95 | |
cpbenite | 1:4fb66612cb9c | 96 | result = (double) imu.mag.x / imu.mag.y; |
cpbenite | 1:4fb66612cb9c | 97 | angleToNorth = getAngleToNorth(imu.mag.x, imu.mag.y, result); |
jvfausto | 0:c9ec02922858 | 98 | |
cpbenite | 1:4fb66612cb9c | 99 | pc.printf("it is pointing %f angle from north \r\n\n", angleToNorth); |
cpbenite | 1:4fb66612cb9c | 100 | wait_us(200); |
jvfausto | 0:c9ec02922858 | 101 | } |
cpbenite | 1:4fb66612cb9c | 102 | } |