diff --git a/Arduino/MPU6050/examples/IMU_Zero.ino b/Arduino/MPU6050/examples/IMU_Zero.ino new file mode 100644 index 00000000..34a6b62b --- /dev/null +++ b/Arduino/MPU6050/examples/IMU_Zero.ino @@ -0,0 +1,297 @@ +// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW +// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net) + +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class +// 10/7/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion +// dynamic speed change when closing in +// 2016-10-22 - cosmetic changes +// 2016-10-19 - initial release +// 2013-05-08 - added multiple output formats +// - added seamless Fastwire support +// 2011-10-07 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE 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 SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + + If an MPU6050 is an ideal member of its tribe, is properly warmed up, is at +rest in a neutral position, and has been loaded with the best possible offsets, it +will report 0 for all accelerations and displacements, except for Z acceleration, +for which it will report 16384 (that is, 2^14). Your device probably won't do +quite this well, but good offsets will all get the baseline outputs close to +these target values. + + Put the MPU6050 in a flat and horizontal surface, and leave it operating for +5-10 minutes so its temperature gets stabilized. + + Run this program. A "----- done -----" line will indicate that it has done its best. + + The line just above that will look something like + [567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4] +As will have been shown in interspersed header lines, the six groups making up this +line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration, +X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed +that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration, +and so on. +=============================================== +*/ + +// 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.h" + +// 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 InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 accelgyro; +//MPU6050 accelgyro(0x69); // <-- use for AD0 high + + +const char LBRACKET = '['; +const char RBRACKET = ']'; +const char COMMA = ','; + +const int iAx = 0; +const int iAy = 1; +const int iAz = 2; +const int iGx = 3; +const int iGy = 4; +const int iGz = 5; + +const int NFast = 1000; // the bigger, the better (but slower) +const int NSlow = 10000; // .. +const int LinesBetweenHeaders = 5; + int LowValue[6]; + int HighValue[6]; + int Smoothed[6]; + int LowOffset[6]; + int HighOffset[6]; + int Target[6]; + int LinesOut; + int N; + +void ForceHeader() + { LinesOut = 99; } + +void GetSmoothed() + { int RawValue[6]; + long Sums[6]; + for (int i = iAx; i <= iGz; i++) + { Sums[i] = 0; } + + for (int i = 1; i <= N; i++) + { // get sums + accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], + &RawValue[iGx], &RawValue[iGy], &RawValue[iGz]); + for (int j = iAx; j <= iGz; j++) + Sums[j] = Sums[j] + RawValue[j]; + } // get sums + for (int i = iAx; i <= iGz; i++) + { Smoothed[i] = (Sums[i] + N/2) / N ; } + } // GetSmoothed + +void Initialize() + { + // join I2C bus (I2Cdev library doesn't do this automatically) + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + Serial.begin(9600); + + // initialize device + Serial.println("Initializing I2C devices..."); + accelgyro.initialize(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + } // Initialize + +void SetOffsets(int TheOffsets[6]) + { accelgyro.setXAccelOffset(TheOffsets [iAx]); + accelgyro.setYAccelOffset(TheOffsets [iAy]); + accelgyro.setZAccelOffset(TheOffsets [iAz]); + accelgyro.setXGyroOffset (TheOffsets [iGx]); + accelgyro.setYGyroOffset (TheOffsets [iGy]); + accelgyro.setZGyroOffset (TheOffsets [iGz]); + } // SetOffsets + +void ShowProgress() + { if (LinesOut >= LinesBetweenHeaders) + { // show header + Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro"); + LinesOut = 0; + } // show header + for (int i = iAx; i <= iGz; i++) + { Serial.print(LBRACKET); + Serial.print(LowOffset[i]), + Serial.print(COMMA); + Serial.print(HighOffset[i]); + Serial.print("] --> ["); + Serial.print(LowValue[i]); + Serial.print(COMMA); + Serial.print(HighValue[i]); + if (i == iGz) + { Serial.println(RBRACKET); } + else + { Serial.print("]\t"); } + } + LinesOut++; + } // ShowProgress + +void PullBracketsOut() + { boolean Done = false; + int NextLowOffset[6]; + int NextHighOffset[6]; + + SetOffsets(HighOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { HighValue[i] = Smoothed[i]; // needed for ShowProgress + } + + while (!Done) + { Done = true; + SetOffsets(LowOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got low values + LowValue[i] = Smoothed[i]; + if (LowValue[i] >= Target[i]) + { Done = false; + NextLowOffset[i] = LowOffset[i] - 1000; + } + else + { NextLowOffset[i] = LowOffset[i]; } + } // got low values + ShowProgress(); + for (int i = iAx; i <= iGz; i++) + { LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done + } + } // keep going + + Done = false; + while (!Done) + { Done = true; + SetOffsets(HighOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got high values + HighValue[i] = Smoothed[i]; + if (HighValue[i] <= Target[i]) + { Done = false; + NextHighOffset[i] = HighOffset[i] + 1000; + } + else + { NextHighOffset[i] = HighOffset[i]; } + } // got high values + ShowProgress(); + for (int i = iAx; i <= iGz; i++) + { HighOffset[i] = NextHighOffset[i]; // had to wait until ShowProgress done + } + } // keep going + } // PullBracketOut + +void SetAveraging(int NewN) + { N = NewN; + Serial.print("averaging "); + Serial.print(N); + Serial.println(" readings each time"); + } // SetAveraging + +void setup() + { boolean StillWorking; + int NewOffset[6]; + boolean AllBracketsNarrow; + + Initialize(); + for (int i = iAx; i <= iGz; i++) + { // set targets and initial guesses + Target[i] = 0; // must fix for ZAccel + HighOffset[i] = 0; + LowOffset[i] = 0; + } // set targets and initial guesses + Target[iAz] = 16384; + SetAveraging(NFast); + + Serial.println("expanding:"); + ForceHeader(); + PullBracketsOut(); + + Serial.println("\nclosing in:"); + AllBracketsNarrow = false; + ForceHeader(); + StillWorking = true; + while (StillWorking) + { StillWorking = false; + if (AllBracketsNarrow && (N == NFast)) + { SetAveraging(NSlow); } + else + { AllBracketsNarrow = true; }// tentative + for (int i = iAx; i <= iGz; i++) + { if (HighOffset[i] <= (LowOffset[i]+1)) + { NewOffset[i] = LowOffset[i]; } + else + { // binary search + StillWorking = true; + NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2; + if (HighOffset[i] > (LowOffset[i] + 10)) + { AllBracketsNarrow = false; } + } // binary search + } + SetOffsets(NewOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // closing in + if (Smoothed[i] > Target[i]) + { // use lower half + HighOffset[i] = NewOffset[i]; + HighValue[i] = Smoothed[i]; + } // use lower half + else + { // use upper half + LowOffset[i] = NewOffset[i]; + LowValue[i] = Smoothed[i]; + } // use upper half + } // closing in + ShowProgress(); + } // still working + Serial.println("-------------- done --------------"); + } // setup + +void loop() + { + } // loop diff --git a/Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino b/Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino index b889565e..34a6b62b 100644 --- a/Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino +++ b/Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino @@ -6,6 +6,9 @@ // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: +// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion +// dynamic speed change when closing in +// 2016-10-22 - cosmetic changes // 2016-10-19 - initial release // 2013-05-08 - added multiple output formats // - added seamless Fastwire support @@ -33,12 +36,25 @@ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -Put the MPU6050 in a flat and horizontal surface. - - Leave it operating for 5-10 minutes so temperature gets stabilized. - - Run this program. A "----- done -----" line will indicate that it has done its best. - - For each of the 6 offsets, you'll see 2 adjacent values to choose between, together - with their respective IMU outputs. One of the choices may be a little better than - the other. + If an MPU6050 is an ideal member of its tribe, is properly warmed up, is at +rest in a neutral position, and has been loaded with the best possible offsets, it +will report 0 for all accelerations and displacements, except for Z acceleration, +for which it will report 16384 (that is, 2^14). Your device probably won't do +quite this well, but good offsets will all get the baseline outputs close to +these target values. + + Put the MPU6050 in a flat and horizontal surface, and leave it operating for +5-10 minutes so its temperature gets stabilized. + + Run this program. A "----- done -----" line will indicate that it has done its best. + + The line just above that will look something like + [567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4] +As will have been shown in interspersed header lines, the six groups making up this +line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration, +X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed +that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration, +and so on. =============================================== */ @@ -72,7 +88,8 @@ const int iGx = 3; const int iGy = 4; const int iGz = 5; -const int N = 1000; // the bigger, the smoother (and slower) +const int NFast = 1000; // the bigger, the better (but slower) +const int NSlow = 10000; // .. const int LinesBetweenHeaders = 5; int LowValue[6]; int HighValue[6]; @@ -81,6 +98,7 @@ const int LinesBetweenHeaders = 5; int HighOffset[6]; int Target[6]; int LinesOut; + int N; void ForceHeader() { LinesOut = 99; } @@ -93,8 +111,8 @@ void GetSmoothed() for (int i = 1; i <= N; i++) { // get sums - accelgyro.getMotion6(&RawValue[0], &RawValue[1], &RawValue[2], - &RawValue[3], &RawValue[4], &RawValue[5]); + accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], + &RawValue[iGx], &RawValue[iGy], &RawValue[iGz]); for (int j = iAx; j <= iGz; j++) Sums[j] = Sums[j] + RawValue[j]; } // get sums @@ -151,7 +169,7 @@ void ShowProgress() else { Serial.print("]\t"); } } - ForceHeader(); + LinesOut++; } // ShowProgress void PullBracketsOut() @@ -172,7 +190,7 @@ void PullBracketsOut() for (int i = iAx; i <= iGz; i++) { // got low values LowValue[i] = Smoothed[i]; - if (LowValue[i] > Target[i]) + if (LowValue[i] >= Target[i]) { Done = false; NextLowOffset[i] = LowOffset[i] - 1000; } @@ -193,7 +211,7 @@ void PullBracketsOut() for (int i = iAx; i <= iGz; i++) { // got high values HighValue[i] = Smoothed[i]; - if (HighValue[i] < Target[i]) + if (HighValue[i] <= Target[i]) { Done = false; NextHighOffset[i] = HighOffset[i] + 1000; } @@ -207,9 +225,17 @@ void PullBracketsOut() } // keep going } // PullBracketOut +void SetAveraging(int NewN) + { N = NewN; + Serial.print("averaging "); + Serial.print(N); + Serial.println(" readings each time"); + } // SetAveraging + void setup() { boolean StillWorking; int NewOffset[6]; + boolean AllBracketsNarrow; Initialize(); for (int i = iAx; i <= iGz; i++) @@ -219,23 +245,31 @@ void setup() LowOffset[i] = 0; } // set targets and initial guesses Target[iAz] = 16384; + SetAveraging(NFast); Serial.println("expanding:"); ForceHeader(); PullBracketsOut(); Serial.println("\nclosing in:"); + AllBracketsNarrow = false; ForceHeader(); StillWorking = true; while (StillWorking) { StillWorking = false; + if (AllBracketsNarrow && (N == NFast)) + { SetAveraging(NSlow); } + else + { AllBracketsNarrow = true; }// tentative for (int i = iAx; i <= iGz; i++) { if (HighOffset[i] <= (LowOffset[i]+1)) { NewOffset[i] = LowOffset[i]; } else { // binary search StillWorking = true; - NewOffset[i] = (LowOffset[i] + HighOffset[i]) /2; + NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2; + if (HighOffset[i] > (LowOffset[i] + 10)) + { AllBracketsNarrow = false; } } // binary search } SetOffsets(NewOffset); @@ -260,4 +294,4 @@ void setup() void loop() { - } // loop + } // loop