![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For nucleoF103 change sda,scl to PB_7,PB_6 Finish Delta Servo control with RS485, and IMU6050
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of MPU6050_DMP_test_for1549 by
main.cpp
- Committer:
- hardtail
- Date:
- 2017-10-25
- Revision:
- 0:6e61e8ec4b42
- Child:
- 2:648583d6e41a
File content as of revision 0:6e61e8ec4b42:
/* Demo code for MPU6050 DMP * I thank Ian Hua. * Copyright (c) 2015 Match * * THE PROGRAM IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE PROGRAM OR THE USE OR OTHER DEALINGS IN * THE PROGRAM. */ // Define Necessary. //#define OUTPUT_QUATERNION //#define OUTPUT_EULER #define OUTPUT_ROLL_PITCH_YAW //#define OUTPUT_FOR_TEAPOT //#define OUTPUT_TEMPERATURE #include "MPU6050_6Axis_MotionApps20.h" #include "mbed.h" #include <stdio.h> #define servotest #ifdef servotest PwmOut sv(P0_0); #endif // FIFO rate = 200Hz / (1 + this value) // For example, 0x01 is 100Hz, 0x03 is 50Hz. // 0x00 to 0x09 #define IMU_FIFO_RATE_DIVIDER 0x09 // Sample rate = 1kHz / (1 + this valye) // For example, 4 is 200Hz. #define IMU_SAMPLE_RATE_DIVIDER 4 // measuring range of gyroscope (±n deg/s) // But other value doesn't yet support. #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 // measuring range of acceleration sensor (±n g) // But other value doesn't yet support. #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 #define PC_BAUDRATE 921600 #include "USBSerial.h" #define DEG_TO_RAD(x) ( x * 0.01745329 ) #define RAD_TO_DEG(x) ( x * 57.29578 ) //RawSerial pc(USBTX, USBRX); USBSerial pc; MPU6050 mpu; // sda, scl pin InterruptIn INT0(P0_9); // INT0 pin const int FIFO_BUFFER_SIZE = 128; uint8_t fifoBuffer[FIFO_BUFFER_SIZE]; uint16_t fifoCount; uint16_t packetSize; bool dmpReady; uint8_t mpuIntStatus; const int snprintf_buffer_size = 100; char snprintf_buffer[snprintf_buffer_size]; uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; struct Offset { int16_t ax, ay, az; int16_t gx, gy, gz; }offset = {150, -350, 1000, -110, 5, 0}; // Measured values struct MPU6050_DmpData { Quaternion q; VectorFloat gravity; // g float roll, pitch, yaw; // rad }dmpData; long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } bool Init(); void dmpDataUpdate(); int main() { MBED_ASSERT(Init() == true); while(1) { } } bool Init() { //pc.baud(PC_BAUDRATE); while(!pc.readable()); pc.getc(); INT0.mode(PullDown); INT0.fall(dmpDataUpdate); mpu.initialize(); if (mpu.testConnection()) { pc.puts("MPU6050 test connection passed.\n"); } else { pc.puts("MPU6050 test connection failed.\n"); return false; } if (mpu.dmpInitialize() == 0) { pc.puts("succeed in MPU6050 DMP Initializing.\n"); } else { pc.puts("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. pc.puts("Init finish!\n"); #ifdef servotest sv.pulsewidth_us(1450); #endif return true; } void dmpDataUpdate() { // Check that this interrupt has enabled. if (dmpReady == false) return; mpuIntStatus = mpu.getIntStatus(); fifoCount = mpu.getFIFOCount(); // Check that this interrupt is a FIFO buffer overflow interrupt. if ((mpuIntStatus & 0x10) || fifoCount == 1024) { mpu.resetFIFO(); //pc.puts("FIFO overflow!\n"); return; // Check that this interrupt is a Data Ready interrupt. } else if (mpuIntStatus & 0x02) { while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); mpu.getFIFOBytes(fifoBuffer, packetSize); #ifdef OUTPUT_QUATERNION mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer); if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Quaternion : w=%f, x=%f, y=%f, z=%f\n", dmpData.q.w, dmpData.q.x, dmpData.q.y, dmpData.q.z ) < 0 ) return; pc.puts(snprintf_buffer); #endif #ifdef OUTPUT_EULER float euler[3]; mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer); mpu.dmpGetEuler(euler, &dmpData.q); if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Euler : psi=%fdeg, theta=%fdeg, phi=%fdeg\n", RAD_TO_DEG(euler[0]), RAD_TO_DEG(euler[1]), RAD_TO_DEG(euler[2]) ) < 0 ) return; pc.puts(snprintf_buffer); #endif #ifdef OUTPUT_ROLL_PITCH_YAW mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer); mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q); float rollPitchYaw[3]; mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity); dmpData.roll = rollPitchYaw[2]; dmpData.pitch = rollPitchYaw[1]; dmpData.yaw = rollPitchYaw[0]; if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(dmpData.roll), RAD_TO_DEG(dmpData.pitch), RAD_TO_DEG(dmpData.yaw) ) < 0 ) return; pc.puts(snprintf_buffer); #ifdef servotest int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450); if(servoPulse > 1450) servoPulse = 1450; if(servoPulse < 500) servoPulse = 500; sv.pulsewidth_us(servoPulse); #endif #endif #ifdef OUTPUT_FOR_TEAPOT teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; for (uint8_t i = 0; i < 14; i++) { pc.putc(teapotPacket[i]); } #endif #ifdef OUTPUT_TEMPERATURE float temp = mpu.getTemperature() / 340.0 + 36.53; if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return; pc.puts(snprintf_buffer); #endif pc.puts("\n"); } }