Merge pull request #273 from RobertRFenichel/master

Add IMU_Zero example for MPU-6050 zero calibration

Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino

@@ -0,0 +1,263 @@
+// 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-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.
+
+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.
+===============================================
+*/
+
+// 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 N = 1000;                // the bigger, the smoother (and slower)
+const int LinesBetweenHeaders = 5;
+      int LowValue[6];
+      int HighValue[6];
+      int Smoothed[6];
+      int LowOffset[6];
+      int HighOffset[6];
+      int Target[6];
+      int LinesOut;
+
+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[0], &RawValue[1], &RawValue[2], 
+                             &RawValue[3], &RawValue[4], &RawValue[5]);
+        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"); }
+      }
+    ForceHeader();
+  } // 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 setup()
+  { boolean StillWorking;
+    int NewOffset[6];
+
+    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;
+
+    Serial.println("expanding:");
+    ForceHeader();
+    PullBracketsOut();
+
+    Serial.println("\nclosing in:");
+    ForceHeader();
+    StillWorking = true;
+    while (StillWorking) 
+      { StillWorking = false;
+        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;
+              } // 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