Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
IMU/DMP.cpp
- Committer:
- tntmarket
- Date:
- 2015-03-25
- Revision:
- 11:425dff6a4af9
- Parent:
- 6:b3baf0fe5b73
File content as of revision 11:425dff6a4af9:
//Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly) /*** THIS IS THE BEST VERSION OF DMP CODE This code demonstrates how to access the MPU6050 IMU and fix the connection errors that occur, using a hardware hack. It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected to the SDA line. Error 1: The SDA line gets stuck on ground. Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71) Error 2: The SDA line gets stuck on 3.3V Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96) (I am currently trying to find a software solution for the above hack) This code works for using the DMP. (See "DMP" for implementation that can do raw accel data) It appears all connection issues have been fixed. Next step: make code neater (in progress) LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5) ***/ #ifndef M_PI #define M_PI 3.14159265358979323846 #endif #include "mbed.h" #include "DMP.h" #include "robot.h" //******* MPU DECLARATION THINGS *****************// int dmp_test = 1; //if this is 0, get raw MPU values //if this is 1, get DMP values int acc_offset[3]; //this saves the offset for acceleration int16_t ax, ay, az; int16_t gx, gy, gz; //******* DMP things ****************// // MPU control/status vars bool dmpReady = false; // set true if DMP init was succesfful uint8_t mpuIntStatus; // holds interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer bool dmpReady2 = false; // set true if DMP init was succesfful uint8_t mpuIntStatus2; // holds interrupt status byte from MPU uint8_t devStatus2; // return status after each device operation (0 = success) uint16_t packetSize2; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount2; // count of all bytes currently in FIFO uint8_t fifoBuffer2[64]; // FIFO storage buffer uint8_t MPU_CONNECTION; // orientation/motion vars Quaternion q; // [w,x,y,z] quaternion container Quaternion q2; // [w,x,y,z] quaternion container VectorInt16 aa; // [x,y,z] accel sensor measurements VectorInt16 aaReal; // [x,y,z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x,y,z] world-frame accel sensor measurements VectorFloat gravity; // [x,y,z] gravity vector VectorFloat gravity2; // [x,y,z] gravity vector float euler[3]; // [psi,theta,phi] Euler angle container float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg] float ypr2[3]; InterruptIn checkpin(PTD7); //interrupt InterruptIn checkpin2(PTD5); //PTD5 //*** Interrupt Detection Routine ***// volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high void dmpDataReady(){ mpuInterrupt = true; } volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high void dmpDataReady2(){ mpuInterrupt2 = true; } //****** END MPU DECLARATION ********************// int test_dmp(); //this wraps up the code req'd to start the DMP void start_dmp(MPU6050); //this will get the DMP ready void update_dmp(); //call this frequently //*********************FUNCTIONS***************************************************************// int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem imuSwitch = 1; LED_OFF; Thread::wait(100); while(mpu.testConnection()==0) { imuSwitch = 0; Thread::wait(100); imuSwitch = 1; Thread::wait(100);// // bt.lock(); // bt.printf("Stuck on IMU1"); // bt.unlock(); } imuSwitch = 1; LED_ON //turn ON LED return 1; } int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem imu2Switch = 1; LED_OFF; Thread::wait(100); while(mpu2.testConnection()==0) { imu2Switch = 0; Thread::wait(100); imu2Switch = 1; Thread::wait(100); //bt.lock(); //bt.printf("Stuck on IMU2"); //bt.unlock(); } imu2Switch = 1; LED_ON //turn ON LED return 1; } void start_dmp(MPU6050 mpu1){ //this will get the DMP ready //initialize device mpu1.reset(); DMP_DBG_MSG("\n\rInitializing I2C devices...") devStatus = mpu1.dmpInitialize(); //insert gyro offsets here// Write a calibration algorithm in the future mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available mpu1.setYGyroOffset(0); mpu1.setZGyroOffset(0); mpu1.setXAccelOffset(0); //the accel offsets don't do anything mpu1.setYAccelOffset(0); mpu1.setZAccelOffset(0); mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); if(devStatus == 0){ //this means initialize was successful //turn on DMP DMP_DBG_MSG("\n\rEnabling DMP") mpu1.setDMPEnabled(true); // enable interrupt detection DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n") checkpin.rise(&dmpDataReady); mpuIntStatus = mpu1.getIntStatus(); //set the DMP ready flag so main loop knows when it is ok to use DMP_DBG_MSG("DMP ready! Waiting for first interrupt") dmpReady = true; //get expected DMP packet size packetSize = mpu1.dmpGetFIFOPacketSize(); }else{ DMP_DBG_MSG("\n\rDMP Initialization failed") DMP_DBG_MSG("\t%d",devStatus) Thread::wait(1000); } acc_offset[0]=0; acc_offset[1]=0; acc_offset[2]=0; mpu.setDLPFMode(MPU6050_DLPF_BW_42); } void start_dmp2(MPU6051 mpu1){ //this will get the DMP ready //initialize device mpu2.reset(); DMP_DBG_MSG("\n\rInitializing I2C devices...") devStatus2 = mpu2.dmpInitialize(); //insert gyro offsets here// Write a calibration algorithm in the future mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available mpu2.setYGyroOffset(0); mpu2.setZGyroOffset(0); mpu2.setXAccelOffset(0); //the accel offsets don't do anything mpu2.setYAccelOffset(0); mpu2.setZAccelOffset(0); mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); if(devStatus2 == 0){ //this means initialize was successful //turn on DMP DMP_DBG_MSG("\n\rEnabling DMP") mpu2.setDMPEnabled(true); // enable interrupt detection DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n") checkpin2.rise(&dmpDataReady2); mpuIntStatus2 = mpu2.getIntStatus(); //set the DMP ready flag so main loop knows when it is ok to use DMP_DBG_MSG("DMP ready! Waiting for first interrupt") dmpReady2 = true; //get expected DMP packet size packetSize2 = mpu2.dmpGetFIFOPacketSize(); }else{ DMP_DBG_MSG("\n\rDMP Initialization failed") DMP_DBG_MSG("\t%d",devStatus2) Thread::wait(1000); } acc_offset[0]=0; acc_offset[1]=0; acc_offset[2]=0; mpu2.setDLPFMode(MPU6050_DLPF_BW_42); } void update_dmp(){ if (!dmpReady) return; while (!mpuInterrupt && fifoCount < packetSize) { // other program behavior stuff here // if you are really paranoid you can frequently test in between other // stuff to see if mpuInterrupt is true, and if so, "break;" from the // while() loop to immediately process the MPU data // . } // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; //this resets the interrupt flag mpuIntStatus = mpu.getIntStatus(); //get current FIFO count; fifoCount = mpu.getFIFOCount(); //check for overflow (only happens if your code sucks) if((mpuIntStatus & 0x10) || fifoCount == 1024){ //reset so we can continue cleanly mpu.resetFIFO(); DMP_DBG_MSG("\n\rFIFO overflow") } else if (mpuIntStatus & 0x02){ int16_t x,y,z; //wait for correct available data length (should be very short) while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); //read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); //track FIFO count here in the case there is > 1 packet available //(allows us to immediately read more w/o waiting for interrupt) fifoCount -= packetSize; mpu.dmpGetQuaternion(&q,fifoBuffer); imu_data.quat[0]=q.w; imu_data.quat[1]=q.x; imu_data.quat[2]=q.y; imu_data.quat[3]=q.z; mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr,&q,&gravity); imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it imu_data.ypr[1] = ypr[1]*180/M_PI; imu_data.ypr[2] = ypr[2]*180/M_PI; mpu.getAcceleration(&x,&y,&z); imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384; imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384; imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384; #ifdef PRINT_GYR DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI) #endif mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); #ifdef PRINT_ACC DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z) #endif //get rotations mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz); float scale = 250/16384; imu_data.gyr[0] = gx*scale; imu_data.gyr[1] = gy*scale; imu_data.gyr[2] = gz*scale; } } void update_dmp2(){ if (!dmpReady2) return; while (!mpuInterrupt2 && fifoCount2 < packetSize2) { // other program behavior stuff here // if you are really paranoid you can frequently test in between other // stuff to see if mpuInterrupt is true, and if so, "break;" from the // while() loop to immediately process the MPU data // . } // reset interrupt flag and get INT_STATUS byte mpuInterrupt2 = false; //this resets the interrupt flag mpuIntStatus2 = mpu2.getIntStatus(); //get current FIFO count; fifoCount2 = mpu2.getFIFOCount(); //check for overflow (only happens if your code sucks) if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){ //reset so we can continue cleanly mpu2.resetFIFO(); DMP_DBG_MSG("\n\rFIFO overflow") } else if (mpuIntStatus2 & 0x02){ int16_t x,y,z; //wait for correct available data length (should be very short) while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount(); //read a packet from FIFO mpu2.getFIFOBytes(fifoBuffer2, packetSize2); //track FIFO count here in the case there is > 1 packet available //(allows us to immediately read more w/o waiting for interrupt) fifoCount2 -= packetSize2; mpu2.dmpGetQuaternion(&q2,fifoBuffer2); imu2_data.quat[0]=q2.w; imu2_data.quat[1]=q2.x; imu2_data.quat[2]=q2.y; imu2_data.quat[3]=q2.z; mpu2.dmpGetGravity(&gravity2, &q2); mpu2.dmpGetYawPitchRoll(ypr2,&q2,&gravity2); imu2_data.ypr[0] = ypr2[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it imu2_data.ypr[1] = ypr2[1]*180/M_PI; imu2_data.ypr[2] = ypr2[2]*180/M_PI; mpu2.getAcceleration(&x,&y,&z); imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384; imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384; imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384; #ifdef PRINT_GYR DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr2[0]*180/PI,ypr2[1]*180/PI,ypr2[2]*180/PI) #endif mpu2.dmpGetAccel(&aa, fifoBuffer2); mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity2); #ifdef PRINT_ACC DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z) #endif //get rotations mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz); float scale = 250/16384; imu2_data.gyr[0] = gx*scale; imu2_data.gyr[1] = gy*scale; imu2_data.gyr[2] = gz*scale; } } void update_acc() { int16_t x,y,z; mpu.getAcceleration(&x,&y,&z); imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384; imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384; imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384; } void calibrate_1() //call this to calibrate the accelerometer { //in the future, modify the OFFSET REGISTERS to make the DMP converge faster //right now, I am only calculating the offset for the accelerometers int sum[3] = {0,0,0}; int i; int16_t x,y,z; int count=0; int t_now = t.read_ms(); while((t.read_ms()-t_now) < 1000) //loop for one second { mpu.getAcceleration(&x,&y,&z); sum[0] += x; sum[1] += y; sum[2] += z; Thread::wait(5); count++; } for(i = 0; i<3; i++) acc_offset[i] = (sum[i]/count); ////bt.printf("ACC_OFF:%d;%d;%d;",acc_offset[0],acc_offset[1],acc_offset[2]);//print the offset used by accelerometer }