![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Cleaned up
Fork of LMU by
Revision 1:4fb66612cb9c, committed 2018-08-03
- Comitter:
- cpbenite
- Date:
- Fri Aug 03 22:00:54 2018 +0000
- Parent:
- 0:c9ec02922858
- Commit message:
- Cleaned up code
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r c9ec02922858 -r 4fb66612cb9c main.cpp --- a/main.cpp Mon Jul 16 18:40:15 2018 +0000 +++ b/main.cpp Fri Aug 03 22:00:54 2018 +0000 @@ -5,7 +5,7 @@ #define PI 3.141593 Serial pc(USBTX,USBRX); - BNO055 imu(PTE25, PTE24); // SDA, SCL + BNO055 imu(D14, D15); // SDA, SCL double y; double x; @@ -14,51 +14,89 @@ double result; // BNO055_ID_INF_TypeDef bno055_id_inf; // BNO055_EULER_TypeDef euler_angles; + + double getAngleToNorth (double x, double y, double result) { + if (y > 0) { + return 90.0 - atan(result) * 180/PI; + } + else if (y < 0) { + return 270.0 - atan(result) * 180/PI; + } + else if (y == 0 && x <= 0) { + return 180; + } + else if (y == 0 && x > 0) { + return 0; + } +} - int main() { -// setup(); - imu.reset(); - pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n"); - while (imu.check() == 0){ - pc.printf("Bosch BNO055 is NOT available!!\r\n"); - wait(.5); - } - // pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n"); - pc.printf("BNO055 found\r\n\r\n"); +void getValues() { + imu.get_accel(); + imu.get_gyro(); + imu.get_mag(); +} + +void printInfo() { + //pc.printf("BNO055 found\r\n\r\n"); pc.printf("Chip ID: %0z\r\n",imu.ID.id); pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel); pc.printf("Gyroscope ID: %0z\r\n",imu.ID.gyro); pc.printf("Magnetometer ID: %0z\r\n\r\n",imu.ID.mag); pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]); pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload); +} + +void setup() { + imu.reset(); imu.set_accel_units(MPERSPERS); imu.setmode(OPERATION_MODE_AMG); imu.read_calibration_data(); imu.write_calibration_data(); imu.set_angle_units(DEGREES); - while(1) - { - imu.setmode(OPERATION_MODE_AMG); - imu.get_accel(); - pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y+.25, imu.accel.z); - imu.get_gyro(); - pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z); - imu.get_mag(); - pc.printf("magnometer: x %f\ty %f\t %f \r\n", imu.mag.x, imu.mag.y, imu.mag.z); - x = imu.mag.x; - y = imu.mag.y; +} + +void printAccel() { + pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y + .25, imu.accel.z); +} + +void printGyro() { + pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z); +} + +void printMag() { + pc.printf("magnometer { x: %d\t y: %d\t z: %d }\r\n", imu.mag.x, imu.mag.y, imu.mag.z); +} + +void printValues() { + printAccel(); + printGyro(); + printMag(); +} + +void checkAvail() { + while (imu.check() == 0){ + pc.printf("Bosch BNO055 is NOT available!!\r\n"); + wait_ms(200); + } + + pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n"); +} + +int main() { + + setup(); + //pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n"); + checkAvail(); + printInfo(); + + while (1) { + getValues(); + printMag(); - result = x/y; + result = (double) imu.mag.x / imu.mag.y; + angleToNorth = getAngleToNorth(imu.mag.x, imu.mag.y, result); - if(imu.mag.y>0) - angleToNorth = 90.0 - atan(result)*180/PI; - else if(imu.mag.y<0) - angleToNorth = 270.0 - atan(result)*180/PI; - else if(y == 0 && x <= 0) - angleToNorth = 180; - else if(y == 0 && x > 0) - angleToNorth = 0; - pc.printf("it is pointing %f angle from north \r\n", angleToNorth); - wait(1); + pc.printf("it is pointing %f angle from north \r\n\n", angleToNorth); + wait_us(200); } -} \ No newline at end of file +}