├── 1.Remote control operation Manual and APK ├── Balance_Car_EN.apk ├── Manual-1.jpg └── Manual-2.jpg ├── 2.Arduino IDE Programming Tutorials（SDK） ├── For Arduino UNO board │ ├── 1.Arduino Compile tools │ │ ├── How to switch the language of the IDE.docx │ │ ├── Installation instructions of Arduino IDE.docx │ │ └── Use instructions of Arduino IDE.docx │ └── 2.Solution to installation driver failure │ │ ├── 0.instruction.doc │ │ ├── Arduino UNO REV3.inf │ │ ├── mdmcpq.PNF │ │ ├── mdmcpq.inf │ │ └── usbser.sys └── For UNO board │ ├── 1.Uart drive（CH340） │ ├── CH341SER.EXE │ ├── DRIVER │ │ ├── CH341PT.DLL │ │ ├── CH341S64.SYS │ │ ├── CH341S98.SYS │ │ ├── CH341SER.INF │ │ ├── CH341SER.SYS │ │ └── CH341SER.VXD │ └── Method of drive installation .docx │ ├── 2.Arduino Compile tools │ ├── 1.Installation instructions of Arduino IDE.docx │ ├── 2.Use instructions of Arduino IDE.docx │ └── 3.How to switch the language of the IDE.docx │ └── Arduino-1.7.8.org-windows download link.txt ├── 3.Arduino balance car code_2019_2_14 └── bst_abc │ ├── BalanceCar.cpp │ ├── BalanceCar.h │ ├── I2Cdev.cpp │ ├── I2Cdev.h │ ├── JJ_MPU6050_DMP_6Axis.h │ ├── KalmanFilter.cpp │ ├── KalmanFilter.h │ ├── MPU6050.cpp │ ├── MPU6050.h │ ├── MPU6050_6Axis_MotionApps20.h │ ├── MPU6050_9Axis_MotionApps41.h │ ├── MsTimer2.cpp │ ├── MsTimer2.h │ ├── PinChangeInt.h │ ├── bst_abc.ino │ ├── helper_3dmath.h │ └── readme.txt ├── 4.SCH ├── Arduino balance car schematic_new version.pdf ├── Arduino balance car schematic_old version.pdf └── Readme.png ├── 5.Charging and battery └── 1.Charging method and battery use precautions.pdf ├── Note.docx ├── README.md ├── balance car Bluetooth communication protocol.pdf ├── balance car Bluetooth communication protocol.xlsx └── balance_robot.jpg /1.Remote control operation Manual and APK/Balance_Car_EN.apk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/1.Remote control operation Manual and APK/Balance_Car_EN.apk -------------------------------------------------------------------------------- /1.Remote control operation Manual and APK/Manual-1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/1.Remote control operation Manual and APK/Manual-1.jpg -------------------------------------------------------------------------------- /1.Remote control operation Manual and APK/Manual-2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/1.Remote control operation Manual and APK/Manual-2.jpg -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/1.Arduino Compile tools/How to switch the language of the IDE.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/1.Arduino Compile tools/How to switch the language of the IDE.docx -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/1.Arduino Compile tools/Installation instructions of Arduino IDE.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/1.Arduino Compile tools/Installation instructions of Arduino IDE.docx -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/1.Arduino Compile tools/Use instructions of Arduino IDE.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/1.Arduino Compile tools/Use instructions of Arduino IDE.docx -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/0.instruction.doc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/0.instruction.doc -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/Arduino UNO REV3.inf: -------------------------------------------------------------------------------- 1 | ;************************************************************ 2 | ; Windows USB CDC ACM Setup File 3 | ; Copyright (c) 2000 Microsoft Corporation 4 | 5 | 6 | [Version] 7 | Signature="$Windows NT$" 8 | Class=Ports 9 | ClassGuid={4D36E978-E325-11CE-BFC1-08002BE10318} 10 | Provider=%MFGNAME% 11 | LayoutFile=layout.inf 12 | CatalogFile=%MFGFILENAME%.cat 13 | DriverVer=11/15/2007,5.1.2600.0 14 | 15 | [Manufacturer] 16 | %MFGNAME%=DeviceList, NTamd64 17 | 18 | [DestinationDirs] 19 | DefaultDestDir=12 20 | 21 | 22 | ;------------------------------------------------------------------------------ 23 | ; Windows 2000/XP/Vista-32bit Sections 24 | ;------------------------------------------------------------------------------ 25 | 26 | [DriverInstall.nt] 27 | include=mdmcpq.inf 28 | CopyFiles=DriverCopyFiles.nt 29 | AddReg=DriverInstall.nt.AddReg 30 | 31 | [DriverCopyFiles.nt] 32 | usbser.sys,,,0x20 33 | 34 | [DriverInstall.nt.AddReg] 35 | HKR,,DevLoader,,*ntkern 36 | HKR,,NTMPDriver,,%DRIVERFILENAME%.sys 37 | HKR,,EnumPropPages32,,"MsPorts.dll,SerialPortPropPageProvider" 38 | 39 | [DriverInstall.nt.Services] 40 | AddService=usbser, 0x00000002, DriverService.nt 41 | 42 | [DriverService.nt] 43 | DisplayName=%SERVICE% 44 | ServiceType=1 45 | StartType=3 46 | ErrorControl=1 47 | ServiceBinary=%12%\%DRIVERFILENAME%.sys 48 | 49 | ;------------------------------------------------------------------------------ 50 | ; Vista-64bit Sections 51 | ;------------------------------------------------------------------------------ 52 | 53 | [DriverInstall.NTamd64] 54 | include=mdmcpq.inf 55 | CopyFiles=DriverCopyFiles.NTamd64 56 | AddReg=DriverInstall.NTamd64.AddReg 57 | 58 | [DriverCopyFiles.NTamd64] 59 | %DRIVERFILENAME%.sys,,,0x20 60 | 61 | [DriverInstall.NTamd64.AddReg] 62 | HKR,,DevLoader,,*ntkern 63 | HKR,,NTMPDriver,,%DRIVERFILENAME%.sys 64 | HKR,,EnumPropPages32,,"MsPorts.dll,SerialPortPropPageProvider" 65 | 66 | [DriverInstall.NTamd64.Services] 67 | AddService=usbser, 0x00000002, DriverService.NTamd64 68 | 69 | [DriverService.NTamd64] 70 | DisplayName=%SERVICE% 71 | ServiceType=1 72 | StartType=3 73 | ErrorControl=1 74 | ServiceBinary=%12%\%DRIVERFILENAME%.sys 75 | 76 | 77 | ;------------------------------------------------------------------------------ 78 | ; Vendor and Product ID Definitions 79 | ;------------------------------------------------------------------------------ 80 | ; When developing your USB device, the VID and PID used in the PC side 81 | ; application program and the firmware on the microcontroller must match. 82 | ; Modify the below line to use your VID and PID. Use the format as shown below. 83 | ; Note: One INF file can be used for multiple devices with different VID and PIDs. 84 | ; For each supported device, append ",USB\VID_xxxx&PID_yyyy" to the end of the line. 85 | ;------------------------------------------------------------------------------ 86 | [SourceDisksFiles] 87 | [SourceDisksNames] 88 | [DeviceList] 89 | %DESCRIPTION%=DriverInstall, USB\VID_2341&PID_0043 90 | 91 | [DeviceList.NTamd64] 92 | %DESCRIPTION%=DriverInstall, USB\VID_2341&PID_0043 93 | 94 | 95 | ;------------------------------------------------------------------------------ 96 | ; String Definitions 97 | ;------------------------------------------------------------------------------ 98 | ;Modify these strings to customize your device 99 | ;------------------------------------------------------------------------------ 100 | [Strings] 101 | MFGFILENAME="CDC_vista" 102 | DRIVERFILENAME ="usbser" 103 | MFGNAME="Arduino LLC (www.arduino.cc)" 104 | INSTDISK="Arduino UNO R3 Driver Installer" 105 | DESCRIPTION="Arduino UNO R3" 106 | SERVICE="USB RS-232 Emulation Driver" -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/mdmcpq.PNF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/mdmcpq.PNF -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/mdmcpq.inf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/mdmcpq.inf -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/usbser.sys: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For Arduino UNO board/2.Solution to installation driver failure/usbser.sys -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/CH341SER.EXE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/CH341SER.EXE -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341PT.DLL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341PT.DLL -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341S64.SYS: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341S64.SYS -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341S98.SYS: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341S98.SYS -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341SER.INF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341SER.INF -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341SER.SYS: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341SER.SYS -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341SER.VXD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/DRIVER/CH341SER.VXD -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/Method of drive installation .docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/1.Uart drive（CH340）/Method of drive installation .docx -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/2.Arduino Compile tools/1.Installation instructions of Arduino IDE.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/2.Arduino Compile tools/1.Installation instructions of Arduino IDE.docx -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/2.Arduino Compile tools/2.Use instructions of Arduino IDE.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/2.Arduino Compile tools/2.Use instructions of Arduino IDE.docx -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/2.Arduino Compile tools/3.How to switch the language of the IDE.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/2.Arduino IDE Programming Tutorials（SDK）/For UNO board/2.Arduino Compile tools/3.How to switch the language of the IDE.docx -------------------------------------------------------------------------------- /2.Arduino IDE Programming Tutorials（SDK）/For UNO board/Arduino-1.7.8.org-windows download link.txt: -------------------------------------------------------------------------------- 1 | https://drive.google.com/open?id=1STxhKCssOMAMi60mxhn_JCivNxjUCosJ -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/BalanceCar.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | BalanceCar.h BalanceCar.cpp - Library for BST-Balance car code. 3 | Created by SKY ZHU&ROOMS LUO, OCTOBER 2, 2016. 4 | JUST FOR THE Company of Technology of yahboom. 5 | In order to avoid Infringement Act,this core is not for the commerce except being authorized by the writer. 6 | */ 7 | 8 | #include "./BalanceCar.h" 9 | 10 | //////////////////�ٶ�PI//////////////////// 11 | double BalanceCar::speedpiout(double kps,double kis,double kds,int f,int b,double p0) 12 | { 13 | float speeds = (pulseleft + pulseright) * 1.0; //���� ����ֵ 14 | pulseright = pulseleft = 0; 15 | speeds_filterold *= 0.7; //һ�׻����˲� 16 | float speeds_filter = speeds_filterold + speeds * 0.3; 17 | speeds_filterold = speeds_filter; 18 | positions += speeds_filter; 19 | positions += f; //ȫ���������ں� 20 | positions += b; //ȫ���������ں� 21 | positions = constrain(positions, -3000,3000); //�����ֱ��� 22 | double output = kis * (p0 - positions) + kps * (p0 - speeds_filter); //�ٶȻ����� PI 23 | if(flag1==1) 24 | { 25 | positions=0; 26 | } 27 | 28 | return output; 29 | } 30 | //////////////////�ٶ�PI//////////////////// 31 | 32 | float BalanceCar::turnspin(int turnleftflag,int turnrightflag,int spinleftflag,int spinrightflag,double kpturn,double kdturn,float Gyroz) 33 | { 34 | int spinonce = 0; 35 | float turnspeed = 0; 36 | float rotationratio = 0; 37 | float turnout_put = 0; 38 | 39 | if (turnleftflag == 1 || turnrightflag == 1 || spinleftflag == 1 || spinrightflag == 1) 40 | { 41 | if (spinonce == 0) //��תǰ�жϵ�ǰ���٣���ǿС����Ӧ�ԡ� 42 | { 43 | turnspeed = ( pulseright + pulseleft); //С����ǰ���� �����ʾ 44 | spinonce++; 45 | } 46 | 47 | if (turnspeed < 0) //С����ǰ�ٶȾ���ֵ 48 | { 49 | turnspeed = -turnspeed; 50 | 51 | } 52 | if(turnleftflag==1||turnrightflag==1) 53 | { 54 | turnmax=3; 55 | turnmin=-3; 56 | } 57 | if(spinleftflag==1||spinrightflag==1) 58 | { 59 | turnmax=10; 60 | turnmin=-10; 61 | } 62 | rotationratio = 5 / turnspeed; //����С���ٶ��趨ֵ 63 | if (rotationratio < 0.5)rotationratio = 0.5; 64 | if (rotationratio > 5)rotationratio = 5; 65 | } 66 | else 67 | { 68 | rotationratio = 0.5; 69 | spinonce = 0; 70 | turnspeed = 0; 71 | } 72 | if (turnleftflag == 1 || spinleftflag == 1)//���ݷ���������� 73 | { 74 | turnout += rotationratio; 75 | } 76 | else if (turnrightflag == 1 || spinrightflag == 1)//���ݷ���������� 77 | { 78 | turnout -= rotationratio; 79 | } 80 | else turnout = 0; 81 | if (turnout > turnmax) turnout = turnmax;//��ֵ���ֵ���� 82 | if (turnout < turnmin) turnout = turnmin;//��ֵ��Сֵ���� 83 | 84 | turnout_put = -turnout * kpturn - Gyroz * kdturn;//��תPD�㷨���� �ں��ٶȺ�Z����ת��λ�� 85 | return turnout_put; 86 | } 87 | 88 | void BalanceCar::pwma(double speedoutput,float rotationoutput,float angle,float angle6,int turnleftflag,int turnrightflag,int spinleftflag,int spinrightflag, 89 | int f,int b,float accelz,int Pin1,int Pin2,int Pin3,int Pin4,int PinPWMA,int PinPWMB) 90 | { 91 | 92 | pwm1 = -angleoutput - speedoutput - rotationoutput; //Left���PWM���ֵ 93 | pwm2 = -angleoutput - speedoutput + rotationoutput;//Right���PWM���ֵ 94 | 95 | //����ֵ���� 96 | if (pwm1 > 255) pwm1 = 255; 97 | if (pwm1 < -255) pwm1 = -255; 98 | if (pwm2 > 255) pwm2 = 255; 99 | if (pwm2 < -255) pwm2 = -255; 100 | 101 | 102 | //�Ƕȹ���ֹͣ��� 103 | if (angle > 30 || angle < -30) 104 | { 105 | pwm1 = 0; 106 | pwm2 = 0; 107 | } 108 | // 109 | 110 | 111 | 112 | 113 | if (angle6 > 10 || angle6 < -10 &turnleftflag == 0 & turnrightflag == 0 & spinleftflag == 0 & spinrightflag == 0 && f == 0 && b == 0) 114 | { 115 | if(stopl + stopr > 1500||stopl + stopr < -3500) 116 | { 117 | pwm1 = 0; 118 | pwm2 = 0; 119 | flag1=1; 120 | 121 | } 122 | } 123 | else 124 | { 125 | stopl=stopr=0; 126 | flag1=0; 127 | 128 | } 129 | 130 | 131 | 132 | //�������������ж� �����ж� 133 | if (pwm1 >= 0) { 134 | digitalWrite(Pin2, 0); 135 | digitalWrite(Pin1, 1); 136 | analogWrite(PinPWMA, pwm1); 137 | } else { 138 | digitalWrite(Pin2, 1); 139 | digitalWrite(Pin1, 0); 140 | analogWrite(PinPWMA, -pwm1); 141 | } 142 | //�������������ж� �ҵ���ж� 143 | if (pwm2 >= 0) { 144 | digitalWrite(Pin4, 0); 145 | digitalWrite(Pin3, 1); 146 | analogWrite(PinPWMB, pwm2); 147 | } else { 148 | digitalWrite(Pin4, 1); 149 | digitalWrite(Pin3, 0); 150 | analogWrite(PinPWMB, -pwm2); 151 | } 152 | 153 | } 154 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/BalanceCar.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/3.Arduino balance car code_2019_2_14/bst_abc/BalanceCar.h -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/I2Cdev.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - Main I2C device class header file 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 6/9/2012 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 7 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 8 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 9 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 10 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 11 | // 2011-10-03 - added automatic Arduino version detection for ease of use 12 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 13 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 14 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 15 | // 2011-08-02 - added support for 16-bit registers 16 | // - fixed incorrect Doxygen comments on some methods 17 | // - added timeout value for read operations (thanks mem @ Arduino forums) 18 | // 2011-07-30 - changed read/write function structures to return success or byte counts 19 | // - made all methods static for multi-device memory savings 20 | // 2011-07-28 - initial release 21 | 22 | /* ============================================ 23 | I2Cdev device library code is placed under the MIT license 24 | Copyright (c) 2013 Jeff Rowberg 25 | 26 | Permission is hereby granted, free of charge, to any person obtaining a copy 27 | of this software and associated documentation files (the "Software"), to deal 28 | in the Software without restriction, including without limitation the rights 29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 30 | copies of the Software, and to permit persons to whom the Software is 31 | furnished to do so, subject to the following conditions: 32 | 33 | The above copyright notice and this permission notice shall be included in 34 | all copies or substantial portions of the Software. 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 38 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 39 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 40 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 41 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 42 | THE SOFTWARE. 43 | =============================================== 44 | */ 45 | 46 | #ifndef _I2CDEV_H_ 47 | #define _I2CDEV_H_ 48 | 49 | // ----------------------------------------------------------------------------- 50 | // I2C interface implementation setting 51 | // ----------------------------------------------------------------------------- 52 | #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE 53 | //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE 54 | 55 | // comment this out if you are using a non-optimal IDE/implementation setting 56 | // but want the compiler to shut up about it 57 | #define I2CDEV_IMPLEMENTATION_WARNINGS 58 | 59 | // ----------------------------------------------------------------------------- 60 | // I2C interface implementation options 61 | // ----------------------------------------------------------------------------- 62 | #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino 63 | #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project 64 | // ^^^ NBWire implementation is still buggy w/some interrupts! 65 | #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project 66 | #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library 67 | 68 | // ----------------------------------------------------------------------------- 69 | // Arduino-style "Serial.print" debug constant (uncomment to enable) 70 | // ----------------------------------------------------------------------------- 71 | //#define I2CDEV_SERIAL_DEBUG 72 | 73 | #ifdef ARDUINO 74 | #if ARDUINO < 100 75 | #include "WProgram.h" 76 | #else 77 | #include "Arduino.h" 78 | #endif 79 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 80 | #include 81 | #endif 82 | #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY 83 | #include 84 | #endif 85 | #endif 86 | 87 | // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") 88 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000 89 | 90 | class I2Cdev { 91 | public: 92 | I2Cdev(); 93 | 94 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 95 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 96 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 97 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 98 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 99 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 100 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 101 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 102 | 103 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 104 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 105 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 106 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 107 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 108 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 109 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 110 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 111 | 112 | static uint16_t readTimeout; 113 | }; 114 | 115 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 116 | ////////////////////// 117 | // FastWire 0.24 118 | // This is a library to help faster programs to read I2C devices. 119 | // Copyright(C) 2012 120 | // Francesco Ferrara 121 | ////////////////////// 122 | 123 | /* Master */ 124 | #define TW_START 0x08 125 | #define TW_REP_START 0x10 126 | 127 | /* Master Transmitter */ 128 | #define TW_MT_SLA_ACK 0x18 129 | #define TW_MT_SLA_NACK 0x20 130 | #define TW_MT_DATA_ACK 0x28 131 | #define TW_MT_DATA_NACK 0x30 132 | #define TW_MT_ARB_LOST 0x38 133 | 134 | /* Master Receiver */ 135 | #define TW_MR_ARB_LOST 0x38 136 | #define TW_MR_SLA_ACK 0x40 137 | #define TW_MR_SLA_NACK 0x48 138 | #define TW_MR_DATA_ACK 0x50 139 | #define TW_MR_DATA_NACK 0x58 140 | 141 | #define TW_OK 0 142 | #define TW_ERROR 1 143 | 144 | class Fastwire { 145 | private: 146 | static boolean waitInt(); 147 | 148 | public: 149 | static void setup(int khz, boolean pullup); 150 | static byte beginTransmission(byte device); 151 | static byte write(byte value); 152 | static byte writeBuf(byte device, byte address, byte *data, byte num); 153 | static byte readBuf(byte device, byte address, byte *data, byte num); 154 | static void reset(); 155 | static byte stop(); 156 | }; 157 | #endif 158 | 159 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 160 | // NBWire implementation based heavily on code by Gene Knight 161 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 162 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 163 | 164 | #define NBWIRE_BUFFER_LENGTH 32 165 | 166 | class TwoWire { 167 | private: 168 | static uint8_t rxBuffer[]; 169 | static uint8_t rxBufferIndex; 170 | static uint8_t rxBufferLength; 171 | 172 | static uint8_t txAddress; 173 | static uint8_t txBuffer[]; 174 | static uint8_t txBufferIndex; 175 | static uint8_t txBufferLength; 176 | 177 | // static uint8_t transmitting; 178 | static void (*user_onRequest)(void); 179 | static void (*user_onReceive)(int); 180 | static void onRequestService(void); 181 | static void onReceiveService(uint8_t*, int); 182 | 183 | public: 184 | TwoWire(); 185 | void begin(); 186 | void begin(uint8_t); 187 | void begin(int); 188 | void beginTransmission(uint8_t); 189 | //void beginTransmission(int); 190 | uint8_t endTransmission(uint16_t timeout=0); 191 | void nbendTransmission(void (*function)(int)) ; 192 | uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); 193 | //uint8_t requestFrom(int, int); 194 | void nbrequestFrom(uint8_t, int, void (*function)(int)); 195 | void send(uint8_t); 196 | void send(uint8_t*, uint8_t); 197 | //void send(int); 198 | void send(char*); 199 | uint8_t available(void); 200 | uint8_t receive(void); 201 | void onReceive(void (*)(int)); 202 | void onRequest(void (*)(void)); 203 | }; 204 | 205 | #define TWI_READY 0 206 | #define TWI_MRX 1 207 | #define TWI_MTX 2 208 | #define TWI_SRX 3 209 | #define TWI_STX 4 210 | 211 | #define TW_WRITE 0 212 | #define TW_READ 1 213 | 214 | #define TW_MT_SLA_NACK 0x20 215 | #define TW_MT_DATA_NACK 0x30 216 | 217 | #define CPU_FREQ 16000000L 218 | #define TWI_FREQ 100000L 219 | #define TWI_BUFFER_LENGTH 32 220 | 221 | /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ 222 | 223 | #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) 224 | #define TW_STATUS (TWSR & TW_STATUS_MASK) 225 | #define TW_START 0x08 226 | #define TW_REP_START 0x10 227 | #define TW_MT_SLA_ACK 0x18 228 | #define TW_MT_SLA_NACK 0x20 229 | #define TW_MT_DATA_ACK 0x28 230 | #define TW_MT_DATA_NACK 0x30 231 | #define TW_MT_ARB_LOST 0x38 232 | #define TW_MR_ARB_LOST 0x38 233 | #define TW_MR_SLA_ACK 0x40 234 | #define TW_MR_SLA_NACK 0x48 235 | #define TW_MR_DATA_ACK 0x50 236 | #define TW_MR_DATA_NACK 0x58 237 | #define TW_ST_SLA_ACK 0xA8 238 | #define TW_ST_ARB_LOST_SLA_ACK 0xB0 239 | #define TW_ST_DATA_ACK 0xB8 240 | #define TW_ST_DATA_NACK 0xC0 241 | #define TW_ST_LAST_DATA 0xC8 242 | #define TW_SR_SLA_ACK 0x60 243 | #define TW_SR_ARB_LOST_SLA_ACK 0x68 244 | #define TW_SR_GCALL_ACK 0x70 245 | #define TW_SR_ARB_LOST_GCALL_ACK 0x78 246 | #define TW_SR_DATA_ACK 0x80 247 | #define TW_SR_DATA_NACK 0x88 248 | #define TW_SR_GCALL_DATA_ACK 0x90 249 | #define TW_SR_GCALL_DATA_NACK 0x98 250 | #define TW_SR_STOP 0xA0 251 | #define TW_NO_INFO 0xF8 252 | #define TW_BUS_ERROR 0x00 253 | 254 | //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) 255 | //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) 256 | 257 | #ifndef sbi // set bit 258 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 259 | #endif // sbi 260 | 261 | #ifndef cbi // clear bit 262 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 263 | #endif // cbi 264 | 265 | extern TwoWire Wire; 266 | 267 | #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 268 | 269 | #endif /* _I2CDEV_H_ */ 270 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/JJ_MPU6050_DMP_6Axis.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 5/20/2013 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Updated by Jose Julio 09/17/2013 7 | // This code is based on MPU6050_6Axis_MotionApps20.h file 8 | // - Reduced FIFO packet size (18 bytes 9 | // - Optimized DMP initialization (less memory) 10 | 11 | /* ============================================ 12 | I2Cdev device library code is placed under the MIT license 13 | Copyright (c) 2012 Jeff Rowberg 14 | 15 | Permission is hereby granted, free of charge, to any person obtaining a copy 16 | of this software and associated documentation files (the "Software"), to deal 17 | in the Software without restriction, including without limitation the rights 18 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | copies of the Software, and to permit persons to whom the Software is 20 | furnished to do so, subject to the following conditions: 21 | 22 | The above copyright notice and this permission notice shall be included in 23 | all copies or substantial portions of the Software. 24 | 25 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 26 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 27 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 28 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 29 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 30 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 31 | THE SOFTWARE. 32 | =============================================== 33 | */ 34 | 35 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ 36 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ 37 | 38 | #include "I2Cdev.h" 39 | #include "helper_3dmath.h" 40 | 41 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board 42 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 43 | 44 | #include "MPU6050.h" 45 | 46 | // Tom Carpenter's conditional PROGMEM code 47 | // http://forum.arduino.cc/index.php?topic=129407.0 48 | #ifndef __arm__ 49 | #include 50 | #else 51 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 52 | #ifndef __PGMSPACE_H_ 53 | #define __PGMSPACE_H_ 1 54 | #include 55 | 56 | #define PROGMEM 57 | #define PGM_P const char * 58 | #define PSTR(str) (str) 59 | #define F(x) x 60 | 61 | typedef void prog_void; 62 | typedef char prog_char; 63 | typedef unsigned char prog_uchar; 64 | typedef int8_t prog_int8_t; 65 | typedef uint8_t prog_uint8_t; 66 | typedef int16_t prog_int16_t; 67 | typedef uint16_t prog_uint16_t; 68 | typedef int32_t prog_int32_t; 69 | typedef uint32_t prog_uint32_t; 70 | 71 | #define strcpy_P(dest, src) strcpy((dest), (src)) 72 | #define strcat_P(dest, src) strcat((dest), (src)) 73 | #define strcmp_P(a, b) strcmp((a), (b)) 74 | 75 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 76 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 77 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 78 | #define pgm_read_float(addr) (*(const float *)(addr)) 79 | 80 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 81 | #define pgm_read_word_near(addr) pgm_read_word(addr) 82 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 83 | #define pgm_read_float_near(addr) pgm_read_float(addr) 84 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 85 | #define pgm_read_word_far(addr) pgm_read_word(addr) 86 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 87 | #define pgm_read_float_far(addr) pgm_read_float(addr) 88 | #endif 89 | #endif 90 | 91 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 92 | * unavailable, unless you happen to be amazing as decompiling binary by 93 | * hand (in which case, please contact me, and I'm totally serious). 94 | * 95 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 96 | * DMP reverse-engineering he did to help make this bit of wizardry 97 | * possible. 98 | */ 99 | 100 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 101 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 102 | // after moving string constants to flash memory storage using the F() 103 | // compiler macro (Arduino IDE 1.0+ required). 104 | 105 | //#define DEBUG 106 | #ifdef DEBUG 107 | #define DEBUG_PRINT(x) Serial.print(x) 108 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 109 | #define DEBUG_PRINTLN(x) Serial.println(x) 110 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 111 | #else 112 | #define DEBUG_PRINT(x) 113 | #define DEBUG_PRINTF(x, y) 114 | #define DEBUG_PRINTLN(x) 115 | #define DEBUG_PRINTLNF(x, y) 116 | #endif 117 | 118 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 119 | #define MPU6050_DMP_CONFIG_SIZE 111 // 174 //192 // dmpConfig[] 120 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 121 | 122 | /* ================================================================================================ * 123 | | 18-byte FIFO packet structure: | 124 | | | 125 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][FOOTER] | 126 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 | 127 | | | 128 | * ================================================================================================ */ 129 | 130 | // this block of memory gets written to the MPU on start-up, and it seems 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 132 | // second though) 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 134 | // bank 0, 256 bytes 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 151 | 152 | // bank 1, 256 bytes 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 169 | 170 | // bank 2, 256 bytes 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 187 | 188 | // bank 3, 256 bytes 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 205 | 206 | // bank 4, 256 bytes 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 223 | 224 | // bank 5, 256 bytes 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 241 | 242 | // bank 6, 256 bytes 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 259 | 260 | // bank 7, 138 bytes (remainder) 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 270 | }; 271 | 272 | // thanks to Noah Zerkin for piecing this stuff together! 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 274 | // BANK OFFSET LENGTH [DATA] 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 283 | //0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 284 | //0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 285 | //0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 286 | //0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 287 | //0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 288 | //0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 289 | //0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 290 | //0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 291 | //0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 295 | //0x04, 0x02, 0x03, 0x98, 0x98, 0x98, // CFG_MOTION_BIAS inv_turn_off bias correction 296 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 297 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 298 | // SPECIAL 0x01 = enable interrupts 299 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 300 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 301 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 302 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 303 | //0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 304 | //0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 305 | //0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 306 | 0x02, 0x16, 0x02, 0x00, 0x00 // D_0_22 inv_set_fifo_rate // Original 0x01 307 | 308 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 309 | 310 | // It is important to make sure the host processor can keep up with reading and processing 311 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 312 | }; 313 | 314 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 315 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 316 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 317 | 0x01, 0x6A, 0x02, 0x06, 0x00, 318 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 319 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 320 | 0x01, 0x62, 0x02, 0x00, 0x00, 321 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 322 | }; 323 | 324 | uint8_t MPU6050::dmpInitialize() { 325 | // reset device 326 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 327 | reset(); 328 | delay(30); // wait after reset 329 | 330 | // enable sleep mode and wake cycle 331 | /*Serial.println(F("Enabling sleep mode...")); 332 | setSleepEnabled(true); 333 | Serial.println(F("Enabling wake cycle...")); 334 | setWakeCycleEnabled(true);*/ 335 | 336 | // disable sleep mode 337 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 338 | setSleepEnabled(false); 339 | 340 | // get MPU hardware revision 341 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 342 | setMemoryBank(0x10, true, true); 343 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 344 | setMemoryStartAddress(0x06); 345 | DEBUG_PRINTLN(F("Checking hardware revision...")); 346 | uint8_t hwRevision = readMemoryByte(); 347 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 348 | DEBUG_PRINTLNF(hwRevision, HEX); 349 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 350 | setMemoryBank(0, false, false); 351 | 352 | // check OTP bank valid 353 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 354 | uint8_t otpValid = getOTPBankValid(); 355 | DEBUG_PRINT(F("OTP bank is ")); 356 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 357 | 358 | // get X/Y/Z gyro offsets 359 | DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 360 | int8_t xgOffsetTC = getXGyroOffsetTC(); 361 | int8_t ygOffsetTC = getYGyroOffsetTC(); 362 | int8_t zgOffsetTC = getZGyroOffsetTC(); 363 | DEBUG_PRINT(F("X gyro offset = ")); 364 | DEBUG_PRINTLN(xgOffset); 365 | DEBUG_PRINT(F("Y gyro offset = ")); 366 | DEBUG_PRINTLN(ygOffset); 367 | DEBUG_PRINT(F("Z gyro offset = ")); 368 | DEBUG_PRINTLN(zgOffset); 369 | 370 | // setup weird slave stuff (?) 371 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 372 | setSlaveAddress(0, 0x7F); 373 | DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 374 | setI2CMasterModeEnabled(false); 375 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 376 | setSlaveAddress(0, 0x68); 377 | DEBUG_PRINTLN(F("Resetting I2C Master control...")); 378 | resetI2CMaster(); 379 | delay(20); 380 | 381 | // load DMP code into memory banks 382 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 383 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 384 | DEBUG_PRINTLN(F(" bytes)")); 385 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 386 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 387 | 388 | // write DMP configuration 389 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 390 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 391 | DEBUG_PRINTLN(F(" bytes in config def)")); 392 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 393 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 394 | 395 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 396 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 397 | 398 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 399 | setIntEnabled(0x12); 400 | 401 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 402 | setRate(4); // 1khz / (1 + 4) = 200 Hz 403 | 404 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 405 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 406 | 407 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 408 | setDLPFMode(MPU6050_DLPF_BW_42); 409 | 410 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 411 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 412 | 413 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 414 | setDMPConfig1(0x03); 415 | setDMPConfig2(0x00); 416 | 417 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 418 | setOTPBankValid(false); 419 | 420 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 421 | setXGyroOffsetTC(xgOffsetTC); 422 | setYGyroOffsetTC(ygOffsetTC); 423 | setZGyroOffsetTC(zgOffsetTC); 424 | 425 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 426 | //setXGyroOffset(0); 427 | //setYGyroOffset(0); 428 | //setZGyroOffset(0); 429 | 430 | DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 431 | uint8_t dmpUpdate[16], j; 432 | uint16_t pos = 0; 433 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 434 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 435 | 436 | DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 437 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 438 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 439 | 440 | DEBUG_PRINTLN(F("Resetting FIFO...")); 441 | resetFIFO(); 442 | 443 | DEBUG_PRINTLN(F("Reading FIFO count...")); 444 | uint16_t fifoCount = getFIFOCount(); 445 | uint8_t fifoBuffer[128]; 446 | 447 | DEBUG_PRINT(F("Current FIFO count=")); 448 | DEBUG_PRINTLN(fifoCount); 449 | getFIFOBytes(fifoBuffer, fifoCount); 450 | 451 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 452 | setMotionDetectionThreshold(2); 453 | 454 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 455 | setZeroMotionDetectionThreshold(156); 456 | 457 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 458 | setMotionDetectionDuration(80); 459 | 460 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 461 | setZeroMotionDetectionDuration(0); 462 | 463 | DEBUG_PRINTLN(F("Resetting FIFO...")); 464 | resetFIFO(); 465 | 466 | DEBUG_PRINTLN(F("Enabling FIFO...")); 467 | setFIFOEnabled(true); 468 | 469 | DEBUG_PRINTLN(F("Enabling DMP...")); 470 | setDMPEnabled(true); 471 | 472 | DEBUG_PRINTLN(F("Resetting DMP...")); 473 | resetDMP(); 474 | 475 | DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 476 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 477 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 478 | 479 | DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 480 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 481 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 482 | 483 | DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 484 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 485 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 486 | 487 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 488 | while ((fifoCount = getFIFOCount()) < 3); 489 | 490 | DEBUG_PRINT(F("Current FIFO count=")); 491 | DEBUG_PRINTLN(fifoCount); 492 | DEBUG_PRINTLN(F("Reading FIFO data...")); 493 | getFIFOBytes(fifoBuffer, fifoCount); 494 | 495 | DEBUG_PRINTLN(F("Reading interrupt status...")); 496 | uint8_t mpuIntStatus = getIntStatus(); 497 | 498 | DEBUG_PRINT(F("Current interrupt status=")); 499 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 500 | 501 | DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 502 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 503 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 504 | 505 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 506 | while ((fifoCount = getFIFOCount()) < 3); 507 | 508 | DEBUG_PRINT(F("Current FIFO count=")); 509 | DEBUG_PRINTLN(fifoCount); 510 | 511 | DEBUG_PRINTLN(F("Reading FIFO data...")); 512 | getFIFOBytes(fifoBuffer, fifoCount); 513 | 514 | DEBUG_PRINTLN(F("Reading interrupt status...")); 515 | mpuIntStatus = getIntStatus(); 516 | 517 | DEBUG_PRINT(F("Current interrupt status=")); 518 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 519 | 520 | DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 521 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 522 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 523 | 524 | DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 525 | 526 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 527 | setDMPEnabled(false); 528 | 529 | DEBUG_PRINTLN(F("Setting up internal 18-byte (default) DMP packet buffer...")); 530 | dmpPacketSize = 18; // original 42 bytes; 531 | 532 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 533 | resetFIFO(); 534 | getIntStatus(); 535 | } else { 536 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 537 | return 2; // configuration block loading failed 538 | } 539 | } else { 540 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 541 | return 1; // main binary block loading failed 542 | } 543 | return 0; // success 544 | } 545 | 546 | bool MPU6050::dmpPacketAvailable() { 547 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 548 | } 549 | 550 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 551 | // uint8_t MPU6050::dmpGetFIFORate(); 552 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 553 | // uint8_t MPU6050::dmpGetSampleFrequency(); 554 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 555 | 556 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 557 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 558 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 559 | 560 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 561 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 562 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 563 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 564 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 565 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 566 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 567 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 568 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 569 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 570 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 571 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 572 | 573 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 574 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 575 | if (packet == 0) packet = dmpPacketBuffer; 576 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 577 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 578 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 579 | return 0; 580 | } 581 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 582 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 583 | if (packet == 0) packet = dmpPacketBuffer; 584 | data[0] = (packet[28] << 8) + packet[29]; 585 | data[1] = (packet[32] << 8) + packet[33]; 586 | data[2] = (packet[36] << 8) + packet[37]; 587 | return 0; 588 | } 589 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 590 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 591 | if (packet == 0) packet = dmpPacketBuffer; 592 | v -> x = (packet[28] << 8) + packet[29]; 593 | v -> y = (packet[32] << 8) + packet[33]; 594 | v -> z = (packet[36] << 8) + packet[37]; 595 | return 0; 596 | } 597 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 598 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 599 | if (packet == 0) packet = dmpPacketBuffer; 600 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 601 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 602 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 603 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 604 | return 0; 605 | } 606 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 607 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 608 | if (packet == 0) packet = dmpPacketBuffer; 609 | data[0] = ((packet[0] << 8) + packet[1]); 610 | data[1] = ((packet[4] << 8) + packet[5]); 611 | data[2] = ((packet[8] << 8) + packet[9]); 612 | data[3] = ((packet[12] << 8) + packet[13]); 613 | return 0; 614 | } 615 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 616 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 617 | int16_t qI[4]; 618 | uint8_t status = dmpGetQuaternion(qI, packet); 619 | if (status == 0) { 620 | q -> w = (float)qI[0] / 16384.0f; 621 | q -> x = (float)qI[1] / 16384.0f; 622 | q -> y = (float)qI[2] / 16384.0f; 623 | q -> z = (float)qI[3] / 16384.0f; 624 | return 0; 625 | } 626 | return status; // int16 return value, indicates error if this line is reached 627 | } 628 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 629 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 630 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 631 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 632 | if (packet == 0) packet = dmpPacketBuffer; 633 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 634 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 635 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 636 | return 0; 637 | } 638 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 639 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 640 | if (packet == 0) packet = dmpPacketBuffer; 641 | data[0] = (packet[16] << 8) + packet[17]; 642 | data[1] = (packet[20] << 8) + packet[21]; 643 | data[2] = (packet[24] << 8) + packet[25]; 644 | return 0; 645 | } 646 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 647 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 648 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 649 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 650 | v -> x = vRaw -> x - gravity -> x*8192; 651 | v -> y = vRaw -> y - gravity -> y*8192; 652 | v -> z = vRaw -> z - gravity -> z*8192; 653 | return 0; 654 | } 655 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 656 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 657 | // rotate measured 3D acceleration vector into original state 658 | // frame of reference based on orientation quaternion 659 | memcpy(v, vReal, sizeof(VectorInt16)); 660 | v -> rotate(q); 661 | return 0; 662 | } 663 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 664 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 665 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 666 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 667 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 668 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 669 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 670 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 671 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 672 | return 0; 673 | } 674 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 675 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 676 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 677 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 678 | 679 | 680 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 681 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi 682 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 683 | data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi 684 | return 0; 685 | } 686 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 687 | // yaw: (about Z axis) 688 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 689 | // pitch: (nose up/down, about Y axis) 690 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 691 | // roll: (tilt left/right, about X axis) 692 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 693 | return 0; 694 | } 695 | 696 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 697 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 698 | 699 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 700 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 701 | if (dmpData[k] < 0x10) Serial.print("0"); 702 | Serial.print(dmpData[k], HEX); 703 | Serial.print(" "); 704 | } 705 | Serial.print("\n");*/ 706 | //Serial.println((uint16_t)dmpPacketBuffer); 707 | return 0; 708 | } 709 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 710 | uint8_t status; 711 | uint8_t buf[dmpPacketSize]; 712 | for (uint8_t i = 0; i < numPackets; i++) { 713 | // read packet from FIFO 714 | getFIFOBytes(buf, dmpPacketSize); 715 | 716 | // process packet 717 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 718 | 719 | // increment external process count variable, if supplied 720 | if (processed != 0) *processed++; 721 | } 722 | return 0; 723 | } 724 | 725 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 726 | 727 | // uint8_t MPU6050::dmpInitFIFOParam(); 728 | // uint8_t MPU6050::dmpCloseFIFO(); 729 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 730 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 731 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 732 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 733 | // void MPU6050::dmpOverrideQuaternion(long *q); 734 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 735 | return dmpPacketSize; 736 | } 737 | 738 | 739 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 740 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/KalmanFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | KalmanFilter.h KalmanFilter.cpp - Library for BST-Balance car code. 3 | Created by SKY ZHU&ROOMS LUO, OCTOBER 2, 2016. 4 | JUST FOR THE Company of Technology of yahboom. 5 | In order to avoid Infringement Act,this core is not for the commerce except being authorized by the writer. 6 | */ 7 | 8 | #include "./KalmanFilter.h" 9 | 10 | //////////////////////////yijielvbo//////////////////// 11 | void KalmanFilter::Yiorderfilter(float angle_m, float gyro_m,float dt,float K1) 12 | { 13 | angle6 = K1 * angle_m + (1 - K1) * (angle6 + gyro_m * dt); 14 | // return angle6; 15 | } 16 | 17 | 18 | ////////////////////////kalman///////////////////////// 19 | 20 | void KalmanFilter::Kalman_Filter(double angle_m, double gyro_m,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0) 21 | { 22 | angle += (gyro_m - q_bias) * dt; 23 | angle_err = angle_m - angle; 24 | Pdot[0] = Q_angle - P[0][1] - P[1][0]; 25 | Pdot[1] = - P[1][1]; 26 | Pdot[2] = - P[1][1]; 27 | Pdot[3] = Q_gyro; 28 | P[0][0] += Pdot[0] * dt; 29 | P[0][1] += Pdot[1] * dt; 30 | P[1][0] += Pdot[2] * dt; 31 | P[1][1] += Pdot[3] * dt; 32 | PCt_0 = C_0 * P[0][0]; 33 | PCt_1 = C_0 * P[1][0]; 34 | E = R_angle + C_0 * PCt_0; 35 | K_0 = PCt_0 / E; 36 | K_1 = PCt_1 / E; 37 | t_0 = PCt_0; 38 | t_1 = C_0 * P[0][1]; 39 | P[0][0] -= K_0 * t_0; 40 | P[0][1] -= K_0 * t_1; 41 | P[1][0] -= K_1 * t_0; 42 | P[1][1] -= K_1 * t_1; 43 | angle += K_0 * angle_err; //���ŽǶ� 44 | q_bias += K_1 * angle_err; 45 | angle_dot = gyro_m - q_bias; //���Ž��ٶ� 46 | } 47 | 48 | ////////////////////////kalman///////////////////////// 49 | 50 | 51 | ///////////////////////////// Angle test///////////////////////////////// 52 | void KalmanFilter::Angletest(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro, 53 | float R_angle,float C_0,float K1) 54 | { 55 | // int flag; 56 | //ƽ����� 57 | float Angle = atan2(ay , az) * 57.3; //�Ƕȼ��㹫ʽ,Angle:һ�׻����˲��������С��������б�Ƕ� 58 | Gyro_x = (gx - 128.1) / 131; //�Ƕ�ת�� 59 | Kalman_Filter(Angle, Gyro_x, dt, Q_angle, Q_gyro,R_angle,C_0); //�����˲� 60 | //��ת�Ƕ�Z����� 61 | if (gz > 32768) gz -= 65536; //ǿ��ת��2g 1g 62 | Gyro_z = -gz / 131; //Z�����ת�� 63 | accelz = az / 16.4; 64 | 65 | float angleAx = atan2(ax, az) * 180 / PI; //������x��н� 66 | Gyro_y = -gy / 131.00; //������ٶ� 67 | Yiorderfilter(angleAx, Gyro_y, dt, K1); //һ���˲� 68 | 69 | } 70 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/KalmanFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | KalmanFilter.h KalmanFilter.cpp - Library for BST-Balance car code. 3 | Created by SKY ZHU&ROOMS LUO, OCTOBER 2, 2016. 4 | JUST FOR THE Company of Technology of yahboom. 5 | In order to avoid Infringement Act,this core is not for the commerce except being authorized by the writer. 6 | */ 7 | 8 | #ifndef KalmanFilter_h 9 | #define KalmanFilter_h 10 | 11 | #if defined(ARDUINO) && (ARDUINO >= 100) 12 | #include 13 | #else 14 | #include 15 | #endif 16 | 17 | 18 | class KalmanFilter 19 | { 20 | public: 21 | void Yiorderfilter(float angle_m, float gyro_m,float dt,float K1); 22 | void Kalman_Filter(double angle_m, double gyro_m,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0); 23 | void Angletest(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro, 24 | float R_angle,float C_0,float K1); 25 | float Gyro_x,Gyro_y,Gyro_z; 26 | float accelz = 0; 27 | float angle; 28 | float angle6; 29 | private: 30 | float angle_err,q_bias; 31 | float Pdot[4] = { 0, 0, 0, 0}; 32 | float P[2][2] = {{ 1, 0 }, { 0, 1 }}; 33 | float PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; 34 | float angle_dot; 35 | 36 | }; 37 | #endif 38 | // 39 | // END OF FILE 40 | // -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/MPU6050.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 10/3/2011 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE 10 | // DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF 11 | // YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. 12 | 13 | /* ============================================ 14 | I2Cdev device library code is placed under the MIT license 15 | Copyright (c) 2012 Jeff Rowberg 16 | 17 | Permission is hereby granted, free of charge, to any person obtaining a copy 18 | of this software and associated documentation files (the "Software"), to deal 19 | in the Software without restriction, including without limitation the rights 20 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 21 | copies of the Software, and to permit persons to whom the Software is 22 | furnished to do so, subject to the following conditions: 23 | 24 | The above copyright notice and this permission notice shall be included in 25 | all copies or substantial portions of the Software. 26 | 27 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 29 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 30 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 31 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 32 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 33 | THE SOFTWARE. 34 | =============================================== 35 | */ 36 | 37 | #ifndef _MPU6050_H_ 38 | #define _MPU6050_H_ 39 | 40 | #include "I2Cdev.h" 41 | //#include 42 | 43 | 44 | 45 | #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board 46 | #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) 47 | #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW 48 | 49 | #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 50 | #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 51 | #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 52 | #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN 53 | #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN 54 | #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN 55 | #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS 56 | #define MPU6050_RA_XA_OFFS_L_TC 0x07 57 | #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS 58 | #define MPU6050_RA_YA_OFFS_L_TC 0x09 59 | #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS 60 | #define MPU6050_RA_ZA_OFFS_L_TC 0x0B 61 | #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR 62 | #define MPU6050_RA_XG_OFFS_USRL 0x14 63 | #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR 64 | #define MPU6050_RA_YG_OFFS_USRL 0x16 65 | #define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR 66 | #define MPU6050_RA_ZG_OFFS_USRL 0x18 67 | #define MPU6050_RA_SMPLRT_DIV 0x19 68 | #define MPU6050_RA_CONFIG 0x1A 69 | #define MPU6050_RA_GYRO_CONFIG 0x1B 70 | #define MPU6050_RA_ACCEL_CONFIG 0x1C 71 | #define MPU6050_RA_FF_THR 0x1D 72 | #define MPU6050_RA_FF_DUR 0x1E 73 | #define MPU6050_RA_MOT_THR 0x1F 74 | #define MPU6050_RA_MOT_DUR 0x20 75 | #define MPU6050_RA_ZRMOT_THR 0x21 76 | #define MPU6050_RA_ZRMOT_DUR 0x22 77 | #define MPU6050_RA_FIFO_EN 0x23 78 | #define MPU6050_RA_I2C_MST_CTRL 0x24 79 | #define MPU6050_RA_I2C_SLV0_ADDR 0x25 80 | #define MPU6050_RA_I2C_SLV0_REG 0x26 81 | #define MPU6050_RA_I2C_SLV0_CTRL 0x27 82 | #define MPU6050_RA_I2C_SLV1_ADDR 0x28 83 | #define MPU6050_RA_I2C_SLV1_REG 0x29 84 | #define MPU6050_RA_I2C_SLV1_CTRL 0x2A 85 | #define MPU6050_RA_I2C_SLV2_ADDR 0x2B 86 | #define MPU6050_RA_I2C_SLV2_REG 0x2C 87 | #define MPU6050_RA_I2C_SLV2_CTRL 0x2D 88 | #define MPU6050_RA_I2C_SLV3_ADDR 0x2E 89 | #define MPU6050_RA_I2C_SLV3_REG 0x2F 90 | #define MPU6050_RA_I2C_SLV3_CTRL 0x30 91 | #define MPU6050_RA_I2C_SLV4_ADDR 0x31 92 | #define MPU6050_RA_I2C_SLV4_REG 0x32 93 | #define MPU6050_RA_I2C_SLV4_DO 0x33 94 | #define MPU6050_RA_I2C_SLV4_CTRL 0x34 95 | #define MPU6050_RA_I2C_SLV4_DI 0x35 96 | #define MPU6050_RA_I2C_MST_STATUS 0x36 97 | #define MPU6050_RA_INT_PIN_CFG 0x37 98 | #define MPU6050_RA_INT_ENABLE 0x38 99 | #define MPU6050_RA_DMP_INT_STATUS 0x39 100 | #define MPU6050_RA_INT_STATUS 0x3A 101 | #define MPU6050_RA_ACCEL_XOUT_H 0x3B 102 | #define MPU6050_RA_ACCEL_XOUT_L 0x3C 103 | #define MPU6050_RA_ACCEL_YOUT_H 0x3D 104 | #define MPU6050_RA_ACCEL_YOUT_L 0x3E 105 | #define MPU6050_RA_ACCEL_ZOUT_H 0x3F 106 | #define MPU6050_RA_ACCEL_ZOUT_L 0x40 107 | #define MPU6050_RA_TEMP_OUT_H 0x41 108 | #define MPU6050_RA_TEMP_OUT_L 0x42 109 | #define MPU6050_RA_GYRO_XOUT_H 0x43 110 | #define MPU6050_RA_GYRO_XOUT_L 0x44 111 | #define MPU6050_RA_GYRO_YOUT_H 0x45 112 | #define MPU6050_RA_GYRO_YOUT_L 0x46 113 | #define MPU6050_RA_GYRO_ZOUT_H 0x47 114 | #define MPU6050_RA_GYRO_ZOUT_L 0x48 115 | #define MPU6050_RA_EXT_SENS_DATA_00 0x49 116 | #define MPU6050_RA_EXT_SENS_DATA_01 0x4A 117 | #define MPU6050_RA_EXT_SENS_DATA_02 0x4B 118 | #define MPU6050_RA_EXT_SENS_DATA_03 0x4C 119 | #define MPU6050_RA_EXT_SENS_DATA_04 0x4D 120 | #define MPU6050_RA_EXT_SENS_DATA_05 0x4E 121 | #define MPU6050_RA_EXT_SENS_DATA_06 0x4F 122 | #define MPU6050_RA_EXT_SENS_DATA_07 0x50 123 | #define MPU6050_RA_EXT_SENS_DATA_08 0x51 124 | #define MPU6050_RA_EXT_SENS_DATA_09 0x52 125 | #define MPU6050_RA_EXT_SENS_DATA_10 0x53 126 | #define MPU6050_RA_EXT_SENS_DATA_11 0x54 127 | #define MPU6050_RA_EXT_SENS_DATA_12 0x55 128 | #define MPU6050_RA_EXT_SENS_DATA_13 0x56 129 | #define MPU6050_RA_EXT_SENS_DATA_14 0x57 130 | #define MPU6050_RA_EXT_SENS_DATA_15 0x58 131 | #define MPU6050_RA_EXT_SENS_DATA_16 0x59 132 | #define MPU6050_RA_EXT_SENS_DATA_17 0x5A 133 | #define MPU6050_RA_EXT_SENS_DATA_18 0x5B 134 | #define MPU6050_RA_EXT_SENS_DATA_19 0x5C 135 | #define MPU6050_RA_EXT_SENS_DATA_20 0x5D 136 | #define MPU6050_RA_EXT_SENS_DATA_21 0x5E 137 | #define MPU6050_RA_EXT_SENS_DATA_22 0x5F 138 | #define MPU6050_RA_EXT_SENS_DATA_23 0x60 139 | #define MPU6050_RA_MOT_DETECT_STATUS 0x61 140 | #define MPU6050_RA_I2C_SLV0_DO 0x63 141 | #define MPU6050_RA_I2C_SLV1_DO 0x64 142 | #define MPU6050_RA_I2C_SLV2_DO 0x65 143 | #define MPU6050_RA_I2C_SLV3_DO 0x66 144 | #define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 145 | #define MPU6050_RA_SIGNAL_PATH_RESET 0x68 146 | #define MPU6050_RA_MOT_DETECT_CTRL 0x69 147 | #define MPU6050_RA_USER_CTRL 0x6A 148 | #define MPU6050_RA_PWR_MGMT_1 0x6B 149 | #define MPU6050_RA_PWR_MGMT_2 0x6C 150 | #define MPU6050_RA_BANK_SEL 0x6D 151 | #define MPU6050_RA_MEM_START_ADDR 0x6E 152 | #define MPU6050_RA_MEM_R_W 0x6F 153 | #define MPU6050_RA_DMP_CFG_1 0x70 154 | #define MPU6050_RA_DMP_CFG_2 0x71 155 | #define MPU6050_RA_FIFO_COUNTH 0x72 156 | #define MPU6050_RA_FIFO_COUNTL 0x73 157 | #define MPU6050_RA_FIFO_R_W 0x74 158 | #define MPU6050_RA_WHO_AM_I 0x75 159 | 160 | #define MPU6050_TC_PWR_MODE_BIT 7 161 | #define MPU6050_TC_OFFSET_BIT 6 162 | #define MPU6050_TC_OFFSET_LENGTH 6 163 | #define MPU6050_TC_OTP_BNK_VLD_BIT 0 164 | 165 | #define MPU6050_VDDIO_LEVEL_VLOGIC 0 166 | #define MPU6050_VDDIO_LEVEL_VDD 1 167 | 168 | #define MPU6050_CFG_EXT_SYNC_SET_BIT 5 169 | #define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 170 | #define MPU6050_CFG_DLPF_CFG_BIT 2 171 | #define MPU6050_CFG_DLPF_CFG_LENGTH 3 172 | 173 | #define MPU6050_EXT_SYNC_DISABLED 0x0 174 | #define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 175 | #define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 176 | #define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 177 | #define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 178 | #define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 179 | #define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 180 | #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 181 | 182 | #define MPU6050_DLPF_BW_256 0x00 183 | #define MPU6050_DLPF_BW_188 0x01 184 | #define MPU6050_DLPF_BW_98 0x02 185 | #define MPU6050_DLPF_BW_42 0x03 186 | #define MPU6050_DLPF_BW_20 0x04 187 | #define MPU6050_DLPF_BW_10 0x05 188 | #define MPU6050_DLPF_BW_5 0x06 189 | 190 | #define MPU6050_GCONFIG_FS_SEL_BIT 4 191 | #define MPU6050_GCONFIG_FS_SEL_LENGTH 2 192 | 193 | #define MPU6050_GYRO_FS_250 0x00 194 | #define MPU6050_GYRO_FS_500 0x01 195 | #define MPU6050_GYRO_FS_1000 0x02 196 | #define MPU6050_GYRO_FS_2000 0x03 197 | 198 | #define MPU6050_ACONFIG_XA_ST_BIT 7 199 | #define MPU6050_ACONFIG_YA_ST_BIT 6 200 | #define MPU6050_ACONFIG_ZA_ST_BIT 5 201 | #define MPU6050_ACONFIG_AFS_SEL_BIT 4 202 | #define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 203 | #define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 204 | #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 205 | 206 | #define MPU6050_ACCEL_FS_2 0x00 207 | #define MPU6050_ACCEL_FS_4 0x01 208 | #define MPU6050_ACCEL_FS_8 0x02 209 | #define MPU6050_ACCEL_FS_16 0x03 210 | 211 | #define MPU6050_DHPF_RESET 0x00 212 | #define MPU6050_DHPF_5 0x01 213 | #define MPU6050_DHPF_2P5 0x02 214 | #define MPU6050_DHPF_1P25 0x03 215 | #define MPU6050_DHPF_0P63 0x04 216 | #define MPU6050_DHPF_HOLD 0x07 217 | 218 | #define MPU6050_TEMP_FIFO_EN_BIT 7 219 | #define MPU6050_XG_FIFO_EN_BIT 6 220 | #define MPU6050_YG_FIFO_EN_BIT 5 221 | #define MPU6050_ZG_FIFO_EN_BIT 4 222 | #define MPU6050_ACCEL_FIFO_EN_BIT 3 223 | #define MPU6050_SLV2_FIFO_EN_BIT 2 224 | #define MPU6050_SLV1_FIFO_EN_BIT 1 225 | #define MPU6050_SLV0_FIFO_EN_BIT 0 226 | 227 | #define MPU6050_MULT_MST_EN_BIT 7 228 | #define MPU6050_WAIT_FOR_ES_BIT 6 229 | #define MPU6050_SLV_3_FIFO_EN_BIT 5 230 | #define MPU6050_I2C_MST_P_NSR_BIT 4 231 | #define MPU6050_I2C_MST_CLK_BIT 3 232 | #define MPU6050_I2C_MST_CLK_LENGTH 4 233 | 234 | #define MPU6050_CLOCK_DIV_348 0x0 235 | #define MPU6050_CLOCK_DIV_333 0x1 236 | #define MPU6050_CLOCK_DIV_320 0x2 237 | #define MPU6050_CLOCK_DIV_308 0x3 238 | #define MPU6050_CLOCK_DIV_296 0x4 239 | #define MPU6050_CLOCK_DIV_286 0x5 240 | #define MPU6050_CLOCK_DIV_276 0x6 241 | #define MPU6050_CLOCK_DIV_267 0x7 242 | #define MPU6050_CLOCK_DIV_258 0x8 243 | #define MPU6050_CLOCK_DIV_500 0x9 244 | #define MPU6050_CLOCK_DIV_471 0xA 245 | #define MPU6050_CLOCK_DIV_444 0xB 246 | #define MPU6050_CLOCK_DIV_421 0xC 247 | #define MPU6050_CLOCK_DIV_400 0xD 248 | #define MPU6050_CLOCK_DIV_381 0xE 249 | #define MPU6050_CLOCK_DIV_364 0xF 250 | 251 | #define MPU6050_I2C_SLV_RW_BIT 7 252 | #define MPU6050_I2C_SLV_ADDR_BIT 6 253 | #define MPU6050_I2C_SLV_ADDR_LENGTH 7 254 | #define MPU6050_I2C_SLV_EN_BIT 7 255 | #define MPU6050_I2C_SLV_BYTE_SW_BIT 6 256 | #define MPU6050_I2C_SLV_REG_DIS_BIT 5 257 | #define MPU6050_I2C_SLV_GRP_BIT 4 258 | #define MPU6050_I2C_SLV_LEN_BIT 3 259 | #define MPU6050_I2C_SLV_LEN_LENGTH 4 260 | 261 | #define MPU6050_I2C_SLV4_RW_BIT 7 262 | #define MPU6050_I2C_SLV4_ADDR_BIT 6 263 | #define MPU6050_I2C_SLV4_ADDR_LENGTH 7 264 | #define MPU6050_I2C_SLV4_EN_BIT 7 265 | #define MPU6050_I2C_SLV4_INT_EN_BIT 6 266 | #define MPU6050_I2C_SLV4_REG_DIS_BIT 5 267 | #define MPU6050_I2C_SLV4_MST_DLY_BIT 4 268 | #define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 269 | 270 | #define MPU6050_MST_PASS_THROUGH_BIT 7 271 | #define MPU6050_MST_I2C_SLV4_DONE_BIT 6 272 | #define MPU6050_MST_I2C_LOST_ARB_BIT 5 273 | #define MPU6050_MST_I2C_SLV4_NACK_BIT 4 274 | #define MPU6050_MST_I2C_SLV3_NACK_BIT 3 275 | #define MPU6050_MST_I2C_SLV2_NACK_BIT 2 276 | #define MPU6050_MST_I2C_SLV1_NACK_BIT 1 277 | #define MPU6050_MST_I2C_SLV0_NACK_BIT 0 278 | 279 | #define MPU6050_INTCFG_INT_LEVEL_BIT 7 280 | #define MPU6050_INTCFG_INT_OPEN_BIT 6 281 | #define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 282 | #define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 283 | #define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 284 | #define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 285 | #define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 286 | #define MPU6050_INTCFG_CLKOUT_EN_BIT 0 287 | 288 | #define MPU6050_INTMODE_ACTIVEHIGH 0x00 289 | #define MPU6050_INTMODE_ACTIVELOW 0x01 290 | 291 | #define MPU6050_INTDRV_PUSHPULL 0x00 292 | #define MPU6050_INTDRV_OPENDRAIN 0x01 293 | 294 | #define MPU6050_INTLATCH_50USPULSE 0x00 295 | #define MPU6050_INTLATCH_WAITCLEAR 0x01 296 | 297 | #define MPU6050_INTCLEAR_STATUSREAD 0x00 298 | #define MPU6050_INTCLEAR_ANYREAD 0x01 299 | 300 | #define MPU6050_INTERRUPT_FF_BIT 7 301 | #define MPU6050_INTERRUPT_MOT_BIT 6 302 | #define MPU6050_INTERRUPT_ZMOT_BIT 5 303 | #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 304 | #define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 305 | #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 306 | #define MPU6050_INTERRUPT_DMP_INT_BIT 1 307 | #define MPU6050_INTERRUPT_DATA_RDY_BIT 0 308 | 309 | // TODO: figure out what these actually do 310 | // UMPL source code is not very obivous 311 | #define MPU6050_DMPINT_5_BIT 5 312 | #define MPU6050_DMPINT_4_BIT 4 313 | #define MPU6050_DMPINT_3_BIT 3 314 | #define MPU6050_DMPINT_2_BIT 2 315 | #define MPU6050_DMPINT_1_BIT 1 316 | #define MPU6050_DMPINT_0_BIT 0 317 | 318 | #define MPU6050_MOTION_MOT_XNEG_BIT 7 319 | #define MPU6050_MOTION_MOT_XPOS_BIT 6 320 | #define MPU6050_MOTION_MOT_YNEG_BIT 5 321 | #define MPU6050_MOTION_MOT_YPOS_BIT 4 322 | #define MPU6050_MOTION_MOT_ZNEG_BIT 3 323 | #define MPU6050_MOTION_MOT_ZPOS_BIT 2 324 | #define MPU6050_MOTION_MOT_ZRMOT_BIT 0 325 | 326 | #define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 327 | #define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 328 | #define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 329 | #define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 330 | #define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 331 | #define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 332 | 333 | #define MPU6050_PATHRESET_GYRO_RESET_BIT 2 334 | #define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 335 | #define MPU6050_PATHRESET_TEMP_RESET_BIT 0 336 | 337 | #define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 338 | #define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 339 | #define MPU6050_DETECT_FF_COUNT_BIT 3 340 | #define MPU6050_DETECT_FF_COUNT_LENGTH 2 341 | #define MPU6050_DETECT_MOT_COUNT_BIT 1 342 | #define MPU6050_DETECT_MOT_COUNT_LENGTH 2 343 | 344 | #define MPU6050_DETECT_DECREMENT_RESET 0x0 345 | #define MPU6050_DETECT_DECREMENT_1 0x1 346 | #define MPU6050_DETECT_DECREMENT_2 0x2 347 | #define MPU6050_DETECT_DECREMENT_4 0x3 348 | 349 | #define MPU6050_USERCTRL_DMP_EN_BIT 7 350 | #define MPU6050_USERCTRL_FIFO_EN_BIT 6 351 | #define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 352 | #define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 353 | #define MPU6050_USERCTRL_DMP_RESET_BIT 3 354 | #define MPU6050_USERCTRL_FIFO_RESET_BIT 2 355 | #define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 356 | #define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 357 | 358 | #define MPU6050_PWR1_DEVICE_RESET_BIT 7 359 | #define MPU6050_PWR1_SLEEP_BIT 6 360 | #define MPU6050_PWR1_CYCLE_BIT 5 361 | #define MPU6050_PWR1_TEMP_DIS_BIT 3 362 | #define MPU6050_PWR1_CLKSEL_BIT 2 363 | #define MPU6050_PWR1_CLKSEL_LENGTH 3 364 | 365 | #define MPU6050_CLOCK_INTERNAL 0x00 366 | #define MPU6050_CLOCK_PLL_XGYRO 0x01 367 | #define MPU6050_CLOCK_PLL_YGYRO 0x02 368 | #define MPU6050_CLOCK_PLL_ZGYRO 0x03 369 | #define MPU6050_CLOCK_PLL_EXT32K 0x04 370 | #define MPU6050_CLOCK_PLL_EXT19M 0x05 371 | #define MPU6050_CLOCK_KEEP_RESET 0x07 372 | 373 | #define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 374 | #define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 375 | #define MPU6050_PWR2_STBY_XA_BIT 5 376 | #define MPU6050_PWR2_STBY_YA_BIT 4 377 | #define MPU6050_PWR2_STBY_ZA_BIT 3 378 | #define MPU6050_PWR2_STBY_XG_BIT 2 379 | #define MPU6050_PWR2_STBY_YG_BIT 1 380 | #define MPU6050_PWR2_STBY_ZG_BIT 0 381 | 382 | #define MPU6050_WAKE_FREQ_1P25 0x0 383 | #define MPU6050_WAKE_FREQ_2P5 0x1 384 | #define MPU6050_WAKE_FREQ_5 0x2 385 | #define MPU6050_WAKE_FREQ_10 0x3 386 | 387 | #define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 388 | #define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 389 | #define MPU6050_BANKSEL_MEM_SEL_BIT 4 390 | #define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 391 | 392 | #define MPU6050_WHO_AM_I_BIT 6 393 | #define MPU6050_WHO_AM_I_LENGTH 6 394 | 395 | #define MPU6050_DMP_MEMORY_BANKS 8 396 | #define MPU6050_DMP_MEMORY_BANK_SIZE 256 397 | #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 398 | 399 | // note: DMP code memory blocks defined at end of header file 400 | 401 | class MPU6050 { 402 | public: 403 | MPU6050(); 404 | MPU6050(uint8_t address); 405 | 406 | void initialize(); 407 | bool testConnection(); 408 | 409 | // AUX_VDDIO register 410 | uint8_t getAuxVDDIOLevel(); 411 | void setAuxVDDIOLevel(uint8_t level); 412 | 413 | // SMPLRT_DIV register 414 | uint8_t getRate(); 415 | void setRate(uint8_t rate); 416 | 417 | // CONFIG register 418 | uint8_t getExternalFrameSync(); 419 | void setExternalFrameSync(uint8_t sync); 420 | uint8_t getDLPFMode(); 421 | void setDLPFMode(uint8_t bandwidth); 422 | 423 | // GYRO_CONFIG register 424 | uint8_t getFullScaleGyroRange(); 425 | void setFullScaleGyroRange(uint8_t range); 426 | 427 | // ACCEL_CONFIG register 428 | bool getAccelXSelfTest(); 429 | void setAccelXSelfTest(bool enabled); 430 | bool getAccelYSelfTest(); 431 | void setAccelYSelfTest(bool enabled); 432 | bool getAccelZSelfTest(); 433 | void setAccelZSelfTest(bool enabled); 434 | uint8_t getFullScaleAccelRange(); 435 | void setFullScaleAccelRange(uint8_t range); 436 | uint8_t getDHPFMode(); 437 | void setDHPFMode(uint8_t mode); 438 | 439 | // FF_THR register 440 | uint8_t getFreefallDetectionThreshold(); 441 | void setFreefallDetectionThreshold(uint8_t threshold); 442 | 443 | // FF_DUR register 444 | uint8_t getFreefallDetectionDuration(); 445 | void setFreefallDetectionDuration(uint8_t duration); 446 | 447 | // MOT_THR register 448 | uint8_t getMotionDetectionThreshold(); 449 | void setMotionDetectionThreshold(uint8_t threshold); 450 | 451 | // MOT_DUR register 452 | uint8_t getMotionDetectionDuration(); 453 | void setMotionDetectionDuration(uint8_t duration); 454 | 455 | // ZRMOT_THR register 456 | uint8_t getZeroMotionDetectionThreshold(); 457 | void setZeroMotionDetectionThreshold(uint8_t threshold); 458 | 459 | // ZRMOT_DUR register 460 | uint8_t getZeroMotionDetectionDuration(); 461 | void setZeroMotionDetectionDuration(uint8_t duration); 462 | 463 | // FIFO_EN register 464 | bool getTempFIFOEnabled(); 465 | void setTempFIFOEnabled(bool enabled); 466 | bool getXGyroFIFOEnabled(); 467 | void setXGyroFIFOEnabled(bool enabled); 468 | bool getYGyroFIFOEnabled(); 469 | void setYGyroFIFOEnabled(bool enabled); 470 | bool getZGyroFIFOEnabled(); 471 | void setZGyroFIFOEnabled(bool enabled); 472 | bool getAccelFIFOEnabled(); 473 | void setAccelFIFOEnabled(bool enabled); 474 | bool getSlave2FIFOEnabled(); 475 | void setSlave2FIFOEnabled(bool enabled); 476 | bool getSlave1FIFOEnabled(); 477 | void setSlave1FIFOEnabled(bool enabled); 478 | bool getSlave0FIFOEnabled(); 479 | void setSlave0FIFOEnabled(bool enabled); 480 | 481 | // I2C_MST_CTRL register 482 | bool getMultiMasterEnabled(); 483 | void setMultiMasterEnabled(bool enabled); 484 | bool getWaitForExternalSensorEnabled(); 485 | void setWaitForExternalSensorEnabled(bool enabled); 486 | bool getSlave3FIFOEnabled(); 487 | void setSlave3FIFOEnabled(bool enabled); 488 | bool getSlaveReadWriteTransitionEnabled(); 489 | void setSlaveReadWriteTransitionEnabled(bool enabled); 490 | uint8_t getMasterClockSpeed(); 491 | void setMasterClockSpeed(uint8_t speed); 492 | 493 | // I2C_SLV* registers (Slave 0-3) 494 | uint8_t getSlaveAddress(uint8_t num); 495 | void setSlaveAddress(uint8_t num, uint8_t address); 496 | uint8_t getSlaveRegister(uint8_t num); 497 | void setSlaveRegister(uint8_t num, uint8_t reg); 498 | bool getSlaveEnabled(uint8_t num); 499 | void setSlaveEnabled(uint8_t num, bool enabled); 500 | bool getSlaveWordByteSwap(uint8_t num); 501 | void setSlaveWordByteSwap(uint8_t num, bool enabled); 502 | bool getSlaveWriteMode(uint8_t num); 503 | void setSlaveWriteMode(uint8_t num, bool mode); 504 | bool getSlaveWordGroupOffset(uint8_t num); 505 | void setSlaveWordGroupOffset(uint8_t num, bool enabled); 506 | uint8_t getSlaveDataLength(uint8_t num); 507 | void setSlaveDataLength(uint8_t num, uint8_t length); 508 | 509 | // I2C_SLV* registers (Slave 4) 510 | uint8_t getSlave4Address(); 511 | void setSlave4Address(uint8_t address); 512 | uint8_t getSlave4Register(); 513 | void setSlave4Register(uint8_t reg); 514 | void setSlave4OutputByte(uint8_t data); 515 | bool getSlave4Enabled(); 516 | void setSlave4Enabled(bool enabled); 517 | bool getSlave4InterruptEnabled(); 518 | void setSlave4InterruptEnabled(bool enabled); 519 | bool getSlave4WriteMode(); 520 | void setSlave4WriteMode(bool mode); 521 | uint8_t getSlave4MasterDelay(); 522 | void setSlave4MasterDelay(uint8_t delay); 523 | uint8_t getSlate4InputByte(); 524 | 525 | // I2C_MST_STATUS register 526 | bool getPassthroughStatus(); 527 | bool getSlave4IsDone(); 528 | bool getLostArbitration(); 529 | bool getSlave4Nack(); 530 | bool getSlave3Nack(); 531 | bool getSlave2Nack(); 532 | bool getSlave1Nack(); 533 | bool getSlave0Nack(); 534 | 535 | // INT_PIN_CFG register 536 | bool getInterruptMode(); 537 | void setInterruptMode(bool mode); 538 | bool getInterruptDrive(); 539 | void setInterruptDrive(bool drive); 540 | bool getInterruptLatch(); 541 | void setInterruptLatch(bool latch); 542 | bool getInterruptLatchClear(); 543 | void setInterruptLatchClear(bool clear); 544 | bool getFSyncInterruptLevel(); 545 | void setFSyncInterruptLevel(bool level); 546 | bool getFSyncInterruptEnabled(); 547 | void setFSyncInterruptEnabled(bool enabled); 548 | bool getI2CBypassEnabled(); 549 | void setI2CBypassEnabled(bool enabled); 550 | bool getClockOutputEnabled(); 551 | void setClockOutputEnabled(bool enabled); 552 | 553 | // INT_ENABLE register 554 | uint8_t getIntEnabled(); 555 | void setIntEnabled(uint8_t enabled); 556 | bool getIntFreefallEnabled(); 557 | void setIntFreefallEnabled(bool enabled); 558 | bool getIntMotionEnabled(); 559 | void setIntMotionEnabled(bool enabled); 560 | bool getIntZeroMotionEnabled(); 561 | void setIntZeroMotionEnabled(bool enabled); 562 | bool getIntFIFOBufferOverflowEnabled(); 563 | void setIntFIFOBufferOverflowEnabled(bool enabled); 564 | bool getIntI2CMasterEnabled(); 565 | void setIntI2CMasterEnabled(bool enabled); 566 | bool getIntDataReadyEnabled(); 567 | void setIntDataReadyEnabled(bool enabled); 568 | 569 | // INT_STATUS register 570 | uint8_t getIntStatus(); 571 | bool getIntFreefallStatus(); 572 | bool getIntMotionStatus(); 573 | bool getIntZeroMotionStatus(); 574 | bool getIntFIFOBufferOverflowStatus(); 575 | bool getIntI2CMasterStatus(); 576 | bool getIntDataReadyStatus(); 577 | 578 | // ACCEL_*OUT_* registers 579 | void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); 580 | void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); 581 | void getAcceleration(int16_t* x, int16_t* y, int16_t* z); 582 | int16_t getAccelerationX(); 583 | int16_t getAccelerationY(); 584 | int16_t getAccelerationZ(); 585 | 586 | // TEMP_OUT_* registers 587 | int16_t getTemperature(); 588 | 589 | // GYRO_*OUT_* registers 590 | void getRotation(int16_t* x, int16_t* y, int16_t* z); 591 | int16_t getRotationX(); 592 | int16_t getRotationY(); 593 | int16_t getRotationZ(); 594 | 595 | // EXT_SENS_DATA_* registers 596 | uint8_t getExternalSensorByte(int position); 597 | uint16_t getExternalSensorWord(int position); 598 | uint32_t getExternalSensorDWord(int position); 599 | 600 | // MOT_DETECT_STATUS register 601 | bool getXNegMotionDetected(); 602 | bool getXPosMotionDetected(); 603 | bool getYNegMotionDetected(); 604 | bool getYPosMotionDetected(); 605 | bool getZNegMotionDetected(); 606 | bool getZPosMotionDetected(); 607 | bool getZeroMotionDetected(); 608 | 609 | // I2C_SLV*_DO register 610 | void setSlaveOutputByte(uint8_t num, uint8_t data); 611 | 612 | // I2C_MST_DELAY_CTRL register 613 | bool getExternalShadowDelayEnabled(); 614 | void setExternalShadowDelayEnabled(bool enabled); 615 | bool getSlaveDelayEnabled(uint8_t num); 616 | void setSlaveDelayEnabled(uint8_t num, bool enabled); 617 | 618 | // SIGNAL_PATH_RESET register 619 | void resetGyroscopePath(); 620 | void resetAccelerometerPath(); 621 | void resetTemperaturePath(); 622 | 623 | // MOT_DETECT_CTRL register 624 | uint8_t getAccelerometerPowerOnDelay(); 625 | void setAccelerometerPowerOnDelay(uint8_t delay); 626 | uint8_t getFreefallDetectionCounterDecrement(); 627 | void setFreefallDetectionCounterDecrement(uint8_t decrement); 628 | uint8_t getMotionDetectionCounterDecrement(); 629 | void setMotionDetectionCounterDecrement(uint8_t decrement); 630 | 631 | // USER_CTRL register 632 | bool getFIFOEnabled(); 633 | void setFIFOEnabled(bool enabled); 634 | bool getI2CMasterModeEnabled(); 635 | void setI2CMasterModeEnabled(bool enabled); 636 | void switchSPIEnabled(bool enabled); 637 | void resetFIFO(); 638 | void resetI2CMaster(); 639 | void resetSensors(); 640 | 641 | // PWR_MGMT_1 register 642 | void reset(); 643 | bool getSleepEnabled(); 644 | void setSleepEnabled(bool enabled); 645 | bool getWakeCycleEnabled(); 646 | void setWakeCycleEnabled(bool enabled); 647 | bool getTempSensorEnabled(); 648 | void setTempSensorEnabled(bool enabled); 649 | uint8_t getClockSource(); 650 | void setClockSource(uint8_t source); 651 | 652 | // PWR_MGMT_2 register 653 | uint8_t getWakeFrequency(); 654 | void setWakeFrequency(uint8_t frequency); 655 | bool getStandbyXAccelEnabled(); 656 | void setStandbyXAccelEnabled(bool enabled); 657 | bool getStandbyYAccelEnabled(); 658 | void setStandbyYAccelEnabled(bool enabled); 659 | bool getStandbyZAccelEnabled(); 660 | void setStandbyZAccelEnabled(bool enabled); 661 | bool getStandbyXGyroEnabled(); 662 | void setStandbyXGyroEnabled(bool enabled); 663 | bool getStandbyYGyroEnabled(); 664 | void setStandbyYGyroEnabled(bool enabled); 665 | bool getStandbyZGyroEnabled(); 666 | void setStandbyZGyroEnabled(bool enabled); 667 | 668 | // FIFO_COUNT_* registers 669 | uint16_t getFIFOCount(); 670 | 671 | // FIFO_R_W register 672 | uint8_t getFIFOByte(); 673 | void setFIFOByte(uint8_t data); 674 | void getFIFOBytes(uint8_t *data, uint8_t length); 675 | 676 | // WHO_AM_I register 677 | uint8_t getDeviceID(); 678 | void setDeviceID(uint8_t id); 679 | 680 | // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== 681 | 682 | // XG_OFFS_TC register 683 | uint8_t getOTPBankValid(); 684 | void setOTPBankValid(bool enabled); 685 | int8_t getXGyroOffsetTC(); 686 | void setXGyroOffsetTC(int8_t offset); 687 | 688 | // YG_OFFS_TC register 689 | int8_t getYGyroOffsetTC(); 690 | void setYGyroOffsetTC(int8_t offset); 691 | 692 | // ZG_OFFS_TC register 693 | int8_t getZGyroOffsetTC(); 694 | void setZGyroOffsetTC(int8_t offset); 695 | 696 | // X_FINE_GAIN register 697 | int8_t getXFineGain(); 698 | void setXFineGain(int8_t gain); 699 | 700 | // Y_FINE_GAIN register 701 | int8_t getYFineGain(); 702 | void setYFineGain(int8_t gain); 703 | 704 | // Z_FINE_GAIN register 705 | int8_t getZFineGain(); 706 | void setZFineGain(int8_t gain); 707 | 708 | // XA_OFFS_* registers 709 | int16_t getXAccelOffset(); 710 | void setXAccelOffset(int16_t offset); 711 | 712 | // YA_OFFS_* register 713 | int16_t getYAccelOffset(); 714 | void setYAccelOffset(int16_t offset); 715 | 716 | // ZA_OFFS_* register 717 | int16_t getZAccelOffset(); 718 | void setZAccelOffset(int16_t offset); 719 | 720 | // XG_OFFS_USR* registers 721 | int16_t getXGyroOffset(); 722 | void setXGyroOffset(int16_t offset); 723 | 724 | // YG_OFFS_USR* register 725 | int16_t getYGyroOffset(); 726 | void setYGyroOffset(int16_t offset); 727 | 728 | // ZG_OFFS_USR* register 729 | int16_t getZGyroOffset(); 730 | void setZGyroOffset(int16_t offset); 731 | 732 | // INT_ENABLE register (DMP functions) 733 | bool getIntPLLReadyEnabled(); 734 | void setIntPLLReadyEnabled(bool enabled); 735 | bool getIntDMPEnabled(); 736 | void setIntDMPEnabled(bool enabled); 737 | 738 | // DMP_INT_STATUS 739 | bool getDMPInt5Status(); 740 | bool getDMPInt4Status(); 741 | bool getDMPInt3Status(); 742 | bool getDMPInt2Status(); 743 | bool getDMPInt1Status(); 744 | bool getDMPInt0Status(); 745 | 746 | // INT_STATUS register (DMP functions) 747 | bool getIntPLLReadyStatus(); 748 | bool getIntDMPStatus(); 749 | 750 | // USER_CTRL register (DMP functions) 751 | bool getDMPEnabled(); 752 | void setDMPEnabled(bool enabled); 753 | void resetDMP(); 754 | 755 | // BANK_SEL register 756 | void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); 757 | 758 | // MEM_START_ADDR register 759 | void setMemoryStartAddress(uint8_t address); 760 | 761 | // MEM_R_W register 762 | uint8_t readMemoryByte(); 763 | void writeMemoryByte(uint8_t data); 764 | void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); 765 | bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); 766 | bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); 767 | 768 | bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); 769 | bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); 770 | 771 | // DMP_CFG_1 register 772 | uint8_t getDMPConfig1(); 773 | void setDMPConfig1(uint8_t config); 774 | 775 | // DMP_CFG_2 register 776 | uint8_t getDMPConfig2(); 777 | void setDMPConfig2(uint8_t config); 778 | 779 | // special methods for MotionApps 2.0 implementation 780 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 781 | uint8_t *dmpPacketBuffer; 782 | uint16_t dmpPacketSize; 783 | 784 | uint8_t dmpInitialize(); 785 | bool dmpPacketAvailable(); 786 | 787 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 788 | uint8_t dmpGetFIFORate(); 789 | uint8_t dmpGetSampleStepSizeMS(); 790 | uint8_t dmpGetSampleFrequency(); 791 | int32_t dmpDecodeTemperature(int8_t tempReg); 792 | 793 | 794 | // Register callbacks after a packet of FIFO data is processed 795 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 796 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 797 | uint8_t dmpRunFIFORateProcesses(); 798 | 799 | // Setup FIFO for various output 800 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 801 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 802 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 803 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 804 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 805 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 806 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 807 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 808 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 809 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 810 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 811 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 812 | 813 | // Get Fixed Point data from FIFO 814 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 815 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 816 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 817 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 818 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 819 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 820 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 821 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 822 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 823 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 824 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 825 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 826 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 827 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 828 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 829 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 830 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 831 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 832 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 833 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 834 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 835 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 836 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 837 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 838 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 839 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 840 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 841 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 842 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 843 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 844 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 845 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 846 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 847 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 848 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 849 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 850 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 851 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 852 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 853 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 854 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 855 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 856 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 857 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 858 | 859 | uint8_t dmpGetEuler(float *data, Quaternion *q); 860 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 861 | 862 | // Get Floating Point data from FIFO 863 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 864 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 865 | 866 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 867 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 868 | 869 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 870 | 871 | uint8_t dmpInitFIFOParam(); 872 | uint8_t dmpCloseFIFO(); 873 | uint8_t dmpSetGyroDataSource(uint8_t source); 874 | uint8_t dmpDecodeQuantizedAccel(); 875 | uint32_t dmpGetGyroSumOfSquare(); 876 | uint32_t dmpGetAccelSumOfSquare(); 877 | void dmpOverrideQuaternion(long *q); 878 | uint16_t dmpGetFIFOPacketSize(); 879 | #endif 880 | 881 | // special methods for MotionApps 4.1 implementation 882 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 883 | uint8_t *dmpPacketBuffer; 884 | uint16_t dmpPacketSize; 885 | 886 | uint8_t dmpInitialize(); 887 | bool dmpPacketAvailable(); 888 | 889 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 890 | uint8_t dmpGetFIFORate(); 891 | uint8_t dmpGetSampleStepSizeMS(); 892 | uint8_t dmpGetSampleFrequency(); 893 | int32_t dmpDecodeTemperature(int8_t tempReg); 894 | 895 | // Register callbacks after a packet of FIFO data is processed 896 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 897 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 898 | uint8_t dmpRunFIFORateProcesses(); 899 | 900 | // Setup FIFO for various output 901 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 902 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 903 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 904 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 905 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 906 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 907 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 908 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 909 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 910 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 911 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 912 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 913 | 914 | // Get Fixed Point data from FIFO 915 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 916 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 917 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 918 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 919 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 920 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 921 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 922 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 923 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 924 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 925 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 926 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 927 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 928 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 929 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 930 | uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); 931 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 932 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 933 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 934 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 935 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 936 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 937 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 938 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 939 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 940 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 941 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 942 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 943 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 944 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 945 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 946 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 947 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 948 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 949 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 950 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 951 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 952 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 953 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 954 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 955 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 956 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 957 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 958 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 959 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 960 | 961 | uint8_t dmpGetEuler(float *data, Quaternion *q); 962 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 963 | 964 | // Get Floating Point data from FIFO 965 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 966 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 967 | 968 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 969 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 970 | 971 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 972 | 973 | uint8_t dmpInitFIFOParam(); 974 | uint8_t dmpCloseFIFO(); 975 | uint8_t dmpSetGyroDataSource(uint8_t source); 976 | uint8_t dmpDecodeQuantizedAccel(); 977 | uint32_t dmpGetGyroSumOfSquare(); 978 | uint32_t dmpGetAccelSumOfSquare(); 979 | void dmpOverrideQuaternion(long *q); 980 | uint16_t dmpGetFIFOPacketSize(); 981 | #endif 982 | 983 | private: 984 | uint8_t devAddr; 985 | uint8_t buffer[14]; 986 | }; 987 | 988 | #endif /* _MPU6050_H_ */ 989 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/MPU6050_6Axis_MotionApps20.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 5/20/2013 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ 34 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ 35 | 36 | #include "I2Cdev.h" 37 | #include "helper_3dmath.h" 38 | 39 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board 40 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 41 | 42 | #include "MPU6050.h" 43 | 44 | // Tom Carpenter's conditional PROGMEM code 45 | // http://forum.arduino.cc/index.php?topic=129407.0 46 | #ifndef __arm__ 47 | #include 48 | #else 49 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 50 | #ifndef __PGMSPACE_H_ 51 | #define __PGMSPACE_H_ 1 52 | #include 53 | 54 | #define PROGMEM 55 | #define PGM_P const char * 56 | #define PSTR(str) (str) 57 | #define F(x) x 58 | 59 | typedef void prog_void; 60 | typedef char prog_char; 61 | typedef unsigned char prog_uchar; 62 | typedef int8_t prog_int8_t; 63 | typedef uint8_t prog_uint8_t; 64 | typedef int16_t prog_int16_t; 65 | typedef uint16_t prog_uint16_t; 66 | typedef int32_t prog_int32_t; 67 | typedef uint32_t prog_uint32_t; 68 | 69 | #define strcpy_P(dest, src) strcpy((dest), (src)) 70 | #define strcat_P(dest, src) strcat((dest), (src)) 71 | #define strcmp_P(a, b) strcmp((a), (b)) 72 | 73 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 74 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 75 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 76 | #define pgm_read_float(addr) (*(const float *)(addr)) 77 | 78 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 79 | #define pgm_read_word_near(addr) pgm_read_word(addr) 80 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 81 | #define pgm_read_float_near(addr) pgm_read_float(addr) 82 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 83 | #define pgm_read_word_far(addr) pgm_read_word(addr) 84 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 85 | #define pgm_read_float_far(addr) pgm_read_float(addr) 86 | #endif 87 | #endif 88 | 89 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 90 | * unavailable, unless you happen to be amazing as decompiling binary by 91 | * hand (in which case, please contact me, and I'm totally serious). 92 | * 93 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 94 | * DMP reverse-engineering he did to help make this bit of wizardry 95 | * possible. 96 | */ 97 | 98 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 99 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 100 | // after moving string constants to flash memory storage using the F() 101 | // compiler macro (Arduino IDE 1.0+ required). 102 | 103 | //#define DEBUG 104 | #ifdef DEBUG 105 | #define DEBUG_PRINT(x) Serial.print(x) 106 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 107 | #define DEBUG_PRINTLN(x) Serial.println(x) 108 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 109 | #else 110 | #define DEBUG_PRINT(x) 111 | #define DEBUG_PRINTF(x, y) 112 | #define DEBUG_PRINTLN(x) 113 | #define DEBUG_PRINTLNF(x, y) 114 | #endif 115 | 116 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 117 | #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] 118 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 119 | 120 | /* ================================================================================================ * 121 | | Default MotionApps v2.0 42-byte FIFO packet structure: | 122 | | | 123 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 124 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 125 | | | 126 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 127 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 128 | * ================================================================================================ */ 129 | 130 | // this block of memory gets written to the MPU on start-up, and it seems 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 132 | // second though) 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 134 | // bank 0, 256 bytes 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 151 | 152 | // bank 1, 256 bytes 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 169 | 170 | // bank 2, 256 bytes 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 187 | 188 | // bank 3, 256 bytes 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 205 | 206 | // bank 4, 256 bytes 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 223 | 224 | // bank 5, 256 bytes 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 241 | 242 | // bank 6, 256 bytes 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 259 | 260 | // bank 7, 138 bytes (remainder) 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 270 | }; 271 | 272 | // thanks to Noah Zerkin for piecing this stuff together! 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 274 | // BANK OFFSET LENGTH [DATA] 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 283 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 284 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 285 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 286 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 287 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 288 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 289 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 290 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 291 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 295 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 296 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 297 | // SPECIAL 0x01 = enable interrupts 298 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 299 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 300 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 301 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 302 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 303 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 304 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 305 | 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate 306 | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 310 | 311 | // It is important to make sure the host processor can keep up with reading and processing 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 313 | }; 314 | 315 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 316 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 317 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 318 | 0x01, 0x6A, 0x02, 0x06, 0x00, 319 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 320 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 321 | 0x01, 0x62, 0x02, 0x00, 0x00, 322 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 323 | }; 324 | 325 | uint8_t MPU6050::dmpInitialize() { 326 | // reset device 327 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 328 | reset(); 329 | delay(30); // wait after reset 330 | 331 | // enable sleep mode and wake cycle 332 | /*Serial.println(F("Enabling sleep mode...")); 333 | setSleepEnabled(true); 334 | Serial.println(F("Enabling wake cycle...")); 335 | setWakeCycleEnabled(true);*/ 336 | 337 | // disable sleep mode 338 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 339 | setSleepEnabled(false); 340 | 341 | // get MPU hardware revision 342 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 343 | setMemoryBank(0x10, true, true); 344 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 345 | setMemoryStartAddress(0x06); 346 | DEBUG_PRINTLN(F("Checking hardware revision...")); 347 | uint8_t hwRevision = readMemoryByte(); 348 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 349 | DEBUG_PRINTLNF(hwRevision, HEX); 350 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 351 | setMemoryBank(0, false, false); 352 | 353 | // check OTP bank valid 354 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 355 | uint8_t otpValid = getOTPBankValid(); 356 | DEBUG_PRINT(F("OTP bank is ")); 357 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 358 | 359 | // get X/Y/Z gyro offsets 360 | DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 361 | int8_t xgOffsetTC = getXGyroOffsetTC(); 362 | int8_t ygOffsetTC = getYGyroOffsetTC(); 363 | int8_t zgOffsetTC = getZGyroOffsetTC(); 364 | DEBUG_PRINT(F("X gyro offset = ")); 365 | DEBUG_PRINTLN(xgOffset); 366 | DEBUG_PRINT(F("Y gyro offset = ")); 367 | DEBUG_PRINTLN(ygOffset); 368 | DEBUG_PRINT(F("Z gyro offset = ")); 369 | DEBUG_PRINTLN(zgOffset); 370 | 371 | // setup weird slave stuff (?) 372 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 373 | setSlaveAddress(0, 0x7F); 374 | DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 375 | setI2CMasterModeEnabled(false); 376 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 377 | setSlaveAddress(0, 0x68); 378 | DEBUG_PRINTLN(F("Resetting I2C Master control...")); 379 | resetI2CMaster(); 380 | delay(20); 381 | 382 | // load DMP code into memory banks 383 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 384 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 385 | DEBUG_PRINTLN(F(" bytes)")); 386 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 387 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 388 | 389 | // write DMP configuration 390 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 391 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 392 | DEBUG_PRINTLN(F(" bytes in config def)")); 393 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 394 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 395 | 396 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 397 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 398 | 399 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 400 | setIntEnabled(0x12); 401 | 402 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 403 | setRate(4); // 1khz / (1 + 4) = 200 Hz 404 | 405 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 406 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 407 | 408 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 409 | setDLPFMode(MPU6050_DLPF_BW_42); 410 | 411 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 412 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 413 | 414 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 415 | setDMPConfig1(0x03); 416 | setDMPConfig2(0x00); 417 | 418 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 419 | setOTPBankValid(false); 420 | 421 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 422 | setXGyroOffsetTC(xgOffsetTC); 423 | setYGyroOffsetTC(ygOffsetTC); 424 | setZGyroOffsetTC(zgOffsetTC); 425 | 426 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 427 | //setXGyroOffset(0); 428 | //setYGyroOffset(0); 429 | //setZGyroOffset(0); 430 | 431 | DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 432 | uint8_t dmpUpdate[16], j; 433 | uint16_t pos = 0; 434 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 435 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 436 | 437 | DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 438 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 439 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 440 | 441 | DEBUG_PRINTLN(F("Resetting FIFO...")); 442 | resetFIFO(); 443 | 444 | DEBUG_PRINTLN(F("Reading FIFO count...")); 445 | uint16_t fifoCount = getFIFOCount(); 446 | uint8_t fifoBuffer[128]; 447 | 448 | DEBUG_PRINT(F("Current FIFO count=")); 449 | DEBUG_PRINTLN(fifoCount); 450 | getFIFOBytes(fifoBuffer, fifoCount); 451 | 452 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 453 | setMotionDetectionThreshold(2); 454 | 455 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 456 | setZeroMotionDetectionThreshold(156); 457 | 458 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 459 | setMotionDetectionDuration(80); 460 | 461 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 462 | setZeroMotionDetectionDuration(0); 463 | 464 | DEBUG_PRINTLN(F("Resetting FIFO...")); 465 | resetFIFO(); 466 | 467 | DEBUG_PRINTLN(F("Enabling FIFO...")); 468 | setFIFOEnabled(true); 469 | 470 | DEBUG_PRINTLN(F("Enabling DMP...")); 471 | setDMPEnabled(true); 472 | 473 | DEBUG_PRINTLN(F("Resetting DMP...")); 474 | resetDMP(); 475 | 476 | DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 477 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 478 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 479 | 480 | DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 481 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 482 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 483 | 484 | DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 485 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 486 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 487 | 488 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 489 | while ((fifoCount = getFIFOCount()) < 3); 490 | 491 | DEBUG_PRINT(F("Current FIFO count=")); 492 | DEBUG_PRINTLN(fifoCount); 493 | DEBUG_PRINTLN(F("Reading FIFO data...")); 494 | getFIFOBytes(fifoBuffer, fifoCount); 495 | 496 | DEBUG_PRINTLN(F("Reading interrupt status...")); 497 | uint8_t mpuIntStatus = getIntStatus(); 498 | 499 | DEBUG_PRINT(F("Current interrupt status=")); 500 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 501 | 502 | DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 503 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 504 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 505 | 506 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 507 | while ((fifoCount = getFIFOCount()) < 3); 508 | 509 | DEBUG_PRINT(F("Current FIFO count=")); 510 | DEBUG_PRINTLN(fifoCount); 511 | 512 | DEBUG_PRINTLN(F("Reading FIFO data...")); 513 | getFIFOBytes(fifoBuffer, fifoCount); 514 | 515 | DEBUG_PRINTLN(F("Reading interrupt status...")); 516 | mpuIntStatus = getIntStatus(); 517 | 518 | DEBUG_PRINT(F("Current interrupt status=")); 519 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 520 | 521 | DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 522 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 523 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 524 | 525 | DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 526 | 527 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 528 | setDMPEnabled(false); 529 | 530 | DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); 531 | dmpPacketSize = 42; 532 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 533 | return 3; // TODO: proper error code for no memory 534 | }*/ 535 | 536 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 537 | resetFIFO(); 538 | getIntStatus(); 539 | } else { 540 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 541 | return 2; // configuration block loading failed 542 | } 543 | } else { 544 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 545 | return 1; // main binary block loading failed 546 | } 547 | return 0; // success 548 | } 549 | 550 | bool MPU6050::dmpPacketAvailable() { 551 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 552 | } 553 | 554 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 555 | // uint8_t MPU6050::dmpGetFIFORate(); 556 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 557 | // uint8_t MPU6050::dmpGetSampleFrequency(); 558 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 559 | 560 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 561 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 562 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 563 | 564 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 565 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 566 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 567 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 568 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 569 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 570 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 571 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 572 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 573 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 574 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 575 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 576 | 577 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 578 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 579 | if (packet == 0) packet = dmpPacketBuffer; 580 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 581 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 582 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 583 | return 0; 584 | } 585 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 586 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 587 | if (packet == 0) packet = dmpPacketBuffer; 588 | data[0] = (packet[28] << 8) + packet[29]; 589 | data[1] = (packet[32] << 8) + packet[33]; 590 | data[2] = (packet[36] << 8) + packet[37]; 591 | return 0; 592 | } 593 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 594 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 595 | if (packet == 0) packet = dmpPacketBuffer; 596 | v -> x = (packet[28] << 8) + packet[29]; 597 | v -> y = (packet[32] << 8) + packet[33]; 598 | v -> z = (packet[36] << 8) + packet[37]; 599 | return 0; 600 | } 601 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 602 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 603 | if (packet == 0) packet = dmpPacketBuffer; 604 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 605 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 606 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 607 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 608 | return 0; 609 | } 610 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 611 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 612 | if (packet == 0) packet = dmpPacketBuffer; 613 | data[0] = ((packet[0] << 8) + packet[1]); 614 | data[1] = ((packet[4] << 8) + packet[5]); 615 | data[2] = ((packet[8] << 8) + packet[9]); 616 | data[3] = ((packet[12] << 8) + packet[13]); 617 | return 0; 618 | } 619 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 620 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 621 | int16_t qI[4]; 622 | uint8_t status = dmpGetQuaternion(qI, packet); 623 | if (status == 0) { 624 | q -> w = (float)qI[0] / 16384.0f; 625 | q -> x = (float)qI[1] / 16384.0f; 626 | q -> y = (float)qI[2] / 16384.0f; 627 | q -> z = (float)qI[3] / 16384.0f; 628 | return 0; 629 | } 630 | return status; // int16 return value, indicates error if this line is reached 631 | } 632 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 633 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 634 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 635 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 636 | if (packet == 0) packet = dmpPacketBuffer; 637 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 638 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 639 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 640 | return 0; 641 | } 642 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 643 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 644 | if (packet == 0) packet = dmpPacketBuffer; 645 | data[0] = (packet[16] << 8) + packet[17]; 646 | data[1] = (packet[20] << 8) + packet[21]; 647 | data[2] = (packet[24] << 8) + packet[25]; 648 | return 0; 649 | } 650 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 651 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 652 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 653 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 654 | v -> x = vRaw -> x - gravity -> x*8192; 655 | v -> y = vRaw -> y - gravity -> y*8192; 656 | v -> z = vRaw -> z - gravity -> z*8192; 657 | return 0; 658 | } 659 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 660 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 661 | // rotate measured 3D acceleration vector into original state 662 | // frame of reference based on orientation quaternion 663 | memcpy(v, vReal, sizeof(VectorInt16)); 664 | v -> rotate(q); 665 | return 0; 666 | } 667 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 668 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 669 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 670 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 671 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 672 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 673 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 674 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 675 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 676 | return 0; 677 | } 678 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 679 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 680 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 681 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 682 | 683 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 684 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi 685 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 686 | data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi 687 | return 0; 688 | } 689 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 690 | // yaw: (about Z axis) 691 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 692 | // pitch: (nose up/down, about Y axis) 693 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 694 | // roll: (tilt left/right, about X axis) 695 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 696 | return 0; 697 | } 698 | 699 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 700 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 701 | 702 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 703 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 704 | if (dmpData[k] < 0x10) Serial.print("0"); 705 | Serial.print(dmpData[k], HEX); 706 | Serial.print(" "); 707 | } 708 | Serial.print("\n");*/ 709 | //Serial.println((uint16_t)dmpPacketBuffer); 710 | return 0; 711 | } 712 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 713 | uint8_t status; 714 | uint8_t buf[dmpPacketSize]; 715 | for (uint8_t i = 0; i < numPackets; i++) { 716 | // read packet from FIFO 717 | getFIFOBytes(buf, dmpPacketSize); 718 | 719 | // process packet 720 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 721 | 722 | // increment external process count variable, if supplied 723 | if (processed != 0) *processed++; 724 | } 725 | return 0; 726 | } 727 | 728 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 729 | 730 | // uint8_t MPU6050::dmpInitFIFOParam(); 731 | // uint8_t MPU6050::dmpCloseFIFO(); 732 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 733 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 734 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 735 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 736 | // void MPU6050::dmpOverrideQuaternion(long *q); 737 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 738 | return dmpPacketSize; 739 | } 740 | 741 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 742 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/MsTimer2.cpp: -------------------------------------------------------------------------------- 1 | #include "./MsTimer2.h" 2 | 3 | unsigned long MsTimer2::msecs; 4 | void (*MsTimer2::func)(); 5 | volatile unsigned long MsTimer2::count; 6 | volatile char MsTimer2::overflowing; 7 | volatile unsigned int MsTimer2::tcnt2; 8 | 9 | void MsTimer2::set(unsigned long ms, void (*f)()) { 10 | float prescaler = 0.0; 11 | 12 | #if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || (__AVR_ATmega1280__) 13 | TIMSK2 &= ~(1<= 1000000UL) && (F_CPU <= 16000000UL)) { // prescaler set to 64 20 | TCCR2B |= (1< 16Mhz, prescaler set to 128 28 | TCCR2B |= ((1<= 1000000UL) && (F_CPU <= 16000000UL)) { // prescaler set to 64 39 | TCCR2 |= (1< 16Mhz, prescaler set to 128 47 | TCCR2 |= ((1<= 1000000UL) && (F_CPU <= 16000000UL)) { // prescaler set to 64 57 | TCCR2 |= ((1< 16Mhz, prescaler set to 256 65 | TCCR2 |= (1<= msecs && !overflowing) { 110 | overflowing = 1; 111 | count = 0; 112 | (*func)(); 113 | overflowing = 0; 114 | } 115 | } 116 | 117 | ISR(TIMER2_OVF_vect) { 118 | #if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || (__AVR_ATmega1280__) 119 | TCNT2 = MsTimer2::tcnt2; 120 | #elif defined (__AVR_ATmega128__) 121 | TCNT2 = MsTimer2::tcnt2; 122 | #elif defined (__AVR_ATmega8__) 123 | TCNT2 = MsTimer2::tcnt2; 124 | #endif 125 | MsTimer2::_overflow(); 126 | } 127 | 128 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/MsTimer2.h: -------------------------------------------------------------------------------- 1 | #ifndef MsTimer2_h 2 | #define MsTimer2_h 3 | 4 | #include 5 | 6 | namespace MsTimer2 { 7 | extern unsigned long msecs; 8 | extern void (*func)(); 9 | extern volatile unsigned long count; 10 | extern volatile char overflowing; 11 | extern volatile unsigned int tcnt2; 12 | 13 | void set(unsigned long ms, void (*f)()); 14 | void start(); 15 | void stop(); 16 | void _overflow(); 17 | } 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/PinChangeInt.h: -------------------------------------------------------------------------------- 1 | #define PCINT_VERSION 2402 2 | 3 | // 4 | // For the beginners 5 | // 6 | #define detachPinChangeInterrupt(pin) PCintPort::detachInterrupt(pin) 7 | #define attachPinChangeInterrupt(pin,userFunc,mode) PCintPort::attachInterrupt(pin, &userFunc,mode) 8 | #define getInterruptedPin() PCintPort::getArduinoPin() 9 | 10 | // We use 4-character tabstops, so IN VIM: :set ts=4 sw=4 sts=4 11 | // ...that's: ESCAPE key, colon key, then 12 | // "s-e-t SPACE key t-s = 4 SPACE key s-w = 4 SPACE key s-t-s = 4" 13 | 14 | 15 | /* 16 | VERSIONS found in moved to RELEASE_NOTES. 17 | 18 | See the README file for the License and more details. 19 | */ 20 | 21 | #ifndef PinChangeInt_h 22 | #define PinChangeInt_h 23 | 24 | #include "stddef.h" 25 | 26 | // Maurice Beelen, nms277, Akesson Karlpetter, and Orly Andico 27 | // sent in fixes to work with Arduino >= version 1.0 28 | #include 29 | #include 30 | #include // cbi and sbi defined here 31 | 32 | #undef DEBUG 33 | 34 | /* 35 | * Theory: For the IO pins covered by Pin Change Interrupts 36 | * (== all of them on the Atmega168/328, and a subset on the Atmega2560), 37 | * the PCINT corresponding to the pin must be enabled and masked, and 38 | * an ISR routine provided. Since PCINTs are per port, not per pin, the ISR 39 | * must use some logic to actually implement a per-pin interrupt service. 40 | */ 41 | 42 | /* Pin to interrupt map, ATmega328: 43 | * D0-D7 = PCINT 16-23 = PCIR2 = PD = PCIE2 = pcmsk2 44 | * D8-D13 = PCINT 0-5 = PCIR0 = PB = PCIE0 = pcmsk0 45 | * A0-A5 (D14-D19) = PCINT 8-13 = PCIR1 = PC = PCIE1 = pcmsk1 46 | */ 47 | 48 | #undef INLINE_PCINT 49 | #define INLINE_PCINT 50 | // Thanks to cserveny...@gmail.com for MEGA support! 51 | #if defined __AVR_ATmega2560__ || defined __AVR_ATmega1280__ || defined __AVR_ATmega1281__ || defined __AVR_ATmega2561__ || defined __AVR_ATmega640__ 52 | #define __USE_PORT_JK 53 | // Mega does not have PORTA, C or D 54 | #define NO_PORTA_PINCHANGES 55 | #define NO_PORTC_PINCHANGES 56 | #define NO_PORTD_PINCHANGES 57 | #if ((defined(NO_PORTB_PINCHANGES) && defined(NO_PORTJ_PINCHANGES)) || \ 58 | (defined(NO_PORTJ_PINCHANGES) && defined(NO_PORTK_PINCHANGES)) || \ 59 | (defined(NO_PORTK_PINCHANGES) && defined(NO_PORTB_PINCHANGES))) 60 | #define INLINE_PCINT inline 61 | #endif 62 | #else 63 | #define NO_PORTJ_PINCHANGES 64 | #define NO_PORTK_PINCHANGES 65 | #if defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) 66 | #ifndef NO_PORTA_PINCHANGES 67 | #define __USE_PORT_A 68 | #endif 69 | #else 70 | #define NO_PORTA_PINCHANGES 71 | #endif 72 | // if defined only D .OR. only C .OR. only B .OR. only A, then inline it 73 | #if ( (defined(NO_PORTA_PINCHANGES) && defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES)) || \ 74 | (defined(NO_PORTA_PINCHANGES) && defined(NO_PORTB_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \ 75 | (defined(NO_PORTA_PINCHANGES) && defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \ 76 | (defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) ) 77 | #define INLINE_PCINT inline 78 | #endif 79 | #endif 80 | 81 | // Provide drop in compatibility with Chris J. Kiick's PCInt project at 82 | // http://www.arduino.cc/playground/Main/PcInt 83 | #define PCdetachInterrupt(pin) PCintPort::detachInterrupt(pin) 84 | #define PCattachInterrupt(pin,userFunc,mode) PCintPort::attachInterrupt(pin, userFunc,mode) 85 | #define PCgetArduinoPin() PCintPort::getArduinoPin() 86 | 87 | typedef void (*PCIntvoidFuncPtr)(void); 88 | 89 | class PCintPort { 90 | public: 91 | // portB=PCintPort(2, 1,PCMSK1); 92 | // index: portInputReg(*portInputRegister(index)), 93 | // pcindex: PCICRbit(1 << pcindex) 94 | // maskReg: portPCMask(maskReg) 95 | PCintPort(int index,int pcindex, volatile uint8_t& maskReg) : 96 | portInputReg(*portInputRegister(index)), 97 | portPCMask(maskReg), 98 | PCICRbit(1 << pcindex), 99 | portRisingPins(0), 100 | portFallingPins(0), 101 | firstPin(NULL) 102 | #ifdef PINMODE 103 | ,intrCount(0) 104 | #endif 105 | { 106 | #ifdef FLASH 107 | ledsetup(); 108 | #endif 109 | } 110 | volatile uint8_t& portInputReg; 111 | static int8_t attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode); 112 | static void detachInterrupt(uint8_t pin); 113 | INLINE_PCINT void PCint(); 114 | static volatile uint8_t curr; 115 | #ifndef NO_PIN_NUMBER 116 | static volatile uint8_t arduinoPin; 117 | #endif 118 | #ifndef NO_PIN_STATE 119 | static volatile uint8_t pinState; 120 | #endif 121 | #ifdef PINMODE 122 | static volatile uint8_t pinmode; 123 | static volatile uint8_t s_portRisingPins; 124 | static volatile uint8_t s_portFallingPins; 125 | static volatile uint8_t s_lastPinView; 126 | static volatile uint8_t s_pmask; 127 | static volatile char s_PORT; 128 | static volatile uint8_t s_changedPins; 129 | static volatile uint8_t s_portRisingPins_nCurr; 130 | static volatile uint8_t s_portFallingPins_nNCurr; 131 | static volatile uint8_t s_currXORlastPinView; 132 | volatile uint8_t intrCount; 133 | static volatile uint8_t s_count; 134 | static volatile uint8_t pcint_multi; 135 | static volatile uint8_t PCIFRbug; 136 | #endif 137 | #ifdef FLASH 138 | static void ledsetup(void); 139 | #endif 140 | 141 | protected: 142 | class PCintPin { 143 | public: 144 | PCintPin() : 145 | PCintFunc((PCIntvoidFuncPtr)NULL), 146 | mode(0) {} 147 | PCIntvoidFuncPtr PCintFunc; 148 | uint8_t mode; 149 | uint8_t mask; 150 | uint8_t arduinoPin; 151 | PCintPin* next; 152 | }; 153 | void enable(PCintPin* pin, PCIntvoidFuncPtr userFunc, uint8_t mode); 154 | int8_t addPin(uint8_t arduinoPin,PCIntvoidFuncPtr userFunc, uint8_t mode); 155 | volatile uint8_t& portPCMask; 156 | const uint8_t PCICRbit; 157 | volatile uint8_t portRisingPins; 158 | volatile uint8_t portFallingPins; 159 | volatile uint8_t lastPinView; 160 | PCintPin* firstPin; 161 | }; 162 | 163 | #ifndef LIBCALL_PINCHANGEINT // LIBCALL_PINCHANGEINT *********************************************** 164 | volatile uint8_t PCintPort::curr=0; 165 | #ifndef NO_PIN_NUMBER 166 | volatile uint8_t PCintPort::arduinoPin=0; 167 | #endif 168 | #ifndef NO_PIN_STATE 169 | volatile uint8_t PCintPort::pinState=0; 170 | #endif 171 | #ifdef PINMODE 172 | volatile uint8_t PCintPort::pinmode=0; 173 | volatile uint8_t PCintPort::s_portRisingPins=0; 174 | volatile uint8_t PCintPort::s_portFallingPins=0; 175 | volatile uint8_t PCintPort::s_lastPinView=0; 176 | volatile uint8_t PCintPort::s_pmask=0; 177 | volatile char PCintPort::s_PORT='x'; 178 | volatile uint8_t PCintPort::s_changedPins=0; 179 | volatile uint8_t PCintPort::s_portRisingPins_nCurr=0; 180 | volatile uint8_t PCintPort::s_portFallingPins_nNCurr=0; 181 | volatile uint8_t PCintPort::s_currXORlastPinView=0; 182 | volatile uint8_t PCintPort::s_count=0; 183 | volatile uint8_t PCintPort::pcint_multi=0; 184 | volatile uint8_t PCintPort::PCIFRbug=0; 185 | #endif 186 | 187 | #ifdef FLASH 188 | #define PINLED 13 189 | volatile uint8_t *led_port; 190 | uint8_t led_mask; 191 | uint8_t not_led_mask; 192 | boolean ledsetup_run=false; 193 | void PCintPort::ledsetup(void) { 194 | if (! ledsetup_run) { 195 | led_port=portOutputRegister(digitalPinToPort(PINLED)); 196 | led_mask=digitalPinToBitMask(PINLED); 197 | not_led_mask=led_mask^0xFF; 198 | pinMode(PINLED, OUTPUT); digitalWrite(PINLED, LOW); 199 | ledsetup_run=true; 200 | } 201 | }; 202 | #endif 203 | 204 | // 205 | // ATMEGA 644 206 | // 207 | #if defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) // Sanguino, Mosquino uino bobino bonanafannafofino, me my momino... 208 | 209 | #ifndef NO_PORTA_PINCHANGES 210 | PCintPort portA=PCintPort(1, 0,PCMSK0); // port PA==1 (from Arduino.h, Arduino version 1.0) 211 | #endif 212 | #ifndef NO_PORTB_PINCHANGES 213 | PCintPort portB=PCintPort(2, 1,PCMSK1); // port PB==2 (from Arduino.h, Arduino version 1.0) 214 | #endif 215 | #ifndef NO_PORTC_PINCHANGES 216 | PCintPort portC=PCintPort(3, 2,PCMSK2); // port PC==3 (also in pins_arduino.c, Arduino version 022) 217 | #endif 218 | #ifndef NO_PORTD_PINCHANGES 219 | PCintPort portD=PCintPort(4, 3,PCMSK3); // port PD==4 220 | #endif 221 | 222 | #else // others 223 | 224 | #ifndef NO_PORTB_PINCHANGES 225 | PCintPort portB=PCintPort(2, 0,PCMSK0); // port PB==2 (from Arduino.h, Arduino version 1.0) 226 | #endif 227 | #ifndef NO_PORTC_PINCHANGES // note: no PORTC on MEGA 228 | PCintPort portC=PCintPort(3, 1,PCMSK1); // port PC==3 (also in pins_arduino.c, Arduino version 022) 229 | #endif 230 | #ifndef NO_PORTD_PINCHANGES // note: no PORTD on MEGA 231 | PCintPort portD=PCintPort(4, 2,PCMSK2); // port PD==4 232 | #endif 233 | 234 | #endif // defined __AVR_ATmega644__ 235 | 236 | #ifdef __USE_PORT_JK 237 | #ifndef NO_PORTJ_PINCHANGES 238 | PCintPort portJ=PCintPort(10,1,PCMSK1); // port PJ==10 239 | #endif 240 | #ifndef NO_PORTK_PINCHANGES 241 | PCintPort portK=PCintPort(11,2,PCMSK2); // port PK==11 242 | #endif 243 | #endif // USE_PORT_JK 244 | 245 | static PCintPort *lookupPortNumToPort( int portNum ) { 246 | PCintPort *port = NULL; 247 | 248 | switch (portNum) { 249 | #ifndef NO_PORTA_PINCHANGES 250 | case 1: 251 | port=&portA; 252 | break; 253 | #endif 254 | #ifndef NO_PORTB_PINCHANGES 255 | case 2: 256 | port=&portB; 257 | break; 258 | #endif 259 | #ifndef NO_PORTC_PINCHANGES 260 | case 3: 261 | port=&portC; 262 | break; 263 | #endif 264 | #ifndef NO_PORTD_PINCHANGES 265 | case 4: 266 | port=&portD; 267 | break; 268 | #endif 269 | #ifdef __USE_PORT_JK 270 | 271 | #ifndef NO_PORTJ_PINCHANGES 272 | case 10: 273 | port=&portJ; 274 | break; 275 | #endif 276 | 277 | #ifndef NO_PORTK_PINCHANGES 278 | case 11: 279 | port=&portK; 280 | break; 281 | #endif 282 | 283 | #endif // __USE_PORT_JK 284 | } 285 | 286 | return port; 287 | } 288 | 289 | 290 | void PCintPort::enable(PCintPin* p, PCIntvoidFuncPtr userFunc, uint8_t mode) { 291 | // Enable the pin for interrupts by adding to the PCMSKx register. 292 | // ...The final steps; at this point the interrupt is enabled on this pin. 293 | p->mode=mode; 294 | p->PCintFunc=userFunc; 295 | #ifndef NO_PORTJ_PINCHANGES 296 | // A big shout out to jrhelbert for this fix! Thanks!!! 297 | if ((p->arduinoPin == 14) || (p->arduinoPin == 15)) { 298 | portPCMask |= (p->mask << 1); // PORTJ's PCMSK1 is a little odd... 299 | } 300 | else { 301 | portPCMask |= p->mask; 302 | } 303 | #else 304 | portPCMask |= p->mask; 305 | #endif 306 | if ((p->mode == RISING) || (p->mode == CHANGE)) portRisingPins |= p->mask; 307 | if ((p->mode == FALLING) || (p->mode == CHANGE)) portFallingPins |= p->mask; 308 | PCICR |= PCICRbit; 309 | } 310 | 311 | int8_t PCintPort::addPin(uint8_t arduinoPin, PCIntvoidFuncPtr userFunc, uint8_t mode) 312 | { 313 | PCintPin* tmp; 314 | 315 | tmp=firstPin; 316 | // Add to linked list, starting with firstPin. If pin already exists, just enable. 317 | if (firstPin != NULL) { 318 | do { 319 | if (tmp->arduinoPin == arduinoPin) { enable(tmp, userFunc, mode); return(0); } 320 | if (tmp->next == NULL) break; 321 | tmp=tmp->next; 322 | } while (true); 323 | } 324 | 325 | // Create pin p: fill in the data. 326 | PCintPin* p=new PCintPin; 327 | if (p == NULL) return(-1); 328 | p->arduinoPin=arduinoPin; 329 | p->mode = mode; 330 | p->next=NULL; 331 | p->mask = digitalPinToBitMask(arduinoPin); // the mask 332 | 333 | if (firstPin == NULL) firstPin=p; 334 | else tmp->next=p; // NOTE that tmp cannot be NULL. 335 | 336 | #ifdef DEBUG 337 | Serial.print("addPin. pin given: "); Serial.print(arduinoPin, DEC); 338 | int addr = (int) p; 339 | Serial.print(" instance addr: "); Serial.println(addr, HEX); 340 | Serial.print("userFunc addr: "); Serial.println((int)p->PCintFunc, HEX); 341 | #endif 342 | 343 | enable(p, userFunc, mode); 344 | #ifdef DEBUG 345 | Serial.print("addPin. pin given: "); Serial.print(arduinoPin, DEC), Serial.print (" pin stored: "); 346 | int addr = (int) p; 347 | Serial.print(" instance addr: "); Serial.println(addr, HEX); 348 | #endif 349 | return(1); 350 | } 351 | 352 | /* 353 | * attach an interrupt to a specific pin using pin change interrupts. 354 | */ 355 | int8_t PCintPort::attachInterrupt(uint8_t arduinoPin, PCIntvoidFuncPtr userFunc, int mode) 356 | { 357 | PCintPort *port; 358 | uint8_t portNum = digitalPinToPort(arduinoPin); 359 | if ((portNum == NOT_A_PORT) || (userFunc == NULL)) return(-1); 360 | 361 | port=lookupPortNumToPort(portNum); 362 | // Added by GreyGnome... must set the initial value of lastPinView for it to be correct on the 1st interrupt. 363 | // ...but even then, how do you define "correct"? Ultimately, the user must specify (not provisioned for yet). 364 | port->lastPinView=port->portInputReg; 365 | #ifdef DEBUG 366 | Serial.print("attachInterrupt- pin: "); Serial.println(arduinoPin, DEC); 367 | #endif 368 | // map pin to PCIR register 369 | return(port->addPin(arduinoPin,userFunc,mode)); 370 | } 371 | 372 | void PCintPort::detachInterrupt(uint8_t arduinoPin) 373 | { 374 | PCintPort *port; 375 | PCintPin* current; 376 | uint8_t mask; 377 | uint8_t portNum = digitalPinToPort(arduinoPin); 378 | if (portNum == NOT_A_PORT) return; 379 | port=lookupPortNumToPort(portNum); 380 | mask=digitalPinToBitMask(arduinoPin); 381 | current=port->firstPin; 382 | while (current) { 383 | if (current->mask == mask) { // found the target 384 | uint8_t oldSREG = SREG; 385 | cli(); // disable interrupts 386 | #ifndef NO_PORTJ_PINCHANGES 387 | // A big shout out to jrhelbert for this fix! Thanks!!! 388 | if ((arduinoPin == 14) || (arduinoPin == 15)) { 389 | port->portPCMask &= ~(mask << 1); // PORTJ's PCMSK1 is a little odd... 390 | } 391 | else { 392 | port->portPCMask &= ~mask; // disable the mask entry. 393 | } 394 | #else 395 | port->portPCMask &= ~mask; // disable the mask entry. 396 | #endif 397 | if (port->portPCMask == 0) PCICR &= ~(port->PCICRbit); 398 | port->portRisingPins &= ~current->mask; port->portFallingPins &= ~current->mask; 399 | // TODO: This is removed until we can add code that frees memory. 400 | // Note that in the addPin() function, above, we do not define a new pin if it was 401 | // once already defined. 402 | // ... ... 403 | // Link the previous' next to the found next. Then remove the found. 404 | //if (prev != NULL) prev->next=current->next; // linked list skips over current. 405 | //else firstPin=current->next; // at the first pin; save the new first pin 406 | SREG = oldSREG; // Restore register; reenables interrupts 407 | return; 408 | } 409 | current=current->next; 410 | } 411 | } 412 | 413 | // common code for isr handler. "port" is the PCINT number. 414 | // there isn't really a good way to back-map ports and masks to pins. 415 | void PCintPort::PCint() { 416 | 417 | #ifdef FLASH 418 | if (*led_port & led_mask) *led_port&=not_led_mask; 419 | else *led_port|=led_mask; 420 | #endif 421 | #ifndef DISABLE_PCINT_MULTI_SERVICE 422 | uint8_t pcifr; 423 | while (true) { 424 | #endif 425 | // get the pin states for the indicated port. 426 | #ifdef PINMODE 427 | PCintPort::s_lastPinView=lastPinView; 428 | intrCount++; 429 | PCintPort::s_count=intrCount; 430 | #endif 431 | uint8_t changedPins = (PCintPort::curr ^ lastPinView) & 432 | ((portRisingPins & PCintPort::curr ) | ( portFallingPins & ~PCintPort::curr )); 433 | 434 | #ifdef PINMODE 435 | PCintPort::s_currXORlastPinView=PCintPort::curr ^ lastPinView; 436 | PCintPort::s_portRisingPins_nCurr=portRisingPins & PCintPort::curr; 437 | PCintPort::s_portFallingPins_nNCurr=portFallingPins & ~PCintPort::curr; 438 | #endif 439 | lastPinView = PCintPort::curr; 440 | 441 | PCintPin* p = firstPin; 442 | while (p) { 443 | // Trigger interrupt if the bit is high and it's set to trigger on mode RISING or CHANGE 444 | // Trigger interrupt if the bit is low and it's set to trigger on mode FALLING or CHANGE 445 | if (p->mask & changedPins) { 446 | #ifndef NO_PIN_STATE 447 | PCintPort::pinState=PCintPort::curr & p->mask ? HIGH : LOW; 448 | #endif 449 | #ifndef NO_PIN_NUMBER 450 | PCintPort::arduinoPin=p->arduinoPin; 451 | #endif 452 | #ifdef PINMODE 453 | PCintPort::pinmode=p->mode; 454 | PCintPort::s_portRisingPins=portRisingPins; 455 | PCintPort::s_portFallingPins=portFallingPins; 456 | PCintPort::s_pmask=p->mask; 457 | PCintPort::s_changedPins=changedPins; 458 | #endif 459 | p->PCintFunc(); 460 | } 461 | p=p->next; 462 | } 463 | #ifndef DISABLE_PCINT_MULTI_SERVICE 464 | pcifr = PCIFR & PCICRbit; 465 | if (pcifr == 0) break; 466 | PCIFR |= PCICRbit; 467 | #ifdef PINMODE 468 | PCintPort::pcint_multi++; 469 | if (PCIFR & PCICRbit) PCintPort::PCIFRbug=1; // PCIFR & PCICRbit should ALWAYS be 0 here! 470 | #endif 471 | PCintPort::curr=portInputReg; 472 | } 473 | #endif 474 | } 475 | 476 | #ifndef NO_PORTA_PINCHANGES 477 | ISR(PCINT0_vect) { 478 | #ifdef PINMODE 479 | PCintPort::s_PORT='A'; 480 | #endif 481 | PCintPort::curr = portA.portInputReg; 482 | portA.PCint(); 483 | } 484 | #define PORTBVECT PCINT1_vect 485 | #define PORTCVECT PCINT2_vect 486 | #define PORTDVECT PCINT3_vect 487 | #else 488 | #define PORTBVECT PCINT0_vect 489 | #define PORTCVECT PCINT1_vect 490 | #define PORTDVECT PCINT2_vect 491 | #endif 492 | 493 | #ifndef NO_PORTB_PINCHANGES 494 | ISR(PORTBVECT) { 495 | #ifdef PINMODE 496 | PCintPort::s_PORT='B'; 497 | #endif 498 | PCintPort::curr = portB.portInputReg; 499 | portB.PCint(); 500 | } 501 | #endif 502 | 503 | #ifndef NO_PORTC_PINCHANGES 504 | ISR(PORTCVECT) { 505 | #ifdef PINMODE 506 | PCintPort::s_PORT='C'; 507 | #endif 508 | PCintPort::curr = portC.portInputReg; 509 | portC.PCint(); 510 | } 511 | #endif 512 | 513 | #ifndef NO_PORTD_PINCHANGES 514 | ISR(PORTDVECT){ 515 | #ifdef PINMODE 516 | PCintPort::s_PORT='D'; 517 | #endif 518 | PCintPort::curr = portD.portInputReg; 519 | portD.PCint(); 520 | } 521 | #endif 522 | 523 | #ifdef __USE_PORT_JK 524 | #ifndef NO_PORTJ_PINCHANGES 525 | ISR(PCINT1_vect) { 526 | #ifdef PINMODE 527 | PCintPort::s_PORT='J'; 528 | #endif 529 | PCintPort::curr = portJ.portInputReg; 530 | portJ.PCint(); 531 | } 532 | #endif 533 | 534 | #ifndef NO_PORTK_PINCHANGES 535 | ISR(PCINT2_vect){ 536 | #ifdef PINMODE 537 | PCintPort::s_PORT='K'; 538 | #endif 539 | PCintPort::curr = portK.portInputReg; 540 | portK.PCint(); 541 | } 542 | #endif 543 | 544 | #endif // __USE_PORT_JK 545 | 546 | #ifdef GET_PCINT_VERSION 547 | uint16_t getPCIntVersion () { 548 | return ((uint16_t) PCINT_VERSION); 549 | } 550 | #endif // GET_PCINT_VERSION 551 | #endif // #ifndef LIBCALL_PINCHANGEINT ************************************************************* 552 | #endif // #ifndef PinChangeInt_h ******************************************************************* 553 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/bst_abc.ino: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | Yahboom Intelligent Technology Co., Ltd. 3 | Product Name: Arduino Smart Balance Car 4 |   Product model: BST-ABC ver2.0 5 |   Change record: 181207 increase voltage acquisition display (liusen) 6 | ****************************************************************************/ 7 | 8 | 9 | #include "./PinChangeInt.h" 10 | #include "./MsTimer2.h" 11 | //Speed PID is controlled by speed dial counting 12 | #include "./BalanceCar.h" 13 | #include "./KalmanFilter.h" 14 | #include "./I2Cdev.h" 15 | #include "./MPU6050_6Axis_MotionApps20.h" 16 | #include "Wire.h" 17 | 18 | MPU6050 mpu; //实例化一个 MPU6050 对象，对象名称为 mpu 19 | BalanceCar balancecar; 20 | KalmanFilter kalmanfilter; 21 | int16_t ax, ay, az, gx, gy, gz; 22 | //TB6612FNG drive module control signal 23 | #define IN1M 7 24 | #define IN2M 6 25 | #define IN3M 13 26 | #define IN4M 12 27 | #define PWMA 9 28 | #define PWMB 10 29 | #define STBY 8 30 | 31 | #define PinA_left 2 //Interrupt 0 32 | #define PinA_right 4 //Interrupt 1 33 | 34 | //Declare custom variables 35 | float Voltage = 0; 36 | int voltagepin = A0; 37 | int time; 38 | byte inByte; //Serial port receive byte 39 | int num; 40 | double Setpoint; //Angle DIP set point, input, output 41 | double Setpoints, Outputs = 0; //Speed DIP set point, input, output 42 | double kp = 38, ki = 0.0, kd = 0.58; //The parameters you need to modify 43 | double kp_speed =3.8, ki_speed = 0.11, kd_speed = 0.0; //The parameters you need to modify 44 | double kp_turn = 28, ki_turn = 0, kd_turn = 0.29; //Rotate PID settings 45 | const double PID_Original[6] = {38, 0.0, 0.58,4.0, 0.12, 0.0}; //Restore default PID parameters 46 | //Rotate PID parameters 47 | double setp0 = 0, dpwm = 0, dl = 0; //Angle balance point, PWM difference, dead zone，PWM1，PWM2 48 | float value; 49 | 50 | 51 | //********************angle data*********************// 52 | float Q; 53 | float Angle_ax; //Tilt angle calculated from acceleration 54 | float Angle_ay; 55 | float K1 = 0.05; //Weighting the value of the accelerometer 56 | float angle0 = 0.00; //Mechanical balance angle 57 | int slong; 58 | //********************angle data*********************// 59 | 60 | //***************Kalman_Filter*********************// 61 | float Q_angle = 0.001, Q_gyro = 0.005; //Angle data confidence, angular velocity data confidence 62 | float R_angle = 0.5 , C_0 = 1; 63 | float timeChange = 5; 64 | float dt = timeChange * 0.001; //PS：The value of dt is the filter sampling time. 65 | //***************Kalman_Filter*********************// 66 | 67 | //********************************************* 68 | //******************** speed count ************ 69 | //********************************************* 70 | 71 | volatile long count_right = 0; 72 | //The volatile long type is used to ensure that the value is valid when the external interrupt pulse count value is used in other functions. 73 | volatile long count_left = 0; 74 | //The volatile long type is used to ensure that the value is valid when the external interrupt pulse count value is used in other functions. 75 | int speedcc = 0; 76 | 77 | //////////////////////Pulse calculation///////////////////////// 78 | int lz = 0; 79 | int rz = 0; 80 | int rpluse = 0; 81 | int lpluse = 0; 82 | int sumam; 83 | ///////////////////////////////////////////////// 84 | 85 | ///////////////////////////////////////////// 86 | int turncount = 0; //Steering intervention time calculation 87 | float turnoutput = 0; 88 | ///////////////////////////////////////////// 89 | 90 | ///////////////////////////////// 91 | #define run_car '1' 92 | #define back_car '2' 93 | #define left_car '3' 94 | #define right_car '4' 95 | #define stop_car '0' 96 | /*Car running status enumeration*/ 97 | enum { 98 | enSTOP = 0, 99 | enRUN, 100 | enBACK, 101 | enLEFT, 102 | enRIGHT, 103 | enTLEFT, 104 | enTRIGHT 105 | } enCarState; 106 | int incomingByte = 0; //Received data byte 107 | String inputString = ""; //Used to store received content 108 | boolean newLineReceived = false; //Previous data end mark 109 | boolean startBit = false; //Agreement start sign 110 | int g_carstate = enSTOP; //1front2back3left4right0stop 111 | String returntemp = ""; //Store return value 112 | boolean g_autoup = false; 113 | int g_uptimes = 5000; 114 | 115 | int front = 0;//front variable 116 | int back = 0;//back variable 117 | int turnl = 0;//Turn left sign 118 | int turnr = 0;//Turn right sign 119 | int spinl = 0;//Left rotation sign 120 | int spinr = 0;//Right rotation sign 121 | int bluetoothvalue;//Bluetooth control 122 | ///////////////////////////////// 123 | 124 | //////////////////////////////////// 125 | 126 | int chaoshengbo = 0; 127 | int tingzhi = 0; 128 | int jishi = 0; 129 | 130 | //////////////////////////////////// 131 | 132 | 133 | //////////////////////Pulse calculation/////////////////////// 134 | void countpluse() 135 | { 136 | 137 | lz = count_left; 138 | rz = count_right; 139 | 140 | count_left = 0; 141 | count_right = 0; 142 | 143 | lpluse = lz; 144 | rpluse = rz; 145 | 146 | if ((balancecar.pwm1 < 0) && (balancecar.pwm2 < 0)) //Judgment of the direction of movement of the car. 147 | //Back,PWM is the motor voltage is negative 148 | { 149 | rpluse = -rpluse; 150 | lpluse = -lpluse; 151 | } 152 | else if ((balancecar.pwm1 > 0) && (balancecar.pwm2 > 0)) //Judgment of the direction of movement of the car. 153 | //Advane,PWM is the motor voltage is positive 154 | { 155 | rpluse = rpluse; 156 | lpluse = lpluse; 157 | } 158 | else if ((balancecar.pwm1 < 0) && (balancecar.pwm2 > 0)) 159 | //Judgment of the direction of movement of the car. 160 | //Right rotation. (The number of right pulses is positive. The number of left pulses is negative.) 161 | { 162 | rpluse = rpluse; 163 | lpluse = -lpluse; 164 | } 165 | else if ((balancecar.pwm1 > 0) && (balancecar.pwm2 < 0)) //Judgment of the direction of movement of the car. 166 | //Left rotation. (The number of right pulses is negative. The number of left pulses is positive.) 167 | { 168 | rpluse = -rpluse; 169 | lpluse = lpluse; 170 | } 171 | 172 | //Determine if the car is lifted 173 | balancecar.stopr += rpluse; 174 | balancecar.stopl += lpluse; 175 | 176 | //The number of pulses is superimposed, every 5ms into the interrupt 177 | balancecar.pulseright += rpluse; 178 | balancecar.pulseleft += lpluse; 179 | sumam = balancecar.pulseright + balancecar.pulseleft; 180 | } 181 | /////////////////////////////////////////// 182 | 183 | 184 | 185 | ////////////////////////////////////// 186 | void angleout() 187 | { 188 | balancecar.angleoutput = kp * (kalmanfilter.angle + angle0) + kd * kalmanfilter.Gyro_x;//PD 角度环控制 189 | } 190 | ////////////////////////////////////// 191 | 192 | ////////////////////////////////////////////////////////// 193 | ////////////////////////////////////// 194 | ///////////////////////////////////////////////////////// 195 | void inter() 196 | { 197 | sei(); 198 | countpluse(); //Pulse superposition subfunction 199 | mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //IIC acquires six-axis data of MPU6050 ax ay az gx gy gz 200 | kalmanfilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1); 201 | angleout(); // Angle loop control 202 | 203 | speedcc++; 204 | if (speedcc >= 8) //40ms into the speed loop control 205 | { 206 | Outputs = balancecar.speedpiout(kp_speed, ki_speed, kd_speed, front, back, setp0); 207 | speedcc = 0; 208 | } 209 | turncount++; 210 | if (turncount > 4) //40ms into the rotation control 211 | { 212 | turnoutput = balancecar.turnspin(turnl, turnr, spinl, spinr, kp_turn, kd_turn, kalmanfilter.Gyro_z); 213 | turncount = 0; 214 | } 215 | balancecar.posture++; 216 | balancecar.pwma(Outputs, turnoutput, kalmanfilter.angle, kalmanfilter.angle6, turnl, turnr, spinl, spinr, front, back, kalmanfilter.accelz, IN1M, IN2M, IN3M, IN4M, PWMA, PWMB); //小车总PWM输出 217 | 218 | } 219 | ////////////////////////////////////////////////////////// 220 | ///////////////////////////////////// 221 | ///////////////////////////////////////////////////////// 222 | void SendAutoUp() 223 | { 224 | g_uptimes --; 225 | if ((g_autoup == true) && (g_uptimes == 0)) 226 | { 227 | 228 | String CSB;//, VT; 229 | char temp[10]={0}; 230 | float fgx; 231 | float fay; 232 | float leftspeed; 233 | float rightspeed; 234 | 235 | fgx = gx; 236 | fay = ay; 237 | leftspeed = balancecar.pwm1; 238 | rightspeed = balancecar.pwm2; 239 | 240 | double Gx = (double)((fgx - 128.1f) / 131.0f); //Angle conversion 241 | double Ay = ((double)fay / 16384.0f) * 9.8f; 242 | 243 | if(leftspeed > 255 || leftspeed < -255) 244 | return; 245 | if(rightspeed > 255 || rightspeed < -255) 246 | return; 247 | if((Ay < -20) || (Ay > 20)) 248 | return; 249 | if((Gx < -3000) || (Gx > 3000)) 250 | return; 251 | 252 | returntemp = ""; 253 | 254 | memset(temp, 0x00, sizeof(temp)); 255 | dtostrf(leftspeed, 3, 1, temp); // = %3.2f 256 | String LV = temp; 257 | 258 | memset(temp, 0x00, sizeof(temp)); 259 | dtostrf(rightspeed, 3, 1, temp); // = %3.1f 260 | String RV = temp; 261 | 262 | memset(temp, 0x00, sizeof(temp)); 263 | dtostrf(Ay, 2, 2, temp); // = %2.2f 264 | String AC = temp; 265 | 266 | memset(temp, 0x00, sizeof(temp)); 267 | dtostrf(Gx, 4, 2, temp); // = %4.2f 268 | String GY = temp; 269 | 270 | memset(temp, 0x00, sizeof(temp)); 271 | dtostrf(Voltage, 2, 1, temp); // = %2.1f 272 | 273 | String VT = temp; 274 | 275 | CSB = "0.00"; 276 | returntemp = "$LV" + LV + ",RV" + RV + ",AC" + AC + ",GY" + GY + ",CSB" + CSB + ",VT" + VT + "#"; 277 | Serial.print(returntemp); //Return protocol packet 278 | } 279 | 280 | if (g_uptimes == 0) 281 | g_uptimes = 5000; 282 | } 283 | 284 | 285 | // === === 286 | void setup() { 287 | 288 | // Serial.begin(9600); 289 | pinMode(IN1M, OUTPUT); //Control the direction of the motor 1, 01 is forward rotation, 10 is reverse rotation 290 | pinMode(IN2M, OUTPUT); 291 | pinMode(IN3M, OUTPUT); //Control the direction of the motor 2, 01 is forward rotation, 10 is reverse rotation 292 | pinMode(IN4M, OUTPUT); 293 | pinMode(PWMA, OUTPUT); //Left motor PWM 294 | pinMode(PWMB, OUTPUT); //Right motor PWM 295 | pinMode(STBY, OUTPUT); //TB6612FNG enable 296 | 297 | 298 | //Initialize motor dirve module 299 | digitalWrite(IN1M, 0); 300 | digitalWrite(IN2M, 1); 301 | digitalWrite(IN3M, 1); 302 | digitalWrite(IN4M, 0); 303 | digitalWrite(STBY, 1); 304 | analogWrite(PWMA, 0); 305 | analogWrite(PWMB, 0); 306 | 307 | pinMode(PinA_left, INPUT); 308 | pinMode(PinA_right, INPUT); 309 | 310 | // 加入I2C总线 311 | Wire.begin(); //Join the I2C bus sequence 312 | Serial.begin(9600); //Open the serial port and set the baud rate to 115200. 313 | delay(1500); 314 | mpu.initialize(); //Initialize MPU6050 315 | delay(2); 316 | balancecar.pwm1 = 0; 317 | balancecar.pwm2 = 0; 318 | 319 | //5ms timer interrupt setting, Use Timer2 320 | //Note: Using timer2 will affect the PWM output of pin3 pin11, because the PWM uses the timer to control the duty cycle, 321 | //so pay attention to check the pin port of the corresponding timer when using the timer. 322 | MsTimer2::set(5, inter); 323 | MsTimer2::start(); 324 | 325 | } 326 | 327 | ////////////////////////////////////////turn////////////////////////////////// 328 | 329 | void ResetPID() 330 | { 331 | kp = PID_Original[0]; 332 | ki = PID_Original[1]; 333 | kd = PID_Original[2]; //you need to modify the parameters 334 | kp_speed = PID_Original[3]; 335 | ki_speed = PID_Original[4]; 336 | kd_speed = PID_Original[5]; //you need to modify the parameters 337 | } 338 | void ResetCarState() 339 | { 340 | turnl = 0; 341 | turnr = 0; 342 | front = 0; 343 | back = 0; 344 | spinl = 0; 345 | spinr = 0; 346 | turnoutput = 0; 347 | } 348 | 349 | float voltage_test() 350 | { 351 | float fVoltage; 352 | int iVoltage; 353 | pinMode(voltagepin, INPUT); 354 | iVoltage = analogRead(A0); //Get value of A0 355 | fVoltage = (iVoltage / 1023.0) * 5*4.2-0.35; //Converted to voltage value 356 | //Voltage is the ad value (0-1023) collected by port A0. 357 | //0.35 is the adjustment value due to the accuracy of the resistor. 358 | return fVoltage; 359 | } 360 | // === === 361 | void loop() { 362 | Voltage=voltage_test(); 363 | //Serial.print(Voltage,DEC); 364 | // Serial.print("\r\n"); 365 | 366 | String returnstr = "$0,0,0,0,0,0,0,0,0,0,0,0cm,0.0V#"; //Default send 367 | 368 | 369 | attachInterrupt(0, Code_left, CHANGE); 370 | attachPinChangeInterrupt(PinA_right, Code_right, CHANGE); 371 | 372 | //kongzhi(); //Bluetooth interface 373 | // 374 | 375 | //Serial.println(kalmanfilter.angle); 376 | //Serial.print("\t"); 377 | //Serial.print(bluetoothvalue); 378 | //Serial.print("\t"); 379 | // Serial.print( balancecar.angleoutput); 380 | // Serial.print("\t"); 381 | // Serial.print(balancecar.pwm1); 382 | // Serial.print("\t"); 383 | // Serial.println(balancecar.pwm2); 384 | // Serial.print("\t"); 385 | // Serial.println(balancecar.stopr); 386 | // Serial.print("\t"); 387 | 388 | if (newLineReceived) 389 | { 390 | switch (inputString[1]) 391 | { 392 | case run_car: g_carstate = enRUN; break; 393 | case back_car: g_carstate = enBACK; break; 394 | case left_car: g_carstate = enLEFT; break; 395 | case right_car: g_carstate = enRIGHT; break; 396 | case stop_car: g_carstate = enSTOP; break; 397 | default: g_carstate = enSTOP; break; 398 | } 399 | //Determine if the protocol has lost packets 400 | /* if(inputString.length() < 21) 401 | { 402 | 403 | inputString = ""; // clear the string 404 | newLineReceived = false; 405 | //Serial.print(returnstr); 406 | goto a; 407 | }*/ 408 | if (inputString[3] == '1' && inputString.length() == 21) //Left shake 409 | { 410 | g_carstate = enTLEFT; 411 | //Serial.print(returnstr); 412 | } 413 | else if (inputString[3] == '2' && inputString.length() == 21) //Right shake 414 | { 415 | g_carstate = enTRIGHT; 416 | // Serial.print(returnstr); 417 | } 418 | 419 | if (inputString[5] == '1') //Inquire PID 420 | { 421 | char charkp[7], charkd[7], charkpspeed[7], charkispeed[7]; 422 | 423 | dtostrf(kp, 3, 2, charkp); // = %3.2f 424 | dtostrf(kd, 3, 2, charkd); // = %3.2f 425 | dtostrf(kp_speed, 3, 2, charkpspeed); // = %3.2f 426 | dtostrf(ki_speed, 3, 2, charkispeed); // = %3.2f 427 | 428 | String strkp = charkp; String strkd = charkd; String strkpspeed = charkpspeed; String strkispeed = charkispeed; 429 | 430 | returntemp = "$0,0,0,0,0,0,AP" + strkp + ",AD" + strkd + ",VP" + strkpspeed + ",VI" + strkispeed + "#"; 431 | 432 | Serial.print(returntemp); //Return protocol packet 433 | } 434 | else if (inputString[5] == '2') //Restore PID 435 | { 436 | ResetPID(); 437 | Serial.print("$OK#"); //Return protocol packet 438 | } 439 | 440 | if (inputString[7] == '1') //automatic reporting 441 | { 442 | g_autoup = true; 443 | Serial.print("$OK#"); //Return protocol packet 444 | } 445 | else if (inputString[7] == '2') //stop automatic reporting 446 | { 447 | g_autoup = false; 448 | Serial.print("$OK#"); //Return protocol packet 449 | } 450 | 451 | if (inputString[9] == '1') //Angle loop update $0,0,0,0,1,1,AP23.54,AD85.45,VP10.78,VI0.26# 452 | { 453 | int i = inputString.indexOf("AP"); 454 | int ii = inputString.indexOf(",", i); 455 | if(ii > i) 456 | { 457 | String m_skp = inputString.substring(i + 2, ii); 458 | m_skp.replace(".", ""); 459 | int m_kp = m_skp.toInt(); 460 | kp = (double)( (double)m_kp / 100.0f); 461 | } 462 | 463 | 464 | i = inputString.indexOf("AD"); 465 | ii = inputString.indexOf(",", i); 466 | if(ii > i) 467 | { 468 | //ki = inputString.substring(i+2, ii); 469 | String m_skd = inputString.substring(i + 2, ii); 470 | m_skd.replace(".", ""); 471 | int m_kd = m_skd.toInt(); 472 | kd = (double)( (double)m_kd / 100.0f); 473 | } 474 | Serial.print("$OK#"); //Return protocol packet 475 | } 476 | 477 | if (inputString[11] == '1') //Speed loop update 478 | { 479 | int i = inputString.indexOf("VP"); 480 | int ii = inputString.indexOf(",", i); 481 | if(ii > i) 482 | { 483 | String m_svp = inputString.substring(i + 2, ii); 484 | m_svp.replace(".", ""); 485 | int m_vp = m_svp.toInt(); 486 | kp_speed = (double)( (double)m_vp / 100.0f); 487 | } 488 | 489 | 490 | i = inputString.indexOf("VI"); 491 | ii = inputString.indexOf("#", i); 492 | if(ii > i) 493 | { 494 | String m_svi = inputString.substring(i + 2, ii); 495 | m_svi.replace(".", ""); 496 | int m_vi = m_svi.toInt(); 497 | ki_speed = (double)( (double)m_vi / 100.0f); 498 | Serial.print("$OK#"); //Return protocol packet 499 | } 500 | 501 | } 502 | //Restore default state 503 | inputString = ""; // clear the string 504 | newLineReceived = false; 505 | 506 | } 507 | 508 | a: switch (g_carstate) 509 | { 510 | case enSTOP: turnl = 0; turnr = 0; front = 0; back = 0; spinl = 0; spinr = 0; turnoutput = 0; break; 511 | case enRUN: ResetCarState();front = 250; break; 512 | case enLEFT: turnl = 1; break; 513 | case enRIGHT: turnr = 1; break; 514 | case enBACK: ResetCarState();back = -250; break; 515 | case enTLEFT: spinl = 1; break; 516 | case enTRIGHT: spinr = 1; break; 517 | default: front = 0; back = 0; turnl = 0; turnr = 0; spinl = 0; spinr = 0; turnoutput = 0; break; 518 | } 519 | 520 | //Increase automatic reporting 521 | SendAutoUp(); 522 | 523 | 524 | } 525 | 526 | ////////////////////////////////////////pwm/////////////////////////////////// 527 | 528 | 529 | 530 | //////////////////////////Pulse interrupt calculation///////////////////////////////////// 531 | 532 | void Code_left() { 533 | 534 | count_left ++; 535 | 536 | } //Left speed dial count 537 | 538 | 539 | 540 | void Code_right() { 541 | 542 | count_right ++; 543 | 544 | } //Right speed dial count 545 | 546 | //////////////////////////Pulse interrupt calculation///////////////////////////////////// 547 | 548 | //serialEvent() is a new feature in IDE 1.0 and later. 549 | int num1 = 0; 550 | void serialEvent() 551 | { 552 | 553 | while (Serial.available()) 554 | { 555 | incomingByte = Serial.read(); //Read one byte by byte, put the read data into a string array to form a completed packet 556 | if (incomingByte == '$') 557 | { 558 | num1 = 0; 559 | startBit = true; 560 | } 561 | if (startBit == true) 562 | { 563 | num1++; 564 | inputString += (char) incomingByte; //Full-duplex serial port does not need to add delay below, half-duplex need to add delay below 565 | } 566 | if (startBit == true && incomingByte == '#') 567 | { 568 | newLineReceived = true; 569 | startBit = false; 570 | } 571 | 572 | if(num1 >= 80) 573 | { 574 | num1 = 0; 575 | startBit = false; 576 | newLineReceived = false; 577 | inputString = ""; 578 | } 579 | } 580 | } 581 | 582 | 583 | -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/helper_3dmath.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper 2 | // 6/5/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-05 - add 3D math helper file to DMP6 example sketch 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | #ifndef _HELPER_3DMATH_H_ 33 | #define _HELPER_3DMATH_H_ 34 | 35 | class Quaternion { 36 | public: 37 | float w; 38 | float x; 39 | float y; 40 | float z; 41 | 42 | Quaternion() { 43 | w = 1.0f; 44 | x = 0.0f; 45 | y = 0.0f; 46 | z = 0.0f; 47 | } 48 | 49 | Quaternion(float nw, float nx, float ny, float nz) { 50 | w = nw; 51 | x = nx; 52 | y = ny; 53 | z = nz; 54 | } 55 | 56 | Quaternion getProduct(Quaternion q) { 57 | // Quaternion multiplication is defined by: 58 | // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) 59 | // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) 60 | // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) 61 | // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 62 | return Quaternion( 63 | w*q.w - x*q.x - y*q.y - z*q.z, // new w 64 | w*q.x + x*q.w + y*q.z - z*q.y, // new x 65 | w*q.y - x*q.z + y*q.w + z*q.x, // new y 66 | w*q.z + x*q.y - y*q.x + z*q.w); // new z 67 | } 68 | 69 | Quaternion getConjugate() { 70 | return Quaternion(w, -x, -y, -z); 71 | } 72 | 73 | float getMagnitude() { 74 | return sqrt(w*w + x*x + y*y + z*z); 75 | } 76 | 77 | void normalize() { 78 | float m = getMagnitude(); 79 | w /= m; 80 | x /= m; 81 | y /= m; 82 | z /= m; 83 | } 84 | 85 | Quaternion getNormalized() { 86 | Quaternion r(w, x, y, z); 87 | r.normalize(); 88 | return r; 89 | } 90 | }; 91 | 92 | class VectorInt16 { 93 | public: 94 | int16_t x; 95 | int16_t y; 96 | int16_t z; 97 | 98 | VectorInt16() { 99 | x = 0; 100 | y = 0; 101 | z = 0; 102 | } 103 | 104 | VectorInt16(int16_t nx, int16_t ny, int16_t nz) { 105 | x = nx; 106 | y = ny; 107 | z = nz; 108 | } 109 | 110 | float getMagnitude() { 111 | return sqrt(x*x + y*y + z*z); 112 | } 113 | 114 | void normalize() { 115 | float m = getMagnitude(); 116 | x /= m; 117 | y /= m; 118 | z /= m; 119 | } 120 | 121 | VectorInt16 getNormalized() { 122 | VectorInt16 r(x, y, z); 123 | r.normalize(); 124 | return r; 125 | } 126 | 127 | void rotate(Quaternion *q) { 128 | // http://www.cprogramming.com/tutorial/3d/quaternions.html 129 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm 130 | // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation 131 | // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 132 | 133 | // P_out = q * P_in * conj(q) 134 | // - P_out is the output vector 135 | // - q is the orientation quaternion 136 | // - P_in is the input vector (a*aReal) 137 | // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) 138 | Quaternion p(0, x, y, z); 139 | 140 | // quaternion multiplication: q * p, stored back in p 141 | p = q -> getProduct(p); 142 | 143 | // quaternion multiplication: p * conj(q), stored back in p 144 | p = p.getProduct(q -> getConjugate()); 145 | 146 | // p quaternion is now [0, x', y', z'] 147 | x = p.x; 148 | y = p.y; 149 | z = p.z; 150 | } 151 | 152 | VectorInt16 getRotated(Quaternion *q) { 153 | VectorInt16 r(x, y, z); 154 | r.rotate(q); 155 | return r; 156 | } 157 | }; 158 | 159 | class VectorFloat { 160 | public: 161 | float x; 162 | float y; 163 | float z; 164 | 165 | VectorFloat() { 166 | x = 0; 167 | y = 0; 168 | z = 0; 169 | } 170 | 171 | VectorFloat(float nx, float ny, float nz) { 172 | x = nx; 173 | y = ny; 174 | z = nz; 175 | } 176 | 177 | float getMagnitude() { 178 | return sqrt(x*x + y*y + z*z); 179 | } 180 | 181 | void normalize() { 182 | float m = getMagnitude(); 183 | x /= m; 184 | y /= m; 185 | z /= m; 186 | } 187 | 188 | VectorFloat getNormalized() { 189 | VectorFloat r(x, y, z); 190 | r.normalize(); 191 | return r; 192 | } 193 | 194 | void rotate(Quaternion *q) { 195 | Quaternion p(0, x, y, z); 196 | 197 | // quaternion multiplication: q * p, stored back in p 198 | p = q -> getProduct(p); 199 | 200 | // quaternion multiplication: p * conj(q), stored back in p 201 | p = p.getProduct(q -> getConjugate()); 202 | 203 | // p quaternion is now [0, x', y', z'] 204 | x = p.x; 205 | y = p.y; 206 | z = p.z; 207 | } 208 | 209 | VectorFloat getRotated(Quaternion *q) { 210 | VectorFloat r(x, y, z); 211 | r.rotate(q); 212 | return r; 213 | } 214 | }; 215 | 216 | #endif /* _HELPER_3DMATH_H_ */ -------------------------------------------------------------------------------- /3.Arduino balance car code_2019_2_14/bst_abc/readme.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/3.Arduino balance car code_2019_2_14/bst_abc/readme.txt -------------------------------------------------------------------------------- /4.SCH/Arduino balance car schematic_new version.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/4.SCH/Arduino balance car schematic_new version.pdf -------------------------------------------------------------------------------- /4.SCH/Arduino balance car schematic_old version.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/4.SCH/Arduino balance car schematic_old version.pdf -------------------------------------------------------------------------------- /4.SCH/Readme.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/4.SCH/Readme.png -------------------------------------------------------------------------------- /5.Charging and battery/1.Charging method and battery use precautions.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/5.Charging and battery/1.Charging method and battery use precautions.pdf -------------------------------------------------------------------------------- /Note.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/Note.docx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino-Balance-Car 2 | ![](https://github.com/YahboomTechnology/Arduino-Balance-Car/blob/master/balance_robot.jpg) 3 | # Introduction 4 | Self Balance robot is a stable, rugged and durable two-wheel balance vehicle. We optimized the balancer algorithm, and the excellent software algorithm makes Arduino UNO as the core controller of the balance car. The car is a three-layer structure with a load capacity of more than 2KG. We adopt simple and clear modular design, which is suitable for balance car beginners. Arduino UNO board can be taken down easily for secondary development. You can control the car by Android mobile App remote controller connecting with Bluetooth module on the vehicle. This is a multi-functional, autonomous balanced two-wheel vehicle for all types of competitions and research projects. 5 | # Feature 6 | * RTR robot with directly charge system, you can play with it any time. 7 | * This robot adopts antiskid tires, aluminum alloy chassis, high-power motors, and high-precision AB incremental hall encoder. 8 | * The high-quality integrated circuit board is used as the core board of the car, and some direct plug interfaces for insertion of modules are designed on the core board. 9 | * It can be controlled by Bluetooth App remote controller by Android mobile. The APP supports multiple functions: changing control method at any time, such as buttons, gravity, and rocker; viewing and debugging the current PID parameters of the vehicle directly; displaying the waveform of acceleration, gyroscope, and voltage simultaneously. 10 | * The open source code is PID Angle fusion and balance algorithm. After our debugging carefully, we ensure that every vehicle can be used normally. 11 | * The vehicle can control the switch of the balance system automatically by posture recognition. 12 | # Required Best Buy Links 13 | [By on Amazon](https://www.amazon.com/Yahboom-Compatible-Electronics-Programmable-Education/dp/B07FL2QR1V/ref=sr_1_32?m=A1N1A77RUX51FT&marketplaceID=ATVPDKIKX0DER&qid=1567599273&s=merchant-items&sr=1-32) 14 | 15 | [By on Yahboom website](https://category.yahboom.net/collections/a-smart-robot/products/balancecar) 16 | 17 | # Please Contact Us 18 | If you have any problem when using our robot after checking the tutorial, please contact us. 19 | ### Facebook: 20 | https://www.facebook.com/yahboomcar/ 21 | 22 | https://www.facebook.com/yahboomtech 23 | ### WhatsApp: 24 | +86 18682378128 25 | ### Technical support email: 26 | support@yahboom.com 27 | 28 | -------------------------------------------------------------------------------- /balance car Bluetooth communication protocol.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/balance car Bluetooth communication protocol.pdf -------------------------------------------------------------------------------- /balance car Bluetooth communication protocol.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/balance car Bluetooth communication protocol.xlsx -------------------------------------------------------------------------------- /balance_robot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YahboomTechnology/Arduino-Balance-Car/6debb237629c8e7fa0e7d444deb79aebb4e5b192/balance_robot.jpg --------------------------------------------------------------------------------