homework

ESP32 慣性元件

22 Mar 2022 • Zi Ling Wu

實作

系統方塊圖

程式碼1

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 (“Copyleft”). Contact information ——————- Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com */

#include “Kalman.h” // Source: https://github.com/TKJElectronics/KalmanFilter #include “I2Cdev.h” #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include “Wire.h” #endif #include “MPU6050.h” // nodeMCU I2C default SCL : D1, SDA : D2 //#define sda 14 // NodeMCU pin D5 //#define scl 12 // NodeMCU pin D6

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX; // Create the Kalman instances Kalman kalmanY;

/* IMU Data */ MPU6050 imu; int16_t accX, accY, accZ; int16_t gyroX, gyroY, gyroZ;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only double compAngleX, compAngleY; // Calculated angle using a complementary filter double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

uint32_t timer; uint8_t i2cData[14]; // Buffer for I2C data

// TODO: Make calibration routine

void setup() { Serial.begin(115200); //Wire.begin(sda, scl); Wire.begin(); #if ARDUINO >= 157 Wire.setClock(400000UL); // Set I2C frequency to 400kHz #else TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz #endif

imu.initialize();

delay(100); // Wait for sensor to stabilize

/* Set kalman and gyro starting angle */ imu.getAcceleration(&accX, &accY, &accZ);

// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2 // It is then converted from radians to degrees #ifdef RESTRICT_PITCH // Eq. 25 and 26 double roll = atan2(accY, accZ) * RAD_TO_DEG; double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; #else // Eq. 28 and 29 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; double pitch = atan2(-accX, accZ) * RAD_TO_DEG; #endif

kalmanX.setAngle(roll); // Set starting angle kalmanY.setAngle(pitch); gyroXangle = roll; gyroYangle = pitch; compAngleX = roll; compAngleY = pitch;

timer = micros(); }

void loop() { /* Update IMU sensor values */ imu.getAcceleration(&accX, &accY, &accZ); imu.getRotation(&gyroX, &gyroY, &gyroZ);

double dt = (double)(micros() - timer) / 1000000; // Calculate delta time timer = micros();

double gyroXrate = gyroX / 131.0; // Convert to deg/s double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((roll < -90 && kalAngleX > 90)

(roll > 90 && kalAngleX < -90)) { kalmanX.setAngle(roll); compAngleX = roll; kalAngleX = roll; gyroXangle = roll; } else kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

if (abs(kalAngleX) > 90) gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); #else // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((pitch < -90 && kalAngleY > 90)

(pitch > 90 && kalAngleY < -90)) { kalmanY.setAngle(pitch); compAngleY = pitch; kalAngleY = pitch; gyroYangle = pitch; } else kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

if (abs(kalAngleY) > 90) gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter #endif

gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter gyroYangle += gyroYrate * dt; //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate //gyroYangle += kalmanY.getRate() * dt;

compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

// Reset the gyro angle when it has drifted too much if (gyroXangle < -180

gyroXangle > 180) gyroXangle = kalAngleX; if (gyroYangle < -180

gyroYangle > 180) gyroYangle = kalAngleY;

/* Print Data */ #if 0 // Set to 1 to activate Serial.print(accX); Serial.print(“\t”); Serial.print(accY); Serial.print(“\t”); Serial.print(accZ); Serial.print(“\t”); Serial.print(gyroX); Serial.print(“\t”); Serial.print(gyroY); Serial.print(“\t”); Serial.print(gyroZ); Serial.print(“\t”); Serial.print(“\t”); #endif

Serial.print(roll); Serial.print(“\t”); Serial.print(gyroXangle); Serial.print(“\t”); Serial.print(compAngleX); Serial.print(“\t”); Serial.print(kalAngleX); Serial.print(“\t”);

Serial.print(“\t”);

Serial.print(pitch); Serial.print(“\t”); Serial.print(gyroYangle); Serial.print(“\t”); Serial.print(compAngleY); Serial.print(“\t”); Serial.print(kalAngleY); Serial.print(“\t”);

#if 0 // Set to 1 to print the temperature Serial.print(“\t”); double temperature = (double)tempRaw / 340.0 + 36.53; Serial.print(temperature); Serial.print(“\t”); #endif

Serial.print(“\r\n”); delay(2); }

程式碼2

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v6.12) // 6/21/2012 by Jeff Rowberg jeff@rowberg.net // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: // 2019-07-10 - Uses the new version of the DMP Firmware V6.12 // - Note: I believe the Teapot demo is broken with this versin as // - the fifo buffer structure has changed

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files // for both classes must be in the include path of your project #include “I2Cdev.h”

#include “MPU6050_6Axis_MotionApps_V6_12.h” //#include “MPU6050.h” // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation // is used in I2Cdev.h #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include “Wire.h” #endif

// class default I2C address is 0x68 // specific I2C addresses may be passed as a parameter here // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) // AD0 high = 0x69 MPU6050 mpu; //MPU6050 mpu(0x69); // <– use for AD0 high

//#define OUTPUT_READABLE_QUATERNION //#define OUTPUT_READABLE_EULER //#define OUTPUT_READABLE_YAWPITCHROLL //#define OUTPUT_READABLE_REALACCEL //#define OUTPUT_READABLE_WORLDACCEL #define OUTPUT_TEAPOT

#define INTERRUPT_PIN 0 // use pin 2 on Arduino Uno & most boards #define LED_PIN 2 // (Arduino is 13, Teensy is 11, Teensy++ is 6, ESP32 is 2) bool blinkState = false;

// MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 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

// orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 gy; // [x, y, z] gyro 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 float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { ‘$’, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, ‘\r’, ‘\n’ };

// ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; }

// ================================================================ // === INITIAL SETUP === // ================================================================

void setup() { // join I2C bus (I2Cdev library doesn’t do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif

// initialize serial communication // (115200 chosen because it is required for Teapot Demo output, but it’s // really up to you depending on your project) Serial.begin(115200); while (!Serial); // wait for Leonardo enumeration, others continue immediately

// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to // the baud timing being too misaligned with processor ticks. You must use // 38400 or slower in these cases, or use some kind of external separate // crystal solution for the UART timer.

// initialize device Serial.println(F(“Initializing I2C devices…”)); mpu.initialize(); pinMode(INTERRUPT_PIN, INPUT);

// verify connection Serial.println(F(“Testing device connections…”)); Serial.println(mpu.testConnection() ? F(“MPU6050 connection successful”) : F(“MPU6050 connection failed”));

// wait for ready Serial.println(F(“\nSend any character to begin DMP programming and demo: “)); while (Serial.available() && Serial.read()); // empty buffer while (!Serial.available()); // wait for data while (Serial.available() && Serial.read()); // empty buffer again

// load and configure the DMP Serial.println(F(“Initializing DMP…”)); devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity mpu.setXGyroOffset(51); mpu.setYGyroOffset(8); mpu.setZGyroOffset(21); mpu.setXAccelOffset(1150); mpu.setYAccelOffset(-50); mpu.setZAccelOffset(1060); // make sure it worked (returns 0 if so) if (devStatus == 0) { // Calibration Time: generate offsets and calibrate our MPU6050 mpu.CalibrateAccel(6); mpu.CalibrateGyro(6); Serial.println(); mpu.PrintActiveOffsets(); // turn on the DMP, now that it’s ready Serial.println(F(“Enabling DMP…”)); mpu.setDMPEnabled(true);

// enable Arduino interrupt detection Serial.print(F(“Enabling interrupt detection (Arduino external interrupt “)); Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); Serial.println(F(“)…”)); attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it’s okay to use it Serial.println(F(“DMP ready! Waiting for first interrupt…”)); dmpReady = true;

// get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it’s going to break, usually the code will be 1) Serial.print(F(“DMP Initialization failed (code “)); Serial.print(devStatus); Serial.println(F(“)”)); } // configure LED for output pinMode(LED_PIN, OUTPUT); }

// ================================================================ // === MAIN PROGRAM LOOP === // ================================================================

void loop() { // if programming failed, don’t try to do anything if (!dmpReady) return; // read a packet from FIFO if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet

#ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); Serial.print(“quat\t”); Serial.print(q.w); Serial.print(“\t”); Serial.print(q.x); Serial.print(“\t”); Serial.print(q.y); Serial.print(“\t”); Serial.println(q.z); #endif

#ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); Serial.print(“euler\t”); Serial.print(euler[0] * 180 / M_PI); Serial.print(“\t”); Serial.print(euler[1] * 180 / M_PI); Serial.print(“\t”); Serial.println(euler[2] * 180 / M_PI); #endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); Serial.print(“ypr\t”); Serial.print(ypr[0] * 180 / M_PI); Serial.print(“\t”); Serial.print(ypr[1] * 180 / M_PI); Serial.print(“\t”); Serial.print(ypr[2] * 180 / M_PI); /* mpu.dmpGetAccel(&aa, fifoBuffer); Serial.print(“\tRaw Accl XYZ\t”); Serial.print(aa.x); Serial.print(“\t”); Serial.print(aa.y); Serial.print(“\t”); Serial.print(aa.z); mpu.dmpGetGyro(&gy, fifoBuffer); Serial.print(“\tRaw Gyro XYZ\t”); Serial.print(gy.x); Serial.print(“\t”); Serial.print(gy.y); Serial.print(“\t”); Serial.print(gy.z); */ Serial.println();

#endif

#ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial.print(“areal\t”); Serial.print(aaReal.x); Serial.print(“\t”); Serial.print(aaReal.y); Serial.print(“\t”); Serial.println(aaReal.z); #endif

#ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); Serial.print(“aworld\t”); Serial.print(aaWorld.x); Serial.print(“\t”); Serial.print(aaWorld.y); Serial.print(“\t”); Serial.println(aaWorld.z); #endif

#ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: 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]; Serial.write(teapotPacket, 14); teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif

// blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }

This site was last updated June 01, 2023.