![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Delta Battery, CAN_bus, DIO P2P, ROS
Dependencies: mbed mbed-rtos ros_lib_kinetic
main.cpp@0:6e61e8ec4b42, 2017-10-25 (annotated)
- Committer:
- hardtail
- Date:
- Wed Oct 25 08:09:12 2017 +0000
- Revision:
- 0:6e61e8ec4b42
- Child:
- 2:648583d6e41a
first commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hardtail | 0:6e61e8ec4b42 | 1 | /* Demo code for MPU6050 DMP |
hardtail | 0:6e61e8ec4b42 | 2 | * I thank Ian Hua. |
hardtail | 0:6e61e8ec4b42 | 3 | * Copyright (c) 2015 Match |
hardtail | 0:6e61e8ec4b42 | 4 | * |
hardtail | 0:6e61e8ec4b42 | 5 | * THE PROGRAM IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
hardtail | 0:6e61e8ec4b42 | 6 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
hardtail | 0:6e61e8ec4b42 | 7 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
hardtail | 0:6e61e8ec4b42 | 8 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
hardtail | 0:6e61e8ec4b42 | 9 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
hardtail | 0:6e61e8ec4b42 | 10 | * OUT OF OR IN CONNECTION WITH THE PROGRAM OR THE USE OR OTHER DEALINGS IN |
hardtail | 0:6e61e8ec4b42 | 11 | * THE PROGRAM. |
hardtail | 0:6e61e8ec4b42 | 12 | */ |
hardtail | 0:6e61e8ec4b42 | 13 | |
hardtail | 0:6e61e8ec4b42 | 14 | |
hardtail | 0:6e61e8ec4b42 | 15 | // Define Necessary. |
hardtail | 0:6e61e8ec4b42 | 16 | //#define OUTPUT_QUATERNION |
hardtail | 0:6e61e8ec4b42 | 17 | //#define OUTPUT_EULER |
hardtail | 0:6e61e8ec4b42 | 18 | #define OUTPUT_ROLL_PITCH_YAW |
hardtail | 0:6e61e8ec4b42 | 19 | //#define OUTPUT_FOR_TEAPOT |
hardtail | 0:6e61e8ec4b42 | 20 | //#define OUTPUT_TEMPERATURE |
hardtail | 0:6e61e8ec4b42 | 21 | |
hardtail | 0:6e61e8ec4b42 | 22 | #include "MPU6050_6Axis_MotionApps20.h" |
hardtail | 0:6e61e8ec4b42 | 23 | #include "mbed.h" |
hardtail | 0:6e61e8ec4b42 | 24 | #include <stdio.h> |
hardtail | 0:6e61e8ec4b42 | 25 | |
hardtail | 0:6e61e8ec4b42 | 26 | #define servotest |
hardtail | 0:6e61e8ec4b42 | 27 | #ifdef servotest |
hardtail | 0:6e61e8ec4b42 | 28 | PwmOut sv(P0_0); |
hardtail | 0:6e61e8ec4b42 | 29 | #endif |
hardtail | 0:6e61e8ec4b42 | 30 | |
hardtail | 0:6e61e8ec4b42 | 31 | // FIFO rate = 200Hz / (1 + this value) |
hardtail | 0:6e61e8ec4b42 | 32 | // For example, 0x01 is 100Hz, 0x03 is 50Hz. |
hardtail | 0:6e61e8ec4b42 | 33 | // 0x00 to 0x09 |
hardtail | 0:6e61e8ec4b42 | 34 | #define IMU_FIFO_RATE_DIVIDER 0x09 |
hardtail | 0:6e61e8ec4b42 | 35 | |
hardtail | 0:6e61e8ec4b42 | 36 | // Sample rate = 1kHz / (1 + this valye) |
hardtail | 0:6e61e8ec4b42 | 37 | // For example, 4 is 200Hz. |
hardtail | 0:6e61e8ec4b42 | 38 | #define IMU_SAMPLE_RATE_DIVIDER 4 |
hardtail | 0:6e61e8ec4b42 | 39 | |
hardtail | 0:6e61e8ec4b42 | 40 | // measuring range of gyroscope (±n deg/s) |
hardtail | 0:6e61e8ec4b42 | 41 | // But other value doesn't yet support. |
hardtail | 0:6e61e8ec4b42 | 42 | #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 |
hardtail | 0:6e61e8ec4b42 | 43 | |
hardtail | 0:6e61e8ec4b42 | 44 | // measuring range of acceleration sensor (±n g) |
hardtail | 0:6e61e8ec4b42 | 45 | // But other value doesn't yet support. |
hardtail | 0:6e61e8ec4b42 | 46 | #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 |
hardtail | 0:6e61e8ec4b42 | 47 | |
hardtail | 0:6e61e8ec4b42 | 48 | #define PC_BAUDRATE 921600 |
hardtail | 0:6e61e8ec4b42 | 49 | |
hardtail | 0:6e61e8ec4b42 | 50 | |
hardtail | 0:6e61e8ec4b42 | 51 | #include "USBSerial.h" |
hardtail | 0:6e61e8ec4b42 | 52 | |
hardtail | 0:6e61e8ec4b42 | 53 | #define DEG_TO_RAD(x) ( x * 0.01745329 ) |
hardtail | 0:6e61e8ec4b42 | 54 | #define RAD_TO_DEG(x) ( x * 57.29578 ) |
hardtail | 0:6e61e8ec4b42 | 55 | |
hardtail | 0:6e61e8ec4b42 | 56 | |
hardtail | 0:6e61e8ec4b42 | 57 | //RawSerial pc(USBTX, USBRX); |
hardtail | 0:6e61e8ec4b42 | 58 | USBSerial pc; |
hardtail | 0:6e61e8ec4b42 | 59 | MPU6050 mpu; // sda, scl pin |
hardtail | 0:6e61e8ec4b42 | 60 | InterruptIn INT0(P0_9); // INT0 pin |
hardtail | 0:6e61e8ec4b42 | 61 | |
hardtail | 0:6e61e8ec4b42 | 62 | const int FIFO_BUFFER_SIZE = 128; |
hardtail | 0:6e61e8ec4b42 | 63 | uint8_t fifoBuffer[FIFO_BUFFER_SIZE]; |
hardtail | 0:6e61e8ec4b42 | 64 | uint16_t fifoCount; |
hardtail | 0:6e61e8ec4b42 | 65 | uint16_t packetSize; |
hardtail | 0:6e61e8ec4b42 | 66 | bool dmpReady; |
hardtail | 0:6e61e8ec4b42 | 67 | uint8_t mpuIntStatus; |
hardtail | 0:6e61e8ec4b42 | 68 | const int snprintf_buffer_size = 100; |
hardtail | 0:6e61e8ec4b42 | 69 | char snprintf_buffer[snprintf_buffer_size]; |
hardtail | 0:6e61e8ec4b42 | 70 | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; |
hardtail | 0:6e61e8ec4b42 | 71 | |
hardtail | 0:6e61e8ec4b42 | 72 | struct Offset { |
hardtail | 0:6e61e8ec4b42 | 73 | int16_t ax, ay, az; |
hardtail | 0:6e61e8ec4b42 | 74 | int16_t gx, gy, gz; |
hardtail | 0:6e61e8ec4b42 | 75 | }offset = {150, -350, 1000, -110, 5, 0}; // Measured values |
hardtail | 0:6e61e8ec4b42 | 76 | |
hardtail | 0:6e61e8ec4b42 | 77 | struct MPU6050_DmpData { |
hardtail | 0:6e61e8ec4b42 | 78 | Quaternion q; |
hardtail | 0:6e61e8ec4b42 | 79 | VectorFloat gravity; // g |
hardtail | 0:6e61e8ec4b42 | 80 | float roll, pitch, yaw; // rad |
hardtail | 0:6e61e8ec4b42 | 81 | }dmpData; |
hardtail | 0:6e61e8ec4b42 | 82 | |
hardtail | 0:6e61e8ec4b42 | 83 | long map(long x, long in_min, long in_max, long out_min, long out_max) { |
hardtail | 0:6e61e8ec4b42 | 84 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
hardtail | 0:6e61e8ec4b42 | 85 | } |
hardtail | 0:6e61e8ec4b42 | 86 | |
hardtail | 0:6e61e8ec4b42 | 87 | bool Init(); |
hardtail | 0:6e61e8ec4b42 | 88 | void dmpDataUpdate(); |
hardtail | 0:6e61e8ec4b42 | 89 | |
hardtail | 0:6e61e8ec4b42 | 90 | |
hardtail | 0:6e61e8ec4b42 | 91 | int main() { |
hardtail | 0:6e61e8ec4b42 | 92 | MBED_ASSERT(Init() == true); |
hardtail | 0:6e61e8ec4b42 | 93 | |
hardtail | 0:6e61e8ec4b42 | 94 | while(1) { |
hardtail | 0:6e61e8ec4b42 | 95 | |
hardtail | 0:6e61e8ec4b42 | 96 | } |
hardtail | 0:6e61e8ec4b42 | 97 | } |
hardtail | 0:6e61e8ec4b42 | 98 | |
hardtail | 0:6e61e8ec4b42 | 99 | |
hardtail | 0:6e61e8ec4b42 | 100 | bool Init() { |
hardtail | 0:6e61e8ec4b42 | 101 | //pc.baud(PC_BAUDRATE); |
hardtail | 0:6e61e8ec4b42 | 102 | |
hardtail | 0:6e61e8ec4b42 | 103 | while(!pc.readable()); |
hardtail | 0:6e61e8ec4b42 | 104 | pc.getc(); |
hardtail | 0:6e61e8ec4b42 | 105 | |
hardtail | 0:6e61e8ec4b42 | 106 | INT0.mode(PullDown); |
hardtail | 0:6e61e8ec4b42 | 107 | INT0.fall(dmpDataUpdate); |
hardtail | 0:6e61e8ec4b42 | 108 | |
hardtail | 0:6e61e8ec4b42 | 109 | mpu.initialize(); |
hardtail | 0:6e61e8ec4b42 | 110 | if (mpu.testConnection()) { |
hardtail | 0:6e61e8ec4b42 | 111 | pc.puts("MPU6050 test connection passed.\n"); |
hardtail | 0:6e61e8ec4b42 | 112 | } else { |
hardtail | 0:6e61e8ec4b42 | 113 | pc.puts("MPU6050 test connection failed.\n"); |
hardtail | 0:6e61e8ec4b42 | 114 | return false; |
hardtail | 0:6e61e8ec4b42 | 115 | } |
hardtail | 0:6e61e8ec4b42 | 116 | if (mpu.dmpInitialize() == 0) { |
hardtail | 0:6e61e8ec4b42 | 117 | pc.puts("succeed in MPU6050 DMP Initializing.\n"); |
hardtail | 0:6e61e8ec4b42 | 118 | } else { |
hardtail | 0:6e61e8ec4b42 | 119 | pc.puts("failed in MPU6050 DMP Initializing.\n"); |
hardtail | 0:6e61e8ec4b42 | 120 | return false; |
hardtail | 0:6e61e8ec4b42 | 121 | } |
hardtail | 0:6e61e8ec4b42 | 122 | mpu.setXAccelOffset(offset.ax); |
hardtail | 0:6e61e8ec4b42 | 123 | mpu.setYAccelOffset(offset.ay); |
hardtail | 0:6e61e8ec4b42 | 124 | mpu.setZAccelOffset(offset.az); |
hardtail | 0:6e61e8ec4b42 | 125 | mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); |
hardtail | 0:6e61e8ec4b42 | 126 | mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); |
hardtail | 0:6e61e8ec4b42 | 127 | mpu.setXGyroOffsetUser(offset.gx); |
hardtail | 0:6e61e8ec4b42 | 128 | mpu.setYGyroOffsetUser(offset.gy); |
hardtail | 0:6e61e8ec4b42 | 129 | mpu.setZGyroOffsetUser(offset.gz); |
hardtail | 0:6e61e8ec4b42 | 130 | mpu.setDMPEnabled(true); // Enable DMP |
hardtail | 0:6e61e8ec4b42 | 131 | packetSize = mpu.dmpGetFIFOPacketSize(); |
hardtail | 0:6e61e8ec4b42 | 132 | dmpReady = true; // Enable interrupt. |
hardtail | 0:6e61e8ec4b42 | 133 | |
hardtail | 0:6e61e8ec4b42 | 134 | pc.puts("Init finish!\n"); |
hardtail | 0:6e61e8ec4b42 | 135 | |
hardtail | 0:6e61e8ec4b42 | 136 | #ifdef servotest |
hardtail | 0:6e61e8ec4b42 | 137 | sv.pulsewidth_us(1450); |
hardtail | 0:6e61e8ec4b42 | 138 | #endif |
hardtail | 0:6e61e8ec4b42 | 139 | |
hardtail | 0:6e61e8ec4b42 | 140 | return true; |
hardtail | 0:6e61e8ec4b42 | 141 | } |
hardtail | 0:6e61e8ec4b42 | 142 | |
hardtail | 0:6e61e8ec4b42 | 143 | |
hardtail | 0:6e61e8ec4b42 | 144 | void dmpDataUpdate() { |
hardtail | 0:6e61e8ec4b42 | 145 | // Check that this interrupt has enabled. |
hardtail | 0:6e61e8ec4b42 | 146 | if (dmpReady == false) return; |
hardtail | 0:6e61e8ec4b42 | 147 | |
hardtail | 0:6e61e8ec4b42 | 148 | mpuIntStatus = mpu.getIntStatus(); |
hardtail | 0:6e61e8ec4b42 | 149 | fifoCount = mpu.getFIFOCount(); |
hardtail | 0:6e61e8ec4b42 | 150 | |
hardtail | 0:6e61e8ec4b42 | 151 | // Check that this interrupt is a FIFO buffer overflow interrupt. |
hardtail | 0:6e61e8ec4b42 | 152 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { |
hardtail | 0:6e61e8ec4b42 | 153 | mpu.resetFIFO(); |
hardtail | 0:6e61e8ec4b42 | 154 | //pc.puts("FIFO overflow!\n"); |
hardtail | 0:6e61e8ec4b42 | 155 | return; |
hardtail | 0:6e61e8ec4b42 | 156 | |
hardtail | 0:6e61e8ec4b42 | 157 | // Check that this interrupt is a Data Ready interrupt. |
hardtail | 0:6e61e8ec4b42 | 158 | } else if (mpuIntStatus & 0x02) { |
hardtail | 0:6e61e8ec4b42 | 159 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); |
hardtail | 0:6e61e8ec4b42 | 160 | |
hardtail | 0:6e61e8ec4b42 | 161 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
hardtail | 0:6e61e8ec4b42 | 162 | |
hardtail | 0:6e61e8ec4b42 | 163 | #ifdef OUTPUT_QUATERNION |
hardtail | 0:6e61e8ec4b42 | 164 | mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer); |
hardtail | 0:6e61e8ec4b42 | 165 | 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; |
hardtail | 0:6e61e8ec4b42 | 166 | pc.puts(snprintf_buffer); |
hardtail | 0:6e61e8ec4b42 | 167 | #endif |
hardtail | 0:6e61e8ec4b42 | 168 | |
hardtail | 0:6e61e8ec4b42 | 169 | #ifdef OUTPUT_EULER |
hardtail | 0:6e61e8ec4b42 | 170 | float euler[3]; |
hardtail | 0:6e61e8ec4b42 | 171 | mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer); |
hardtail | 0:6e61e8ec4b42 | 172 | mpu.dmpGetEuler(euler, &dmpData.q); |
hardtail | 0:6e61e8ec4b42 | 173 | 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; |
hardtail | 0:6e61e8ec4b42 | 174 | pc.puts(snprintf_buffer); |
hardtail | 0:6e61e8ec4b42 | 175 | #endif |
hardtail | 0:6e61e8ec4b42 | 176 | |
hardtail | 0:6e61e8ec4b42 | 177 | #ifdef OUTPUT_ROLL_PITCH_YAW |
hardtail | 0:6e61e8ec4b42 | 178 | mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer); |
hardtail | 0:6e61e8ec4b42 | 179 | mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q); |
hardtail | 0:6e61e8ec4b42 | 180 | float rollPitchYaw[3]; |
hardtail | 0:6e61e8ec4b42 | 181 | mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity); |
hardtail | 0:6e61e8ec4b42 | 182 | dmpData.roll = rollPitchYaw[2]; |
hardtail | 0:6e61e8ec4b42 | 183 | dmpData.pitch = rollPitchYaw[1]; |
hardtail | 0:6e61e8ec4b42 | 184 | dmpData.yaw = rollPitchYaw[0]; |
hardtail | 0:6e61e8ec4b42 | 185 | |
hardtail | 0:6e61e8ec4b42 | 186 | 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; |
hardtail | 0:6e61e8ec4b42 | 187 | pc.puts(snprintf_buffer); |
hardtail | 0:6e61e8ec4b42 | 188 | |
hardtail | 0:6e61e8ec4b42 | 189 | #ifdef servotest |
hardtail | 0:6e61e8ec4b42 | 190 | int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450); |
hardtail | 0:6e61e8ec4b42 | 191 | if(servoPulse > 1450) servoPulse = 1450; |
hardtail | 0:6e61e8ec4b42 | 192 | if(servoPulse < 500) servoPulse = 500; |
hardtail | 0:6e61e8ec4b42 | 193 | sv.pulsewidth_us(servoPulse); |
hardtail | 0:6e61e8ec4b42 | 194 | #endif |
hardtail | 0:6e61e8ec4b42 | 195 | #endif |
hardtail | 0:6e61e8ec4b42 | 196 | |
hardtail | 0:6e61e8ec4b42 | 197 | #ifdef OUTPUT_FOR_TEAPOT |
hardtail | 0:6e61e8ec4b42 | 198 | teapotPacket[2] = fifoBuffer[0]; |
hardtail | 0:6e61e8ec4b42 | 199 | teapotPacket[3] = fifoBuffer[1]; |
hardtail | 0:6e61e8ec4b42 | 200 | teapotPacket[4] = fifoBuffer[4]; |
hardtail | 0:6e61e8ec4b42 | 201 | teapotPacket[5] = fifoBuffer[5]; |
hardtail | 0:6e61e8ec4b42 | 202 | teapotPacket[6] = fifoBuffer[8]; |
hardtail | 0:6e61e8ec4b42 | 203 | teapotPacket[7] = fifoBuffer[9]; |
hardtail | 0:6e61e8ec4b42 | 204 | teapotPacket[8] = fifoBuffer[12]; |
hardtail | 0:6e61e8ec4b42 | 205 | teapotPacket[9] = fifoBuffer[13]; |
hardtail | 0:6e61e8ec4b42 | 206 | for (uint8_t i = 0; i < 14; i++) { |
hardtail | 0:6e61e8ec4b42 | 207 | pc.putc(teapotPacket[i]); |
hardtail | 0:6e61e8ec4b42 | 208 | } |
hardtail | 0:6e61e8ec4b42 | 209 | #endif |
hardtail | 0:6e61e8ec4b42 | 210 | |
hardtail | 0:6e61e8ec4b42 | 211 | #ifdef OUTPUT_TEMPERATURE |
hardtail | 0:6e61e8ec4b42 | 212 | float temp = mpu.getTemperature() / 340.0 + 36.53; |
hardtail | 0:6e61e8ec4b42 | 213 | if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return; |
hardtail | 0:6e61e8ec4b42 | 214 | pc.puts(snprintf_buffer); |
hardtail | 0:6e61e8ec4b42 | 215 | #endif |
hardtail | 0:6e61e8ec4b42 | 216 | |
hardtail | 0:6e61e8ec4b42 | 217 | pc.puts("\n"); |
hardtail | 0:6e61e8ec4b42 | 218 | } |
hardtail | 0:6e61e8ec4b42 | 219 | } |