Merge pull request #274 from RobertRFenichel/master

Improvements in comments and search strategy.

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 ([email protected])
+
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
+// 10/7/2011 by Jeff Rowberg <[email protected]>
+// 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