Weber Yang / Mbed 2 deprecated AGV_SMT

start to work from here...

Dependencies:   MPU6050-DMP mbed ros_lib_kinetic

Fork of AGV_0411 by Weber Yang

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?

UserRevisionLine numberNew 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 }