Skip to content

Commit f22bbf7

Browse files
nopnop2002nopnop2002
committed
First Release
1 parent 69e42b9 commit f22bbf7

36 files changed

+8967
-1
lines changed

‎IMU_Zero/CMakeLists.txt

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# The following lines of boilerplate have to be in your project's CMakeLists
2+
# in this exact order for cmake to work correctly
3+
cmake_minimum_required(VERSION 3.5)
4+
5+
set(EXTRA_COMPONENT_DIRS ../components/I2Cdev ../components/MPU6050)
6+
7+
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
8+
project(IMU_Zero)

‎IMU_Zero/main/CMakeLists.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
set(COMPONENT_SRCS "main.cpp mpu6050.cpp")
2+
set(COMPONENT_ADD_INCLUDEDIRS ".")
3+
4+
register_component()

‎IMU_Zero/main/Kconfig.projbuild

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
menu "Application Configuration"
2+
3+
config GPIO_RANGE_MAX
4+
int
5+
default 33 if IDF_TARGET_ESP32
6+
default 46 if IDF_TARGET_ESP32S2
7+
default 48 if IDF_TARGET_ESP32S3
8+
default 18 if IDF_TARGET_ESP32C2
9+
default 19 if IDF_TARGET_ESP32C3
10+
11+
config GPIO_SCL
12+
int "SCL GPIO number"
13+
range 0 GPIO_RANGE_MAX
14+
default 22 if IDF_TARGET_ESP32
15+
default 12 if IDF_TARGET_ESP32S2
16+
default 12 if IDF_TARGET_ESP32S3
17+
default 5 if IDF_TARGET_ESP32C3 || IDF_TARGET_ESP32C2
18+
help
19+
GPIO number (IOxx) to I2C SCL.
20+
21+
config GPIO_SDA
22+
int "SDA GPIO number"
23+
range 0 GPIO_RANGE_MAX
24+
default 21 if IDF_TARGET_ESP32
25+
default 11 if IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3
26+
default 11 if IDF_TARGET_ESP32C3 || IDF_TARGET_ESP32C2
27+
help
28+
GPIO number (IOxx) of SDA.
29+
30+
endmenu

‎IMU_Zero/main/component.mk

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
#
2+
# Main component makefile.
3+
#
4+
# This Makefile can be left empty. By default, it will take the sources in the
5+
# src/ directory, compile them and link them into lib(subdirectory_name).a
6+
# in the build directory. This behaviour is entirely configurable,
7+
# please read the ESP-IDF documents if you need to do this.
8+
#

‎IMU_Zero/main/main.cpp

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
#include "freertos/FreeRTOS.h"
2+
#include "freertos/task.h"
3+
#include "esp_log.h"
4+
#include <driver/i2c.h>
5+
6+
extern "C" {
7+
void app_main(void);
8+
}
9+
10+
void mpu6050(void *pvParameters);
11+
12+
void app_main(void)
13+
{
14+
i2c_config_t conf;
15+
conf.mode = I2C_MODE_MASTER;
16+
conf.sda_io_num = (gpio_num_t)CONFIG_GPIO_SDA;
17+
conf.scl_io_num = (gpio_num_t)CONFIG_GPIO_SCL;
18+
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
19+
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
20+
conf.master.clk_speed = 400000;
21+
ESP_ERROR_CHECK(i2c_param_config(I2C_NUM_0, &conf));
22+
ESP_ERROR_CHECK(i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0));
23+
24+
vTaskDelay(500/portTICK_PERIOD_MS);
25+
xTaskCreate(&mpu6050, "IMU", 1024*8, NULL, 5, NULL);
26+
}

‎IMU_Zero/main/mpu6050.cpp

Lines changed: 307 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,307 @@
1+
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
2+
// 6/21/2012 by Jeff Rowberg <[email protected]>
3+
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
4+
//
5+
// Changelog:
6+
// 2023-03-10 - Fit to esp-idf v5
7+
// 2019-07-08 - Added Auto Calibration and offset generator
8+
// - and altered FIFO retrieval sequence to avoid using blocking code
9+
// 2016-04-18 - Eliminated a potential infinite loop
10+
// 2013-05-08 - added seamless Fastwire support
11+
// - added note about gyro calibration
12+
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
13+
// 2012-06-20 - improved FIFO overflow handling and simplified read process
14+
// 2012-06-19 - completely rearranged DMP initialization code and simplification
15+
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
16+
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
17+
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
18+
// - add 3D math helper file to DMP6 example sketch
19+
// - add Euler output and Yaw/Pitch/Roll output formats
20+
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
21+
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
22+
// 2012-05-30 - basic DMP initialization working
23+
24+
/* ============================================
25+
I2Cdev device library code is placed under the MIT license
26+
Copyright (c) 2012 Jeff Rowberg
27+
Permission is hereby granted, free of charge, to any person obtaining a copy
28+
of this software and associated documentation files (the "Software"), to deal
29+
in the Software without restriction, including without limitation the rights
30+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
31+
copies of the Software, and to permit persons to whom the Software is
32+
furnished to do so, subject to the following conditions:
33+
The above copyright notice and this permission notice shall be included in
34+
all copies or substantial portions of the Software.
35+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
36+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
37+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
38+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
39+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
40+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
41+
THE SOFTWARE.
42+
===============================================
43+
*/
44+
45+
#include <driver/i2c.h>
46+
#include <esp_log.h>
47+
#include <esp_err.h>
48+
#include <freertos/FreeRTOS.h>
49+
#include <freertos/task.h>
50+
51+
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
52+
// for both classes must be in the include path of your project
53+
#include "I2Cdev.h"
54+
55+
#include "MPU6050.h" // not necessary if using MotionApps include file
56+
//#include "MPU6050_6Axis_MotionApps20.h"
57+
58+
MPU6050 accelgyro;
59+
60+
const char LBRACKET = '[';
61+
const char RBRACKET = ']';
62+
const char COMMA = ',';
63+
const char BLANK = ' ';
64+
const char PERIOD = '.';
65+
66+
const int iAx = 0;
67+
const int iAy = 1;
68+
const int iAz = 2;
69+
const int iGx = 3;
70+
const int iGy = 4;
71+
const int iGz = 5;
72+
73+
const int usDelay = 3150; // empirical, to hold sampling to 200 Hz
74+
const int NFast = 1000; // the bigger, the better (but slower)
75+
const int NSlow = 10000; // ..
76+
const int LinesBetweenHeaders = 5;
77+
int LowValue[6];
78+
int HighValue[6];
79+
int Smoothed[6];
80+
int LowOffset[6];
81+
int HighOffset[6];
82+
int Target[6];
83+
int LinesOut;
84+
int N;
85+
86+
void ForceHeader()
87+
{ LinesOut = 99; }
88+
89+
void GetSmoothed()
90+
{ int16_t RawValue[6];
91+
int i;
92+
long Sums[6];
93+
for (i = iAx; i <= iGz; i++)
94+
{ Sums[i] = 0; }
95+
// unsigned long Start = micros();
96+
97+
for (i = 1; i <= N; i++) { // get sums
98+
accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz],
99+
&RawValue[iGx], &RawValue[iGy], &RawValue[iGz]);
100+
if ((i % 500) == 0)
101+
printf("%c", PERIOD);
102+
delayMicroseconds(usDelay);
103+
for (int j = iAx; j <= iGz; j++)
104+
Sums[j] = Sums[j] + RawValue[j];
105+
} // get sums
106+
// unsigned long usForN = micros() - Start;
107+
// printf(" reading at ");
108+
// printf("%f", 1000000/((usForN+N/2)/N));
109+
// printf(" Hz\n");
110+
for (i = iAx; i <= iGz; i++)
111+
{ Smoothed[i] = (Sums[i] + N/2) / N ; }
112+
} // GetSmoothed
113+
114+
void Initialize() {
115+
// initialize device
116+
printf("Initializing I2C devices...\n");
117+
accelgyro.initialize();
118+
119+
// verify connection
120+
printf("Testing device connections...");
121+
printf(accelgyro.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
122+
printf("PID tuning Each Dot = 100 readings\n");
123+
/*A tidbit on how PID (PI actually) tuning works.
124+
When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and
125+
integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral
126+
uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it
127+
to the integral value. Each reading narrows the error down to the desired offset. The greater the error from
128+
set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the
129+
integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the
130+
noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100
131+
readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to
132+
the fact it reacts to any noise.
133+
*/
134+
accelgyro.CalibrateAccel(6);
135+
accelgyro.CalibrateGyro(6);
136+
printf("\nat 600 Readings\n");
137+
accelgyro.PrintActiveOffsets();
138+
printf("\n");
139+
accelgyro.CalibrateAccel(1);
140+
accelgyro.CalibrateGyro(1);
141+
printf("700 Total Readings\n");
142+
accelgyro.PrintActiveOffsets();
143+
printf("\n");
144+
accelgyro.CalibrateAccel(1);
145+
accelgyro.CalibrateGyro(1);
146+
printf("800 Total Readings\n");
147+
accelgyro.PrintActiveOffsets();
148+
printf("\n");
149+
accelgyro.CalibrateAccel(1);
150+
accelgyro.CalibrateGyro(1);
151+
printf("900 Total Readings\n");
152+
accelgyro.PrintActiveOffsets();
153+
printf("\n");
154+
accelgyro.CalibrateAccel(1);
155+
accelgyro.CalibrateGyro(1);
156+
printf("1000 Total Readings\n");
157+
accelgyro.PrintActiveOffsets();
158+
printf("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:\n");
159+
} // Initialize
160+
161+
void SetOffsets(int TheOffsets[6]) {
162+
accelgyro.setXAccelOffset(TheOffsets [iAx]);
163+
accelgyro.setYAccelOffset(TheOffsets [iAy]);
164+
accelgyro.setZAccelOffset(TheOffsets [iAz]);
165+
accelgyro.setXGyroOffset (TheOffsets [iGx]);
166+
accelgyro.setYGyroOffset (TheOffsets [iGy]);
167+
accelgyro.setZGyroOffset (TheOffsets [iGz]);
168+
} // SetOffsets
169+
170+
void SetAveraging(int NewN) {
171+
N = NewN;
172+
printf("averaging ");
173+
printf("%d", N);
174+
printf(" readings each time\n");
175+
} // SetAveraging
176+
177+
void ShowProgress() {
178+
if (LinesOut >= LinesBetweenHeaders)
179+
{ // show header
180+
printf("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro\n");
181+
LinesOut = 0;
182+
} // show header
183+
printf("%c", BLANK);
184+
for (int i = iAx; i <= iGz; i++)
185+
{ printf("%c", LBRACKET);
186+
printf("%d", LowOffset[i]),
187+
printf("%c", COMMA);
188+
printf("%d", HighOffset[i]);
189+
printf("] --> [");
190+
printf("%d", LowValue[i]);
191+
printf("%c", COMMA);
192+
printf("%d", HighValue[i]);
193+
if (i == iGz)
194+
{ printf("%c\n",RBRACKET); }
195+
else
196+
{ printf("]\t"); }
197+
}
198+
LinesOut++;
199+
} // ShowProgress
200+
201+
void PullBracketsIn() {
202+
bool AllBracketsNarrow;
203+
bool StillWorking;
204+
int NewOffset[6];
205+
206+
printf("\nclosing in:\n");
207+
AllBracketsNarrow = false;
208+
ForceHeader();
209+
StillWorking = true;
210+
while (StillWorking)
211+
{ StillWorking = false;
212+
if (AllBracketsNarrow && (N == NFast))
213+
{ SetAveraging(NSlow); }
214+
else
215+
{ AllBracketsNarrow = true; }// tentative
216+
for (int i = iAx; i <= iGz; i++)
217+
{ if (HighOffset[i] <= (LowOffset[i]+1))
218+
{ NewOffset[i] = LowOffset[i]; }
219+
else
220+
{ // binary search
221+
StillWorking = true;
222+
NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2;
223+
if (HighOffset[i] > (LowOffset[i] + 10))
224+
{ AllBracketsNarrow = false; }
225+
} // binary search
226+
}
227+
SetOffsets(NewOffset);
228+
GetSmoothed();
229+
for (int i = iAx; i <= iGz; i++)
230+
{ // closing in
231+
if (Smoothed[i] > Target[i])
232+
{ // use lower half
233+
HighOffset[i] = NewOffset[i];
234+
HighValue[i] = Smoothed[i];
235+
} // use lower half
236+
else
237+
{ // use upper half
238+
LowOffset[i] = NewOffset[i];
239+
LowValue[i] = Smoothed[i];
240+
} // use upper half
241+
} // closing in
242+
ShowProgress();
243+
} // still working
244+
245+
} // PullBracketsIn
246+
247+
void PullBracketsOut() {
248+
bool Done = false;
249+
int NextLowOffset[6];
250+
int NextHighOffset[6];
251+
252+
printf("expanding:\n");
253+
ForceHeader();
254+
255+
while (!Done)
256+
{ Done = true;
257+
SetOffsets(LowOffset);
258+
GetSmoothed();
259+
for (int i = iAx; i <= iGz; i++)
260+
{ // got low values
261+
LowValue[i] = Smoothed[i];
262+
if (LowValue[i] >= Target[i])
263+
{ Done = false;
264+
NextLowOffset[i] = LowOffset[i] - 1000;
265+
}
266+
else
267+
{ NextLowOffset[i] = LowOffset[i]; }
268+
} // got low values
269+
270+
SetOffsets(HighOffset);
271+
GetSmoothed();
272+
for (int i = iAx; i <= iGz; i++)
273+
{ // got high values
274+
HighValue[i] = Smoothed[i];
275+
if (HighValue[i] <= Target[i])
276+
{ Done = false;
277+
NextHighOffset[i] = HighOffset[i] + 1000;
278+
}
279+
else
280+
{ NextHighOffset[i] = HighOffset[i]; }
281+
} // got high values
282+
ShowProgress();
283+
for (int i = iAx; i <= iGz; i++)
284+
{ LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done
285+
HighOffset[i] = NextHighOffset[i]; // ..
286+
}
287+
} // keep going
288+
} // PullBracketsOut
289+
290+
void mpu6050(void *pvParameters){
291+
Initialize();
292+
for (int i = iAx; i <= iGz; i++)
293+
{ // set targets and initial guesses
294+
Target[i] = 0; // must fix for ZAccel
295+
HighOffset[i] = 0;
296+
LowOffset[i] = 0;
297+
} // set targets and initial guesses
298+
Target[iAz] = 16384;
299+
SetAveraging(NFast);
300+
301+
PullBracketsOut();
302+
PullBracketsIn();
303+
304+
printf("-------------- done --------------\n");
305+
306+
vTaskDelete(NULL);
307+
}

‎MPU6050_DMP6/CMakeLists.txt

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# The following lines of boilerplate have to be in your project's CMakeLists
2+
# in this exact order for cmake to work correctly
3+
cmake_minimum_required(VERSION 3.5)
4+
5+
set(EXTRA_COMPONENT_DIRS ../components/I2Cdev ../components/MPU6050)
6+
7+
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
8+
project(MPU6050_DMP6)

0 commit comments

Comments
 (0)