Revision 1:1e40c57efbb1, committed 2018-12-19

Comitter:

chawankorn

Date:

Wed Dec 19 08:24:48 2018 +0000

Parent:

0:7f3dd28ad889

Commit message:

for class project

Changed in this revision

--- a/main.cpp	Sat Dec 15 08:49:09 2018 +0000
+++ b/main.cpp	Wed Dec 19 08:24:48 2018 +0000
@@ -1,13 +1,22 @@
 #include "mbed.h"
 #include "BNO055.h"
+#include <ros.h>
+#include <std_msgs/Float64.h>
 
 Serial pc(USBTX, USBRX);
 BNO055 imu(I2C_SDA,I2C_SCL);
 DigitalOut led(LED1);
 
+ros::NodeHandle nh;
+
+std_msgs::Float64 E_roll;
+ros::Publisher pub_roll("Euler roll", &E_roll);
+
 int main() {
     pc.baud(115200);
-    pc.printf("BNO055 Hello World\r\n\r\n");
+    nh.initNode();
+    nh.advertise(pub_roll);
+    //pc.printf("BNO055 Hello World++\r\n\r\n");
     led = 1;
     // Reset the BNO055
     imu.reset();
@@ -18,7 +27,7 @@
             wait(0.1);
         }
     // Display sensor information
-    pc.printf("BNO055 found\r\n\r\n");
+    /*pc.printf("BNO055 found\r\n\r\n");
     pc.printf("Chip          ID: %u\r\n",imu.ID.id);
     pc.printf("Accelerometer ID: %u\r\n",imu.ID.accel);
     pc.printf("Gyroscope     ID: %u\r\n",imu.ID.gyro);
@@ -29,17 +38,24 @@
     for (int i = 0; i<4; i++){
         pc.printf("%u.%u.%u.%u\r\n",imu.ID.serial[i*4],imu.ID.serial[i*4+1],imu.ID.serial[i*4+2],imu.ID.serial[i*4+3]);
     }
-    pc.printf("\r\n");
+    pc.printf("\r\n");*/
 
-    while (true) {
+    while (true) {     
         imu.setmode(OPERATION_MODE_NDOF);
         imu.get_calib();
         imu.get_angles();
         imu.get_temp();
         imu.get_quat();
+        
+        float roll_angle;
+        roll_angle = imu.euler.roll;        
+        E_roll.data = roll_angle;
+        pub_roll.publish(&E_roll);
         pc.printf("Temperature\t%d\n", imu.temperature);
-        pc.printf("Quaternion: w:%f\tx:%f\ty:%f\tz:%f\n", imu.quat.w, imu.quat.x, imu.quat.y, imu.quat.z);
-        //pc.printf("%u\t\t%5.1d\t\t%5.1d\t\t%5.1d\r\n",imu.calib,imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
+        //pc.printf("Quaternion: w:%f\tx:%f\ty:%f\tz:%f\n", imu.quat.w, imu.quat.x, imu.quat.y, imu.quat.z);
+        pc.printf("%u\t roll:%5.1f\t pitch:%5.1f\t yaw:%5.1f\r\n",imu.calib,imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
+        nh.spinOnce();
         wait(0.5);
     }
+    nh.spinOnce();
 }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_kinetic.lib	Wed Dec 19 08:24:48 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f