From 824a33fd433980c2200e45b2429aeeabbd0ef646 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 1 Aug 2018 08:55:41 +0200
Subject: [PATCH] Added the simulated IMU to the example.
---
src/CMakeLists.txt | 2 ++
src/darwin_loc_nav.cpp | 34 ++++++++++++++++++++++++++++++++++
2 files changed, 36 insertions(+)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 77eff4c..acb270a 100644
--- a/src/CMakeLists.txt
+++ b/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})
diff --git a/src/darwin_loc_nav.cpp b/src/darwin_loc_nav.cpp
index 3f126c5..f321cf6 100755
--- a/src/darwin_loc_nav.cpp
+++ b/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();
--
GitLab