Christian Benitez / Mbed 2 deprecated IMU

Cleaned up

Dependencies:   BNO055 mbed

Fork of LMU by Jesus Fausto

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?

UserRevisionLine numberNew 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 }