Branch for servo motor
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_SMT by
Diff: main.cpp
- Revision:
- 7:1a234f02746f
- Parent:
- 6:eadfb1b45bda
- Child:
- 8:4974fc24fbd7
diff -r eadfb1b45bda -r 1a234f02746f main.cpp --- 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