Added the simulated IMU to the example.

src/CMakeLists.txt

@@ -17,6 +17,7 @@ INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${darwin_robot_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(${detectqrcode_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${bno055_imu_driver_INCLUDE_DIR})
 # application source files
 SET(sources darwin_loc_nav.cpp)
 # application header files
@@ -30,3 +31,4 @@ TARGET_LINK_LIBRARIES(darwin_loc_nav ${dynamixel_LIBRARY})
 TARGET_LINK_LIBRARIES(darwin_loc_nav ${darwin_robot_LIBRARY})
 TARGET_LINK_LIBRARIES(darwin_loc_nav ${OpenCV_LIBRARIES})
 TARGET_LINK_LIBRARIES(darwin_loc_nav ${detectqrcode_LIBRARY})
+TARGET_LINK_LIBRARIES(darwin_loc_nav ${bno055_imu_driver_LIBRARY})

src/darwin_loc_nav.cpp

@@ -2,13 +2,16 @@
 #include "darwin_robot_exceptions.h"
 #include "action_id.h"
 #include "detectqrcode.h"
+#include "bno055_imu_driver.h"
 
 #include <iostream>
 
 #ifdef _SIM
 std::string robot_device="/dev/pts/20";
+std::string imu_device="/dev/pts/23";
 #else
 std::string robot_device="A603LOBS";
+std::string imu_device="/dev/ttyUSB0";
 #endif
 
 int main(int argc, char *argv[])
@@ -17,6 +20,7 @@ int main(int argc, char *argv[])
   cv::Mat gray;
   int i=0,num_servos;
   unsigned int present_servos;
+  bool accel_cal,mag_cal,gyro_cal;
   std::vector<std::vector<cv::Point> > squares;
 
   try{
@@ -40,6 +44,36 @@ int main(int argc, char *argv[])
 
     cv::namedWindow("QRcode",1);
 
+    /* initialize IMU */
+    CBNO055IMUDriver imu("darwin_imu");
+    imu.open(imu_device);
+    try{
+      imu.load_calibration("bno055.cal");
+      do{
+        imu.set_operation_mode(ndof_mode);
+        accel_cal=imu.is_accelerometer_calibrated();
+        std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
+        mag_cal=imu.is_magnetometer_calibrated();
+        std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
+        gyro_cal=imu.is_gyroscope_calibrated();
+        std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
+        sleep(1);
+      }while(!accel_cal || !mag_cal || !gyro_cal);
+    }catch(CException &e){
+      std::cout << e.what() << std::endl;
+      do{
+        imu.set_operation_mode(ndof_mode);
+        accel_cal=imu.is_accelerometer_calibrated();
+        std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
+        mag_cal=imu.is_magnetometer_calibrated();
+        std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
+        gyro_cal=imu.is_gyroscope_calibrated();
+        std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
+        sleep(1);
+      }while(!accel_cal || !mag_cal || !gyro_cal);
+      imu.save_calibration("bno055.cal");
+    }
+
     /* initialize robot */
     num_servos=darwin.mm_get_num_servos();
     present_servos=darwin.mm_get_present_servos();