1108

Dependencies:   HC_SR04_Ultrasonic_Library QEI mbed ros_lib_kinetic_weber

Fork of MPU6050IMU by Kris Winer

Files at this revision

API Documentation at this revision

Comitter:
WeberYang
Date:
Thu Nov 08 05:41:28 2018 +0000
Parent:
2:e0381ca0edac
Commit message:
1108

Changed in this revision

HC_SR04_Ultrasonic_Library.lib Show annotated file Show diff for this revision Revisions of this file
MPU6050.h Show annotated file Show diff for this revision Revisions of this file
N5110.lib Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
config.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
ros_lib_kinetic_weber.lib Show annotated file Show diff for this revision Revisions of this file
diff -r e0381ca0edac -r 029450d064bb HC_SR04_Ultrasonic_Library.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HC_SR04_Ultrasonic_Library.lib	Thu Nov 08 05:41:28 2018 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
diff -r e0381ca0edac -r 029450d064bb MPU6050.h
--- a/MPU6050.h	Sun Jun 29 21:53:23 2014 +0000
+++ b/MPU6050.h	Thu Nov 08 05:41:28 2018 +0000
@@ -153,7 +153,7 @@
 int Ascale = AFS_2G;
 
 //Set up I2C, (SDA,SCL)
-I2C i2c(I2C_SDA, I2C_SCL);
+I2C i2c(PB_7, PB_6);
 
 DigitalOut myled(LED1);
    
diff -r e0381ca0edac -r 029450d064bb N5110.lib
--- a/N5110.lib	Sun Jun 29 21:53:23 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/onehorse/code/MPU60506-axisMotionSensor/#313c258ada8a
diff -r e0381ca0edac -r 029450d064bb QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Nov 08 05:41:28 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r e0381ca0edac -r 029450d064bb config.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config.h	Thu Nov 08 05:41:28 2018 +0000
@@ -0,0 +1,22 @@
+// FIFO rate = 200Hz / (1 + this value)
+// For example, 0x01 is 100Hz, 0x03 is 50Hz.
+// 0x00 to 0x09
+#define IMU_FIFO_RATE_DIVIDER 0x09
+
+// Sample rate = 1kHz / (1 + this valye)
+// For example, 4 is 200Hz.
+#define IMU_SAMPLE_RATE_DIVIDER 4
+
+// measuring range of gyroscope (±n deg/s)
+// But other value doesn't yet support.
+#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
+
+// measuring range of acceleration sensor (±n g)
+// But other value doesn't yet support.
+#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
+
+#define PC_BAUDRATE 115200
+
+#define PCF8574_ADDR     (0x40)
+#define PCF8574_ADDR_1     (0x40) 
+#define PCF8574_ADDR_2     (0x42) 
diff -r e0381ca0edac -r 029450d064bb main.cpp
--- a/main.cpp	Sun Jun 29 21:53:23 2014 +0000
+++ b/main.cpp	Thu Nov 08 05:41:28 2018 +0000
@@ -1,224 +1,261 @@
+#include "mbed.h"
+#include "config.h"
+#include "CAN.h"
+#include "MPU6050.h"
+#include "ultrasonic.h"
+#include "QEI.h"
 
-/* MPU6050 Basic Example Code
- by: Kris Winer
- date: May 1, 2014
- license: Beerware - Use this code however you'd like. If you 
- find it useful you can buy me a beer some time.
- 
- Demonstrate  MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as
- parameterizing the register addresses. Added display functions to allow display to on breadboard monitor. 
- No DMP use. We just want to get out the accelerations, temperature, and gyro readings.
- 
- SDA and SCL should have external pull-up resistors (to 3.3V).
- 10k resistors worked for me. They should be on the breakout
- board.
- 
- Hardware setup:
- MPU6050 Breakout --------- Arduino
- 3.3V --------------------- 3.3V
- SDA ----------------------- A4
- SCL ----------------------- A5
- GND ---------------------- GND
- 
-  Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library. 
- Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
- We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
- We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
- */
- 
-#include "mbed.h"
-#include "MPU6050.h"
-#include "N5110.h"
+#include <ros.h>
+#include <ros/time.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/Int16.h>
+#include <geometry_msgs/Quaternion.h>
+#include <std_msgs/Bool.h> 
+#include <sensor_msgs/BatteryState.h>
+
+//=========define function title===================
+void dmpDataUpdate();
+bool pcf8574_write(uint8_t data,uint8_t address);
+bool pcf8574_read(uint8_t* data,uint8_t address);
+int DO_write(uint8_t value,uint8_t address);
+int DI_read(uint8_t address);
+void dist(int distance);
+
+//=======define pin user definition============
+I2C dio_i2c(PB_11, PB_10);
+MPU6050 mpu6050;
+QEI encoder2( PA_0, PA_1, NC, 1000, QEI::X4_ENCODING);
+QEI encoder1( PA_8, PA_9, NC, 1000, QEI::X4_ENCODING);
+Timer t;   // timer for polling
+Ticker IMU_flipper;
+Ticker Sensor_flipper;
+ultrasonic dis1(D3, D4, .01, .05, &dist);    //Set the trigger pin to D8 and the echo pin to D9
+
+
+//==============================ROS==================================
+    ros::NodeHandle  nh;
+    char frameid[] = "/imu";
+    sensor_msgs::Imu imu_msg;
+    ros::Publisher imu_pub("imu_msgs", &imu_msg);
+    std_msgs::Int16 DI;
+    ros::Publisher DI_pub("DI_pub", &DI);
+    
+    geometry_msgs::Vector3 encoder;
+    ros::Publisher Enc_pub("wheel_encoder", &encoder); 
+    
+    std_msgs::Int16 sonar_msg;
+    ros::Publisher pub_sonar("sonar", &sonar_msg);
+    
+    geometry_msgs::Quaternion odom_quat;
 
-// Using NOKIA 5110 monochrome 84 x 48 pixel display
-// pin 9 - Serial clock out (SCLK)
-// pin 8 - Serial data out (DIN)
-// pin 7 - Data/Command select (D/C)
-// pin 5 - LCD chip select (CS)
-// pin 6 - LCD reset (RST)
-//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
+    std_msgs::Int16 DO;
+    void DOActionCb(const std_msgs::Int16 &msg){
+        DO_write(msg.data, PCF8574_ADDR_1);
+    }
+    ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DOActionCb);
+    ros::Time current_time,last_time;
+    
+    std_msgs::Bool flag_reset_encoder;
+    void reset_encoderCb(const std_msgs::Bool &msg){
+        if (msg.data == 1) {
+            encoder1.reset();
+            encoder2.reset();
+        } 
+    }
+    ros::Subscriber<std_msgs::Bool> resetEncoder_sub("resetEncoder", &reset_encoderCb);
+//======================================================================
+
+//=========define function ===================
+bool pcf8574_write(uint8_t data,uint8_t address){
+    return dio_i2c.write(address, (char*) &data, 1, 0) == 0;
+}
+
+bool pcf8574_read(uint8_t* data,uint8_t address){
+    return dio_i2c.read(address, (char*) data, 1, 0) == 0;
+}
+
+int DO_write(uint8_t value,uint8_t address){
+    int ret;
+  
+    ret = pcf8574_write(value,address);
+
+    return ret;
+}
 
-float sum = 0;
-uint32_t sumCount = 0;
+int DI_read(uint8_t address){
+    int ret;
+    uint8_t data=0;
+
+    ret = pcf8574_read(&data,address);
+    if(!ret) return -2;
+    
+    return data;
+}
+void dist(int distance)
+{
+    sonar_msg.data = distance;
+    pub_sonar.publish( &sonar_msg );
+}
 
-   MPU6050 mpu6050;
-   
-   Timer t;
+//==========define parameter===================
+    //=======encoder================
+    long _PreviousLeftEncoderCounts = 0;
+    long _PreviousRightEncoderCounts = 0;
+    double x;
+    double y;
+    double th;
+    
+    //=======imu_msgs================
+    float sum = 0;
+    uint32_t sumCount = 0;
 
-   Serial pc(USBTX, USBRX); // tx, rx
 
-   //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
-   N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
-        
+    //=======ultrsonic
+    int sonar_count;
+    
+
+//=====================================================
 int main()
 {
-  pc.baud(9600);  
-
-  //Set up I2C
-  i2c.frequency(400000);  // use fast (400 kHz) I2C   
-  
-  t.start();        
-  
-  lcd.init();
-  lcd.setBrightness(0.05);
-  
+    //Set up I2C
+    myled = !myled;
+    wait_ms(100);
+    nh.initNode();
+    nh.advertise(imu_pub);
     
-  // Read the WHO_AM_I register, this is a good test of communication
-  uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
-  pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
-  
-  if (whoami == 0x68) // WHO_AM_I should always be 0x68
-  {  
-    pc.printf("MPU6050 is online...");
-    wait(1);
-    lcd.clear();
-    lcd.printString("MPU6050 OK", 0, 0);
-
+    nh.advertise(DI_pub);
+    nh.advertise(Enc_pub);
+    nh.advertise(pub_sonar);
+    
+    nh.subscribe(ACT_sub);
+    nh.subscribe(resetEncoder_sub);
+    dis1.startUpdates();//start mesuring the distance
     
-    mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-    pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
-    pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
-    pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
-    pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
-    pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
-    pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
-    wait(1);
-
-    if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
-    {
-    mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
-    mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
-    mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+    i2c.frequency(400000);  // use fast (400 kHz) I2C   
+    IMU_flipper.attach(&dmpDataUpdate, 0.06);
+    t.start();        
+  
+    // Read the WHO_AM_I register, this is a good test of communication
+    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+//    pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
+  
+    if (whoami == 0x68) // WHO_AM_I should always be 0x68
+    {  
+//        pc.printf("MPU6050 is online...");
+        wait(1);
+    
+        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+        wait(1);
 
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("pass self test", 0, 1);
-    lcd.printString("initializing", 0, 2);  
-    wait(2);
-       }
-    else
-    {
-    pc.printf("Device did not the pass self-test!\n\r");
- 
-       lcd.clear();
-       lcd.printString("MPU6050", 0, 0);
-       lcd.printString("no pass", 0, 1);
-       lcd.printString("self test", 0, 2);      
-      }
+        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
+        {
+            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
+            mpu6050.initMPU6050(); 
+            wait(2);
+        }
+        else
+        {
+//            pc.printf("Device did not the pass self-test!\n\r");
+        }
     }
     else
     {
-    pc.printf("Could not connect to MPU6050: \n\r");
-    pc.printf("%#x \n",  whoami);
- 
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("no connection", 0, 1);
-    lcd.printString("0x", 0, 2);  lcd.setXYAddress(20, 2); lcd.printChar(whoami);
- 
-    while(1) ; // Loop forever if communication doesn't happen
-  }
+//        pc.printf("Could not connect to MPU6050: \n\r");
+//        pc.printf("%#x \n",  whoami);
+        while(1) ; // Loop forever if communication doesn't happen
+    }
 
-
+    while(1) 
+    {
+        dis1.checkDistance();         
 
- while(1) {
-  
-  // If data ready bit set, all data registers have new data
-  if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
-    mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
-    mpu6050.getAres();
-    
-    // Now we'll calculate the accleration value into actual g's
-    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-    ay = (float)accelCount[1]*aRes - accelBias[1];   
-    az = (float)accelCount[2]*aRes - accelBias[2];  
-   
-    mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
-    mpu6050.getGres();
- 
-    // Calculate the gyro value into actual degrees per second
-    gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
-    gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
-    gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
+        DI.data = DI_read(PCF8574_ADDR_2);
+        DI_pub.publish( &DI );
 
-    tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
-    temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
-   }  
-   
-    Now = t.read_us();
-    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-    lastUpdate = Now;
-    
-    sum += deltat;
-    sumCount++;
-    
-    if(lastUpdate - firstUpdate > 10000000.0f) {
-     beta = 0.04;  // decrease filter gain after stabilized
-     zeta = 0.015; // increasey bias drift gain after stabilized
+        nh.spinOnce();
+        wait_ms(100);
+
     }
-    
-   // Pass gyro rate as rad/s
-    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-
-    // Serial print and/or display at 0.5 s rate independent of data rates
-    delt_t = t.read_ms() - count;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
+}
 
-    pc.printf("ax = %f", 1000*ax); 
-    pc.printf(" ay = %f", 1000*ay); 
-    pc.printf(" az = %f  mg\n\r", 1000*az); 
-
-    pc.printf("gx = %f", gx); 
-    pc.printf(" gy = %f", gy); 
-    pc.printf(" gz = %f  deg/s\n\r", gz); 
-    
-    pc.printf(" temperature = %f  C\n\r", temperature); 
+void dmpDataUpdate() {
+       // If data ready bit set, all data registers have new data
+        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) 
+        {  // check if data ready interrupt
+            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+            mpu6050.getAres();
     
-    pc.printf("q0 = %f\n\r", q[0]);
-    pc.printf("q1 = %f\n\r", q[1]);
-    pc.printf("q2 = %f\n\r", q[2]);
-    pc.printf("q3 = %f\n\r", q[3]);      
-    
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("x   y   z", 0, 1);
-    lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
-    lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
-    lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
-    
+            // Now we'll calculate the accleration value into actual g's
+            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+            ay = (float)accelCount[1]*aRes - accelBias[1];   
+            az = (float)accelCount[2]*aRes - accelBias[2];  
+       
+            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+            mpu6050.getGres();
+     
+            // Calculate the gyro value into actual degrees per second
+            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
+            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
+            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
     
-  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
-  // In this coordinate system, the positive z-axis is down toward Earth. 
-  // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
-  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
-  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
-  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
-  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
-  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
-  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
-    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
-    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-    pitch *= 180.0f / PI;
-    yaw   *= 180.0f / PI; 
-    roll  *= 180.0f / PI;
+            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
+            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+        }  
+        Now = t.read_us();
+        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        lastUpdate = Now;
+        
+        sum += deltat;
+        sumCount++;
+        
+        if(lastUpdate - firstUpdate > 10000000.0f) 
+        {
+            beta = 0.04;  // decrease filter gain after stabilized
+            zeta = 0.015; // increasey bias drift gain after stabilized
+        }
+        
+       // Pass gyro rate as rad/s
+        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+
+        // Serial print and/or display at 0.5 s rate independent of data rates
+        delt_t = t.read_ms() - count;
+        // update LCD once per half-second independent of read rate
+        yaw   = atan2(2.1f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
+        pitch = -asin(2.1f * (q[1] * q[3] - q[0] * q[2]));
+        roll  = atan2(2.1f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+        pitch *= 180.0f / PI;
+        yaw   *= 180.0f / PI; 
+        roll  *= 180.0f / PI;
 
-//    pc.printf("Yaw, Pitch, Roll: \n\r");
-//    pc.printf("%f", yaw);
-//    pc.printf(", ");
-//    pc.printf("%f", pitch);
-//    pc.printf(", ");
-//    pc.printf("%f\n\r", roll);
-//    pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
+        imu_msg.header.frame_id = frameid;
+        imu_msg.header.stamp = nh.now();
+        imu_msg.linear_acceleration.x = ax*9.81;//dmpData.acc.x;
+        imu_msg.linear_acceleration.y = ay*9.81;//dmpData.acc.y;
+        imu_msg.linear_acceleration.z = az*9.81;//dmpData.acc.z;
+        imu_msg.angular_velocity.x = gx/180*3.1415926;//dmpData.gyro.x;
+        imu_msg.angular_velocity.y = gy/180*3.1415926;//dmpData.gyro.y;
+        imu_msg.angular_velocity.z = gz/180*3.1415926;//dmpData.gyro.z;
+        imu_msg.orientation.w = q[0];
+        imu_msg.orientation.x = q[1];
+        imu_msg.orientation.y = q[2];
+        imu_msg.orientation.z = q[3];
+        imu_msg.orientation_covariance[8] = 0;
+        imu_msg.angular_velocity_covariance[8] = 0;
+        imu_msg.linear_acceleration_covariance[8] = 0;
+        imu_pub.publish( &imu_msg );  
+        wait_ms(30);//must bigger then 30ms at 115200
+        
+        encoder.x = encoder1.getPulses();
+        encoder.y = encoder2.getPulses();
+        Enc_pub.publish(&encoder); 
+        wait_ms(1);
+        
 
-     pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-     pc.printf("average rate = %f\n\r", (float) sumCount/sum);
- 
-    myled= !myled;
-    count = t.read_ms(); 
-    sum = 0;
-    sumCount = 0; 
+        
+        myled= !myled;
+        count = t.read_ms(); 
+        sum = 0;
+        sumCount = 0; 
 }
-}
- 
- }
\ No newline at end of file
+
diff -r e0381ca0edac -r 029450d064bb mbed.bld
--- a/mbed.bld	Sun Jun 29 21:53:23 2014 +0000
+++ b/mbed.bld	Thu Nov 08 05:41:28 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/0b3ab51c8877
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/9baf128c2fab
\ No newline at end of file
diff -r e0381ca0edac -r 029450d064bb ros_lib_kinetic_weber.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_kinetic_weber.lib	Thu Nov 08 05:41:28 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/WeberYang/code/ros_lib_kinetic_weber/#40f11e1dc026