![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Delta Battery, CAN_bus, DIO P2P, ROS
Dependencies: mbed mbed-rtos ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:6e61e8ec4b42
- Child:
- 2:648583d6e41a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 25 08:09:12 2017 +0000 @@ -0,0 +1,219 @@ +/* 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"); + } +} \ No newline at end of file