1108
Dependencies: HC_SR04_Ultrasonic_Library QEI mbed ros_lib_kinetic_weber
Fork of MPU6050IMU by
main.cpp
- Committer:
- WeberYang
- Date:
- 2018-11-08
- Revision:
- 3:029450d064bb
- Parent:
- 1:cea9d83b8636
File content as of revision 3:029450d064bb:
#include "mbed.h" #include "config.h" #include "CAN.h" #include "MPU6050.h" #include "ultrasonic.h" #include "QEI.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; 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; } 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 ); } //==========define parameter=================== //=======encoder================ long _PreviousLeftEncoderCounts = 0; long _PreviousRightEncoderCounts = 0; double x; double y; double th; //=======imu_msgs================ float sum = 0; uint32_t sumCount = 0; //=======ultrsonic int sonar_count; //===================================================== int main() { //Set up I2C myled = !myled; wait_ms(100); nh.initNode(); nh.advertise(imu_pub); 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 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); 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); while(1) ; // Loop forever if communication doesn't happen } while(1) { dis1.checkDistance(); DI.data = DI_read(PCF8574_ADDR_2); DI_pub.publish( &DI ); nh.spinOnce(); wait_ms(100); } } 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(); // 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]; 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; 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); myled= !myled; count = t.read_ms(); sum = 0; sumCount = 0; }