main.cpp

00001 /*
00002     Ultrasound and IMU Demo
00003     Sam Walsh 2018
00004 */
00005 
00006 
00007 /* Includes ------------------------------------------------------------------*/
00008 
00009 #include "mbed.h"
00010 #include <string.h>
00011 #include <stdlib.h>
00012 #include <stdio.h>
00013 #include <assert.h>
00014 #include "hcsr04.h"
00015 #include    "BNO055.h"
00016 
00017 
00018 /* Definitions ---------------------------------------------------------------*/
00019 
00020 /* Variables -----------------------------------------------------------------*/
00021 
00022 HCSR04 UltrasonicSensor(D7,D8); //Trigger,Echo 
00023 Serial pc(USBTX, USBRX); // tx, rx
00024 I2C    i2c(D4, D5); // SDA, SCL
00025 BNO055 imu(i2c, D6);  // Reset =D6, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
00026 
00027 Timer t;
00028 
00029 //  RAM -------------------------------------------------------------------------------------------
00030 BNO055_ID_INF_TypeDef       bno055_id_inf;
00031 BNO055_EULER_TypeDef        euler_angles;
00032 BNO055_QUATERNION_TypeDef   quaternion;
00033 BNO055_LIN_ACC_TypeDef      linear_acc;
00034 BNO055_GRAVITY_TypeDef      gravity;
00035 BNO055_TEMPERATURE_TypeDef  chip_temp;
00036 
00037 float distance;
00038 
00039 /* Functions -----------------------------------------------------------------*/
00040 
00041 /*=================================== Main ==================================
00042   Prints on the serial over USB the measured distance and lux.
00043   The measures are run in single shot polling mode.
00044 =============================================================================*/
00045 int main()
00046 { 
00047     pc.baud(921600);
00048     pc.printf ("Hello World \n\r"); 
00049     imu.set_mounting_position(MT_P6);
00050     // Is BNO055 avairable?
00051     if (imu.chip_ready() == 0){
00052         do {
00053             pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
00054         } while(imu.reset());
00055     }
00056     pc.printf("Bosch BNO055 is available now!!\r\n");
00057     pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n",
00058                imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));    
00059     imu.read_id_inf(&bno055_id_inf);
00060     pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
00061         bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
00062     pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
00063         bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
00064 
00065     pc.printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],");
00066     pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n");
00067     t.start();
00068     while(1) {
00069         pc.printf ("Ultrasound: %d \t", (int)distance);  
00070         distance = UltrasonicSensor.distance(); 
00071         imu.get_Euler_Angles(&euler_angles);
00072         pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
00073                    euler_angles.h, euler_angles.r, euler_angles.p);
00074         imu.get_quaternion(&quaternion);
00075         pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
00076                    quaternion.w, quaternion.x, quaternion.y, quaternion.z);
00077         imu.get_linear_accel(&linear_acc);
00078         pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
00079                    linear_acc.x, linear_acc.y, linear_acc.z);
00080         imu.get_gravity(&gravity);
00081         pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
00082                    gravity.x, gravity.y, gravity.z);
00083         imu.get_chip_temperature(&chip_temp);
00084         pc.printf("[T],%+d,%+d,",
00085                    chip_temp.acc_chip, chip_temp.gyr_chip);
00086         pc.printf("[S],0x%x,[M],%d\r\n",
00087                    imu.read_calib_status(), t.read_ms());
00088     }
00089 }
00090 
00091 
00092     //while(1){
00093  
00094 //      wait(0.5);
00095 //    }
00096 //}