![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
1108
Dependencies: HC_SR04_Ultrasonic_Library QEI mbed ros_lib_kinetic_weber
Fork of MPU6050IMU by
main.cpp@3:029450d064bb, 2018-11-08 (annotated)
- Committer:
- WeberYang
- Date:
- Thu Nov 08 05:41:28 2018 +0000
- Revision:
- 3:029450d064bb
- Parent:
- 1:cea9d83b8636
1108
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WeberYang | 3:029450d064bb | 1 | #include "mbed.h" |
WeberYang | 3:029450d064bb | 2 | #include "config.h" |
WeberYang | 3:029450d064bb | 3 | #include "CAN.h" |
WeberYang | 3:029450d064bb | 4 | #include "MPU6050.h" |
WeberYang | 3:029450d064bb | 5 | #include "ultrasonic.h" |
WeberYang | 3:029450d064bb | 6 | #include "QEI.h" |
onehorse | 0:65aa78c10981 | 7 | |
WeberYang | 3:029450d064bb | 8 | #include <ros.h> |
WeberYang | 3:029450d064bb | 9 | #include <ros/time.h> |
WeberYang | 3:029450d064bb | 10 | #include <sensor_msgs/Imu.h> |
WeberYang | 3:029450d064bb | 11 | #include <std_msgs/Int16.h> |
WeberYang | 3:029450d064bb | 12 | #include <geometry_msgs/Quaternion.h> |
WeberYang | 3:029450d064bb | 13 | #include <std_msgs/Bool.h> |
WeberYang | 3:029450d064bb | 14 | #include <sensor_msgs/BatteryState.h> |
WeberYang | 3:029450d064bb | 15 | |
WeberYang | 3:029450d064bb | 16 | //=========define function title=================== |
WeberYang | 3:029450d064bb | 17 | void dmpDataUpdate(); |
WeberYang | 3:029450d064bb | 18 | bool pcf8574_write(uint8_t data,uint8_t address); |
WeberYang | 3:029450d064bb | 19 | bool pcf8574_read(uint8_t* data,uint8_t address); |
WeberYang | 3:029450d064bb | 20 | int DO_write(uint8_t value,uint8_t address); |
WeberYang | 3:029450d064bb | 21 | int DI_read(uint8_t address); |
WeberYang | 3:029450d064bb | 22 | void dist(int distance); |
WeberYang | 3:029450d064bb | 23 | |
WeberYang | 3:029450d064bb | 24 | //=======define pin user definition============ |
WeberYang | 3:029450d064bb | 25 | I2C dio_i2c(PB_11, PB_10); |
WeberYang | 3:029450d064bb | 26 | MPU6050 mpu6050; |
WeberYang | 3:029450d064bb | 27 | QEI encoder2( PA_0, PA_1, NC, 1000, QEI::X4_ENCODING); |
WeberYang | 3:029450d064bb | 28 | QEI encoder1( PA_8, PA_9, NC, 1000, QEI::X4_ENCODING); |
WeberYang | 3:029450d064bb | 29 | Timer t; // timer for polling |
WeberYang | 3:029450d064bb | 30 | Ticker IMU_flipper; |
WeberYang | 3:029450d064bb | 31 | Ticker Sensor_flipper; |
WeberYang | 3:029450d064bb | 32 | ultrasonic dis1(D3, D4, .01, .05, &dist); //Set the trigger pin to D8 and the echo pin to D9 |
WeberYang | 3:029450d064bb | 33 | |
WeberYang | 3:029450d064bb | 34 | |
WeberYang | 3:029450d064bb | 35 | //==============================ROS================================== |
WeberYang | 3:029450d064bb | 36 | ros::NodeHandle nh; |
WeberYang | 3:029450d064bb | 37 | char frameid[] = "/imu"; |
WeberYang | 3:029450d064bb | 38 | sensor_msgs::Imu imu_msg; |
WeberYang | 3:029450d064bb | 39 | ros::Publisher imu_pub("imu_msgs", &imu_msg); |
WeberYang | 3:029450d064bb | 40 | std_msgs::Int16 DI; |
WeberYang | 3:029450d064bb | 41 | ros::Publisher DI_pub("DI_pub", &DI); |
WeberYang | 3:029450d064bb | 42 | |
WeberYang | 3:029450d064bb | 43 | geometry_msgs::Vector3 encoder; |
WeberYang | 3:029450d064bb | 44 | ros::Publisher Enc_pub("wheel_encoder", &encoder); |
WeberYang | 3:029450d064bb | 45 | |
WeberYang | 3:029450d064bb | 46 | std_msgs::Int16 sonar_msg; |
WeberYang | 3:029450d064bb | 47 | ros::Publisher pub_sonar("sonar", &sonar_msg); |
WeberYang | 3:029450d064bb | 48 | |
WeberYang | 3:029450d064bb | 49 | geometry_msgs::Quaternion odom_quat; |
onehorse | 0:65aa78c10981 | 50 | |
WeberYang | 3:029450d064bb | 51 | std_msgs::Int16 DO; |
WeberYang | 3:029450d064bb | 52 | void DOActionCb(const std_msgs::Int16 &msg){ |
WeberYang | 3:029450d064bb | 53 | DO_write(msg.data, PCF8574_ADDR_1); |
WeberYang | 3:029450d064bb | 54 | } |
WeberYang | 3:029450d064bb | 55 | ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DOActionCb); |
WeberYang | 3:029450d064bb | 56 | ros::Time current_time,last_time; |
WeberYang | 3:029450d064bb | 57 | |
WeberYang | 3:029450d064bb | 58 | std_msgs::Bool flag_reset_encoder; |
WeberYang | 3:029450d064bb | 59 | void reset_encoderCb(const std_msgs::Bool &msg){ |
WeberYang | 3:029450d064bb | 60 | if (msg.data == 1) { |
WeberYang | 3:029450d064bb | 61 | encoder1.reset(); |
WeberYang | 3:029450d064bb | 62 | encoder2.reset(); |
WeberYang | 3:029450d064bb | 63 | } |
WeberYang | 3:029450d064bb | 64 | } |
WeberYang | 3:029450d064bb | 65 | ros::Subscriber<std_msgs::Bool> resetEncoder_sub("resetEncoder", &reset_encoderCb); |
WeberYang | 3:029450d064bb | 66 | //====================================================================== |
WeberYang | 3:029450d064bb | 67 | |
WeberYang | 3:029450d064bb | 68 | //=========define function =================== |
WeberYang | 3:029450d064bb | 69 | bool pcf8574_write(uint8_t data,uint8_t address){ |
WeberYang | 3:029450d064bb | 70 | return dio_i2c.write(address, (char*) &data, 1, 0) == 0; |
WeberYang | 3:029450d064bb | 71 | } |
WeberYang | 3:029450d064bb | 72 | |
WeberYang | 3:029450d064bb | 73 | bool pcf8574_read(uint8_t* data,uint8_t address){ |
WeberYang | 3:029450d064bb | 74 | return dio_i2c.read(address, (char*) data, 1, 0) == 0; |
WeberYang | 3:029450d064bb | 75 | } |
WeberYang | 3:029450d064bb | 76 | |
WeberYang | 3:029450d064bb | 77 | int DO_write(uint8_t value,uint8_t address){ |
WeberYang | 3:029450d064bb | 78 | int ret; |
WeberYang | 3:029450d064bb | 79 | |
WeberYang | 3:029450d064bb | 80 | ret = pcf8574_write(value,address); |
WeberYang | 3:029450d064bb | 81 | |
WeberYang | 3:029450d064bb | 82 | return ret; |
WeberYang | 3:029450d064bb | 83 | } |
onehorse | 0:65aa78c10981 | 84 | |
WeberYang | 3:029450d064bb | 85 | int DI_read(uint8_t address){ |
WeberYang | 3:029450d064bb | 86 | int ret; |
WeberYang | 3:029450d064bb | 87 | uint8_t data=0; |
WeberYang | 3:029450d064bb | 88 | |
WeberYang | 3:029450d064bb | 89 | ret = pcf8574_read(&data,address); |
WeberYang | 3:029450d064bb | 90 | if(!ret) return -2; |
WeberYang | 3:029450d064bb | 91 | |
WeberYang | 3:029450d064bb | 92 | return data; |
WeberYang | 3:029450d064bb | 93 | } |
WeberYang | 3:029450d064bb | 94 | void dist(int distance) |
WeberYang | 3:029450d064bb | 95 | { |
WeberYang | 3:029450d064bb | 96 | sonar_msg.data = distance; |
WeberYang | 3:029450d064bb | 97 | pub_sonar.publish( &sonar_msg ); |
WeberYang | 3:029450d064bb | 98 | } |
onehorse | 1:cea9d83b8636 | 99 | |
WeberYang | 3:029450d064bb | 100 | //==========define parameter=================== |
WeberYang | 3:029450d064bb | 101 | //=======encoder================ |
WeberYang | 3:029450d064bb | 102 | long _PreviousLeftEncoderCounts = 0; |
WeberYang | 3:029450d064bb | 103 | long _PreviousRightEncoderCounts = 0; |
WeberYang | 3:029450d064bb | 104 | double x; |
WeberYang | 3:029450d064bb | 105 | double y; |
WeberYang | 3:029450d064bb | 106 | double th; |
WeberYang | 3:029450d064bb | 107 | |
WeberYang | 3:029450d064bb | 108 | //=======imu_msgs================ |
WeberYang | 3:029450d064bb | 109 | float sum = 0; |
WeberYang | 3:029450d064bb | 110 | uint32_t sumCount = 0; |
onehorse | 1:cea9d83b8636 | 111 | |
onehorse | 1:cea9d83b8636 | 112 | |
WeberYang | 3:029450d064bb | 113 | //=======ultrsonic |
WeberYang | 3:029450d064bb | 114 | int sonar_count; |
WeberYang | 3:029450d064bb | 115 | |
WeberYang | 3:029450d064bb | 116 | |
WeberYang | 3:029450d064bb | 117 | //===================================================== |
onehorse | 1:cea9d83b8636 | 118 | int main() |
onehorse | 1:cea9d83b8636 | 119 | { |
WeberYang | 3:029450d064bb | 120 | //Set up I2C |
WeberYang | 3:029450d064bb | 121 | myled = !myled; |
WeberYang | 3:029450d064bb | 122 | wait_ms(100); |
WeberYang | 3:029450d064bb | 123 | nh.initNode(); |
WeberYang | 3:029450d064bb | 124 | nh.advertise(imu_pub); |
onehorse | 1:cea9d83b8636 | 125 | |
WeberYang | 3:029450d064bb | 126 | nh.advertise(DI_pub); |
WeberYang | 3:029450d064bb | 127 | nh.advertise(Enc_pub); |
WeberYang | 3:029450d064bb | 128 | nh.advertise(pub_sonar); |
WeberYang | 3:029450d064bb | 129 | |
WeberYang | 3:029450d064bb | 130 | nh.subscribe(ACT_sub); |
WeberYang | 3:029450d064bb | 131 | nh.subscribe(resetEncoder_sub); |
WeberYang | 3:029450d064bb | 132 | dis1.startUpdates();//start mesuring the distance |
onehorse | 1:cea9d83b8636 | 133 | |
WeberYang | 3:029450d064bb | 134 | i2c.frequency(400000); // use fast (400 kHz) I2C |
WeberYang | 3:029450d064bb | 135 | IMU_flipper.attach(&dmpDataUpdate, 0.06); |
WeberYang | 3:029450d064bb | 136 | t.start(); |
WeberYang | 3:029450d064bb | 137 | |
WeberYang | 3:029450d064bb | 138 | // Read the WHO_AM_I register, this is a good test of communication |
WeberYang | 3:029450d064bb | 139 | uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 |
WeberYang | 3:029450d064bb | 140 | // pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); |
WeberYang | 3:029450d064bb | 141 | |
WeberYang | 3:029450d064bb | 142 | if (whoami == 0x68) // WHO_AM_I should always be 0x68 |
WeberYang | 3:029450d064bb | 143 | { |
WeberYang | 3:029450d064bb | 144 | // pc.printf("MPU6050 is online..."); |
WeberYang | 3:029450d064bb | 145 | wait(1); |
WeberYang | 3:029450d064bb | 146 | |
WeberYang | 3:029450d064bb | 147 | mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values |
WeberYang | 3:029450d064bb | 148 | wait(1); |
onehorse | 0:65aa78c10981 | 149 | |
WeberYang | 3:029450d064bb | 150 | 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) |
WeberYang | 3:029450d064bb | 151 | { |
WeberYang | 3:029450d064bb | 152 | mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration |
WeberYang | 3:029450d064bb | 153 | mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
WeberYang | 3:029450d064bb | 154 | mpu6050.initMPU6050(); |
WeberYang | 3:029450d064bb | 155 | wait(2); |
WeberYang | 3:029450d064bb | 156 | } |
WeberYang | 3:029450d064bb | 157 | else |
WeberYang | 3:029450d064bb | 158 | { |
WeberYang | 3:029450d064bb | 159 | // pc.printf("Device did not the pass self-test!\n\r"); |
WeberYang | 3:029450d064bb | 160 | } |
onehorse | 1:cea9d83b8636 | 161 | } |
onehorse | 1:cea9d83b8636 | 162 | else |
onehorse | 1:cea9d83b8636 | 163 | { |
WeberYang | 3:029450d064bb | 164 | // pc.printf("Could not connect to MPU6050: \n\r"); |
WeberYang | 3:029450d064bb | 165 | // pc.printf("%#x \n", whoami); |
WeberYang | 3:029450d064bb | 166 | while(1) ; // Loop forever if communication doesn't happen |
WeberYang | 3:029450d064bb | 167 | } |
onehorse | 0:65aa78c10981 | 168 | |
WeberYang | 3:029450d064bb | 169 | while(1) |
WeberYang | 3:029450d064bb | 170 | { |
WeberYang | 3:029450d064bb | 171 | dis1.checkDistance(); |
onehorse | 0:65aa78c10981 | 172 | |
WeberYang | 3:029450d064bb | 173 | DI.data = DI_read(PCF8574_ADDR_2); |
WeberYang | 3:029450d064bb | 174 | DI_pub.publish( &DI ); |
onehorse | 0:65aa78c10981 | 175 | |
WeberYang | 3:029450d064bb | 176 | nh.spinOnce(); |
WeberYang | 3:029450d064bb | 177 | wait_ms(100); |
WeberYang | 3:029450d064bb | 178 | |
onehorse | 0:65aa78c10981 | 179 | } |
WeberYang | 3:029450d064bb | 180 | } |
onehorse | 1:cea9d83b8636 | 181 | |
WeberYang | 3:029450d064bb | 182 | void dmpDataUpdate() { |
WeberYang | 3:029450d064bb | 183 | // If data ready bit set, all data registers have new data |
WeberYang | 3:029450d064bb | 184 | if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) |
WeberYang | 3:029450d064bb | 185 | { // check if data ready interrupt |
WeberYang | 3:029450d064bb | 186 | mpu6050.readAccelData(accelCount); // Read the x/y/z adc values |
WeberYang | 3:029450d064bb | 187 | mpu6050.getAres(); |
onehorse | 0:65aa78c10981 | 188 | |
WeberYang | 3:029450d064bb | 189 | // Now we'll calculate the accleration value into actual g's |
WeberYang | 3:029450d064bb | 190 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
WeberYang | 3:029450d064bb | 191 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
WeberYang | 3:029450d064bb | 192 | az = (float)accelCount[2]*aRes - accelBias[2]; |
WeberYang | 3:029450d064bb | 193 | |
WeberYang | 3:029450d064bb | 194 | mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values |
WeberYang | 3:029450d064bb | 195 | mpu6050.getGres(); |
WeberYang | 3:029450d064bb | 196 | |
WeberYang | 3:029450d064bb | 197 | // Calculate the gyro value into actual degrees per second |
WeberYang | 3:029450d064bb | 198 | gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set |
WeberYang | 3:029450d064bb | 199 | gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; |
WeberYang | 3:029450d064bb | 200 | gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; |
onehorse | 0:65aa78c10981 | 201 | |
WeberYang | 3:029450d064bb | 202 | tempCount = mpu6050.readTempData(); // Read the x/y/z adc values |
WeberYang | 3:029450d064bb | 203 | temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade |
WeberYang | 3:029450d064bb | 204 | } |
WeberYang | 3:029450d064bb | 205 | Now = t.read_us(); |
WeberYang | 3:029450d064bb | 206 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
WeberYang | 3:029450d064bb | 207 | lastUpdate = Now; |
WeberYang | 3:029450d064bb | 208 | |
WeberYang | 3:029450d064bb | 209 | sum += deltat; |
WeberYang | 3:029450d064bb | 210 | sumCount++; |
WeberYang | 3:029450d064bb | 211 | |
WeberYang | 3:029450d064bb | 212 | if(lastUpdate - firstUpdate > 10000000.0f) |
WeberYang | 3:029450d064bb | 213 | { |
WeberYang | 3:029450d064bb | 214 | beta = 0.04; // decrease filter gain after stabilized |
WeberYang | 3:029450d064bb | 215 | zeta = 0.015; // increasey bias drift gain after stabilized |
WeberYang | 3:029450d064bb | 216 | } |
WeberYang | 3:029450d064bb | 217 | |
WeberYang | 3:029450d064bb | 218 | // Pass gyro rate as rad/s |
WeberYang | 3:029450d064bb | 219 | mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); |
WeberYang | 3:029450d064bb | 220 | |
WeberYang | 3:029450d064bb | 221 | // Serial print and/or display at 0.5 s rate independent of data rates |
WeberYang | 3:029450d064bb | 222 | delt_t = t.read_ms() - count; |
WeberYang | 3:029450d064bb | 223 | // update LCD once per half-second independent of read rate |
WeberYang | 3:029450d064bb | 224 | 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]); |
WeberYang | 3:029450d064bb | 225 | pitch = -asin(2.1f * (q[1] * q[3] - q[0] * q[2])); |
WeberYang | 3:029450d064bb | 226 | 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]); |
WeberYang | 3:029450d064bb | 227 | pitch *= 180.0f / PI; |
WeberYang | 3:029450d064bb | 228 | yaw *= 180.0f / PI; |
WeberYang | 3:029450d064bb | 229 | roll *= 180.0f / PI; |
onehorse | 0:65aa78c10981 | 230 | |
WeberYang | 3:029450d064bb | 231 | imu_msg.header.frame_id = frameid; |
WeberYang | 3:029450d064bb | 232 | imu_msg.header.stamp = nh.now(); |
WeberYang | 3:029450d064bb | 233 | imu_msg.linear_acceleration.x = ax*9.81;//dmpData.acc.x; |
WeberYang | 3:029450d064bb | 234 | imu_msg.linear_acceleration.y = ay*9.81;//dmpData.acc.y; |
WeberYang | 3:029450d064bb | 235 | imu_msg.linear_acceleration.z = az*9.81;//dmpData.acc.z; |
WeberYang | 3:029450d064bb | 236 | imu_msg.angular_velocity.x = gx/180*3.1415926;//dmpData.gyro.x; |
WeberYang | 3:029450d064bb | 237 | imu_msg.angular_velocity.y = gy/180*3.1415926;//dmpData.gyro.y; |
WeberYang | 3:029450d064bb | 238 | imu_msg.angular_velocity.z = gz/180*3.1415926;//dmpData.gyro.z; |
WeberYang | 3:029450d064bb | 239 | imu_msg.orientation.w = q[0]; |
WeberYang | 3:029450d064bb | 240 | imu_msg.orientation.x = q[1]; |
WeberYang | 3:029450d064bb | 241 | imu_msg.orientation.y = q[2]; |
WeberYang | 3:029450d064bb | 242 | imu_msg.orientation.z = q[3]; |
WeberYang | 3:029450d064bb | 243 | imu_msg.orientation_covariance[8] = 0; |
WeberYang | 3:029450d064bb | 244 | imu_msg.angular_velocity_covariance[8] = 0; |
WeberYang | 3:029450d064bb | 245 | imu_msg.linear_acceleration_covariance[8] = 0; |
WeberYang | 3:029450d064bb | 246 | imu_pub.publish( &imu_msg ); |
WeberYang | 3:029450d064bb | 247 | wait_ms(30);//must bigger then 30ms at 115200 |
WeberYang | 3:029450d064bb | 248 | |
WeberYang | 3:029450d064bb | 249 | encoder.x = encoder1.getPulses(); |
WeberYang | 3:029450d064bb | 250 | encoder.y = encoder2.getPulses(); |
WeberYang | 3:029450d064bb | 251 | Enc_pub.publish(&encoder); |
WeberYang | 3:029450d064bb | 252 | wait_ms(1); |
WeberYang | 3:029450d064bb | 253 | |
onehorse | 0:65aa78c10981 | 254 | |
WeberYang | 3:029450d064bb | 255 | |
WeberYang | 3:029450d064bb | 256 | myled= !myled; |
WeberYang | 3:029450d064bb | 257 | count = t.read_ms(); |
WeberYang | 3:029450d064bb | 258 | sum = 0; |
WeberYang | 3:029450d064bb | 259 | sumCount = 0; |
onehorse | 0:65aa78c10981 | 260 | } |
WeberYang | 3:029450d064bb | 261 |