Diff: main.cpp

Revision:

7:1a234f02746f

Parent:

6:eadfb1b45bda

Child:

8:4974fc24fbd7

--- a/main.cpp	Thu May 10 01:39:34 2018 +0000
+++ b/main.cpp	Thu May 10 08:16:14 2018 +0000
@@ -394,30 +394,30 @@
     nh.advertise(pub_rmotor);
     nh.advertise(BT_pub);
     nh.subscribe(cmd_vel_sub);
-    //mpu.initialize();
-//    if (mpu.testConnection()) {
-////        pc.printf("MPU6050 test connection passed.\n");
-//    } else {
-////        pc.printf("MPU6050 test connection failed.\n");
-//        return false;
-//    }
-//    if (mpu.dmpInitialize() == 0) {
-////        pc.printf("succeed in MPU6050 DMP Initializing.\n");
-//    } else {
-////        pc.printf("failed in MPU6050 DMP Initializing.\n");
-//        return false;
-//    }
-//    mpu.setXAccelOffset(offset.ax);
-//    mpu.setYAccelOffset(offset.ay);
-//    mpu.setZAccelOffset(offset.az);
-//    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
-//    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
-//    mpu.setXGyroOffsetUser(offset.gx);
-//    mpu.setYGyroOffsetUser(offset.gy);
-//    mpu.setZGyroOffsetUser(offset.gz);
-//    mpu.setDMPEnabled(true);    // Enable DMP
-//    packetSize = mpu.dmpGetFIFOPacketSize();
-//    dmpReady = true;    // Enable interrupt.
+    mpu.initialize();
+    if (mpu.testConnection()) {
+//        pc.printf("MPU6050 test connection passed.\n");
+    } else {
+//        pc.printf("MPU6050 test connection failed.\n");
+        return false;
+    }
+    if (mpu.dmpInitialize() == 0) {
+//        pc.printf("succeed in MPU6050 DMP Initializing.\n");
+    } else {
+//        pc.printf("failed in MPU6050 DMP Initializing.\n");
+        return false;
+    }
+    mpu.setXAccelOffset(offset.ax);
+    mpu.setYAccelOffset(offset.ay);
+    mpu.setZAccelOffset(offset.az);
+    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
+    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+    mpu.setXGyroOffset(offset.gx);
+    mpu.setYGyroOffset(offset.gy);
+    mpu.setZGyroOffset(offset.gz);
+    mpu.setDMPEnabled(true);    // Enable DMP
+    packetSize = mpu.dmpGetFIFOPacketSize();
+    dmpReady = true;    // Enable interrupt.
     
     //pc.printf("Init finish!\n");
  
@@ -438,19 +438,19 @@
     while(1){   
         seq++;
         t.start();
-//        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
- //       imu_msg.header.stamp = nh.now();
-//        imu_msg.header.frame_id = 0;
-//        imu_msg.header.seq = seq;
-//        imu_msg.accel.x = ax;
-//        imu_msg.accel.y = ay;
-//        imu_msg.accel.z = az;
-//        imu_msg.gyro.x = gx;
-//        imu_msg.gyro.y = gy;
-//        imu_msg.gyro.z = gz;   
+        imu_msg.header.stamp = nh.now();
+        imu_msg.header.frame_id = 0;
+        imu_msg.header.seq = seq;
+        imu_msg.accel.x = ax;
+        imu_msg.accel.y = ay;
+        imu_msg.accel.z = az;
+        imu_msg.gyro.x = gx;
+        imu_msg.gyro.y = gy;
+        imu_msg.gyro.z = gz;   
 //        
-//        imu_pub.publish( &imu_msg );
+        imu_pub.publish( &imu_msg );
         if (can1.read(rxMsg) && (t.read_ms() > 5000))  {
 //            RS232.printf("  ID      = 0x%.3x\r\n", rxMsg.id);
 
@@ -480,6 +480,6 @@
             } // if
         } // if
         nh.spinOnce();
-        wait_ms(20);         
+        wait_ms(10);         
     }
 }
\ No newline at end of file