diff --git a/Buttons.cpp b/Buttons.cpp index 46bbd11..e6e1f8b 100644 --- a/Buttons.cpp +++ b/Buttons.cpp @@ -1,29 +1,50 @@ /* * Buttons.c * - * Created on: 6 mars 2016 - * author: Sebastien CAPOU (neskweek@gmail.com) + * Created on: 21 Octber 2016 + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS */ -#include + #include "Buttons.h" #include "Config.h" #include "SoundFont.h" -extern DFPlayer dfplayer; + extern SoundFont soundFont; -extern bool actionMode; -extern bool configMode; +enum SaberStateEnum {S_STANDBY, S_SABERON, S_CONFIG, S_SLEEP, S_JUKEBOX}; +enum ActionModeSubStatesEnum {AS_HUM, AS_IGNITION, AS_RETRACTION, AS_BLADELOCKUP, AS_BLASTERDEFLECTMOTION, AS_BLASTERDEFLECTPRESS, AS_CLASH, AS_SWING, AS_SPIN, AS_FORCE}; +extern SaberStateEnum SaberState; +extern SaberStateEnum PrevSaberState; +extern ActionModeSubStatesEnum ActionModeSubStates; +//extern bool actionMode; +//extern bool configMode; extern unsigned long sndSuppress; +extern bool hum_playing; extern int8_t modification; extern bool play; -extern bool blasterBlocks; -extern bool lockup; +#ifdef JUKEBOX +extern bool jukebox_play; +extern uint8_t jb_track; +#endif +//extern bool blasterBlocks; +//extern bool lockup; extern int8_t blink; extern bool changeMenu; extern uint8_t menu; extern bool enterMenu; - +#if defined LEDSTRINGS +//extern uint8_t ledPins[] = { LEDSTRING1, LEDSTRING2, LEDSTRING3, LEDSTRING4, +//LEDSTRING5, LEDSTRING6 }; +extern uint8_t blasterPin; +#endif +extern uint8_t blaster; +extern void HumRelaunch(); +extern void SinglePlay_Sound(uint8_t track); +extern void LoopPlay_Sound(uint8_t track); +extern void Pause_Sound(); +extern void Resume_Sound(); +extern void Set_Loop_Playback(); // ==================================================================================== // === BUTTONS CALLBACK FUNCTIONS === // ==================================================================================== @@ -32,33 +53,49 @@ void mainClick() { #if defined LS_BUTTON_DEBUG Serial.println(F("Main button click.")); #endif - if (actionMode) { + if (SaberState==S_SABERON) { if (soundFont.getForce()) { - + ActionModeSubStates=AS_FORCE; // Some Soundfont may not have Force sounds if (millis() - sndSuppress >= 30) { - dfplayer.playPhysicalTrack(soundFont.getForce()); + SinglePlay_Sound(soundFont.getForce()); sndSuppress = millis(); } } - } else if (configMode) { + } else if (SaberState==S_CONFIG) { //Button "+" modification = 1; play = true; - } else if (!configMode && !actionMode) { + } else if (SaberState==S_STANDBY) { // LightSaber poweron - actionMode = true; - ; + SaberState=S_SABERON; + PrevSaberState=S_STANDBY; + ActionModeSubStates=AS_IGNITION; + //actionMode = true; + } +#ifdef JUKEBOX + else if (SaberState==S_JUKEBOX) { +#if defined LS_BUTTON_DEBUG + Serial.print(F("Next JukeBox sound file "));Serial.print(jb_track); +#endif // jump to next song and start playing it + if (jb_track==NR_CONFIGFOLDERFILES+NR_JUKEBOXSONGS) { + jb_track=NR_CONFIGFOLDERFILES+1; // fold back to first song in the dir designated for music playback + } + else { + jb_track++; + } + SinglePlay_Sound(jb_track); } +#endif } // mainClick void mainDoubleClick() { #if defined LS_BUTTON_DEBUG Serial.println(F("Main button double click.")); #endif - if (actionMode) { + if (SaberState==S_SABERON) { //ACTION TO DEFINE - } else if (configMode) { + } else if (SaberState==S_CONFIG) { // Trigger needs to be hardened with some sort of double click combinaison //RESET CONFIG @@ -69,7 +106,7 @@ void mainDoubleClick() { // // } // } - } else if (!configMode && !actionMode) { + } else if (SaberState==S_STANDBY) { //ACTION TO DEFINE } } // mainDoubleClick @@ -78,21 +115,27 @@ void mainLongPressStart() { #if defined LS_BUTTON_DEBUG Serial.println(F("Main button longPress start")); #endif - if (actionMode) { + if (SaberState==S_SABERON) { // LightSaber shutdown - actionMode = false; - } else if (configMode) { + ActionModeSubStates=AS_RETRACTION; + SaberState=S_STANDBY; + PrevSaberState=S_SABERON; + //actionMode = false; + } else if (SaberState==S_CONFIG) { // Change Menu changeMenu = true; enterMenu = true; menu++; #if defined LUXEON - if (menu==4){menu=7;} + if (menu==5){menu=0;} // 3 menu items #endif #if defined LEDSTRINGS - if (menu==2){menu=4;} + if (menu==2){menu=0;} // 2 menu items +#endif +#if defined NEOPIXEL + if (menu==5){menu=0;} // 4 menu items #endif - } else if (!configMode && !actionMode) { + } else if (SaberState==S_STANDBY) { /* * ACTION TO DEFINE */ @@ -103,16 +146,16 @@ void mainLongPress() { #if defined LS_BUTTON_DEBUG Serial.println(F("Main button longPress...")); #endif - if (actionMode) { + if (SaberState==S_SABERON) { /* * ACTION TO DEFINE */ - } else if (configMode) { + } else if (SaberState==S_CONFIG) { /* * ACTION TO DEFINE */ - } else if (!configMode && !actionMode) { + } else if (SaberState==S_STANDBY) { /* * ACTION TO DEFINE */ @@ -123,7 +166,7 @@ void mainLongPressStop() { #if defined LS_BUTTON_DEBUG Serial.println(F("Main button longPress stop")); #endif - if (!configMode && !actionMode) { + if (SaberState==S_STANDBY) { /* * ACTION TO DEFINE */ @@ -134,63 +177,117 @@ void lockupClick() { #if defined LS_BUTTON_DEBUG Serial.println(F("Lockup button click.")); #endif - if (actionMode) { + if (SaberState==S_SABERON) { // Blaster - - blasterBlocks = !blasterBlocks; - - } else if (configMode) { +#if defined LS_BUTTON_DEBUG + Serial.println(F("Start button activated blaster bolt deflect")); +#endif + ActionModeSubStates=AS_BLASTERDEFLECTPRESS; + } else if (SaberState==S_CONFIG) { // Button "-" modification = -1; play = true; - } else if (!configMode && !actionMode) { + } else if (SaberState==S_STANDBY) { /* * ACTION TO DEFINE */ } +#ifdef JUKEBOX + else if (SaberState==S_JUKEBOX) { + if (jukebox_play) { + // pause the song +#if defined LS_BUTTON_DEBUG + Serial.println(F("Pause Song")); +#endif + jukebox_play=false; + Pause_Sound(); + } else { + // resume playing the song +#if defined LS_BUTTON_DEBUG + Serial.println(F("Resume Song")); +#endif + jukebox_play=true; + Resume_Sound(); + } + + } +#endif + } // lockupClick void lockupDoubleClick() { #if defined LS_BUTTON_DEBUG Serial.println(F("Lockup button double click.")); #endif - if (actionMode) { - //ACTION TO DEFINE - } else if (configMode) { - //ACTION TO DEFINE - } else if (!configMode && !actionMode) { + if (SaberState==S_SABERON) { +#if defined LS_BUTTON_DEBUG + Serial.println(F("Start motion triggered blaster bolt deflect")); +#endif + if (ActionModeSubStates!=AS_BLASTERDEFLECTMOTION) { // start motion triggered blaster deflect + ActionModeSubStates=AS_BLASTERDEFLECTMOTION; +#if defined LS_BUTTON_DEBUG + Serial.println(F("Start motion triggered blaster bolt deflect")); +#endif + } + else { // stop motion triggered blaster deflect +#if defined LS_BUTTON_DEBUG + Serial.println(F("End motion triggered blaster bolt deflect")); +#endif + HumRelaunch(); + ActionModeSubStates=AS_HUM; + } + } else if (SaberState==S_CONFIG) { //ACTION TO DEFINE } +#ifdef JUKEBOX + else if (SaberState==S_STANDBY) { +#if defined LS_BUTTON_DEBUG + Serial.println(F("Enter JukeBox")); +#endif + SaberState=S_JUKEBOX; + PrevSaberState=S_STANDBY; + //ACTION TO DEFINE + } else if (SaberState==S_JUKEBOX) { +//Entering JukeBox mode (MP3 player) + SaberState=S_STANDBY; + PrevSaberState=S_JUKEBOX; +// stop/pause track being played + Pause_Sound(); + } +#endif } // lockupDoubleClick void lockupLongPressStart() { #if defined LS_BUTTON_DEBUG Serial.println(F("Lockup button longPress start")); #endif - if (actionMode) { + if (SaberState==S_SABERON) { //Lockup Start - lockup = true; + ActionModeSubStates=AS_BLADELOCKUP; blink=0; -// Serial.println(soundFont.getLockup()); if (soundFont.getLockup()) { - dfplayer.playPhysicalTrack(soundFont.getLockup()); + SinglePlay_Sound(soundFont.getLockup()); sndSuppress = millis(); while (millis() - sndSuppress < 50) { } - dfplayer.setSingleLoop(true); + Set_Loop_Playback(); sndSuppress = millis(); while (millis() - sndSuppress < 50) { } } - } else if (configMode) { + } else if (SaberState==S_CONFIG) { //Leaving Config Mode changeMenu = false; // repeat = true; - configMode = false; + SaberState=S_STANDBY; + PrevSaberState=S_CONFIG; + //configMode = false; - } else if (!configMode && !actionMode) { + } else if (SaberState==S_STANDBY) { //Entering Config Mode - configMode = true; + SaberState=S_CONFIG; + PrevSaberState=S_STANDBY; + //configMode = true; } } // lockupLongPressStart @@ -199,15 +296,17 @@ void lockupLongPress() { #if defined LS_BUTTON_DEBUG Serial.println(F("Lockup button longPress...")); #endif - if (actionMode) { + if (SaberState==S_SABERON) { /* * ACTION TO DEFINE */ - } else if (configMode) { + ActionModeSubStates=AS_BLADELOCKUP; // needed, otherwise the FSM will change to AS_HUM and the lockup will end prematurely when the hum is relaunched + sndSuppress = millis(); // trick the hum relaunch by starting the stopper all over again otherwise the hum relaunch will interrupt the lockup + } else if (SaberState==S_CONFIG) { /* * ACTION TO DEFINE */ - } else if (!configMode && !actionMode) { + } else if (SaberState==S_STANDBY) { /* * ACTION TO DEFINE */ @@ -218,13 +317,10 @@ void lockupLongPressStop() { #if defined LS_BUTTON_DEBUG Serial.println(F("Lockup button longPress stop")); #endif - if (actionMode) { -//Lockup Stop - lockup = false; - dfplayer.playPhysicalTrack(soundFont.getHum()); - delay(70); - dfplayer.setSingleLoop(true); - sndSuppress = millis(); + if (SaberState==S_SABERON) { + HumRelaunch(); + ActionModeSubStates=AS_HUM; } } // lockupLongPressStop + diff --git a/Buttons.h b/Buttons.h index 5acc5b8..f7c5054 100644 --- a/Buttons.h +++ b/Buttons.h @@ -1,8 +1,8 @@ /* * Buttons.h * - * Created on: 6 mars 2016 - * author: Sebastien CAPOU (neskweek@gmail.com) + * Created on: 21 Octber 2016 + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS */ #include @@ -13,9 +13,9 @@ /* * BUTTONS PARAMETERS ************************************/ -#define CLICK 5 // ms you need to press a button to be a click -#define PRESS_ACTION 200 // ms you need to press a button to be a long press, in action mode -#define PRESS_CONFIG 400 // ms you need to press a button to be a long press, in config mode +#define CLICK 200 //5 // ms you need to press a button to be a click +#define PRESS_ACTION 600 //200 // ms you need to press a button to be a long press, in action mode +#define PRESS_CONFIG 600 //400 // ms you need to press a button to be a long press, in config mode /************************************/ @@ -38,3 +38,5 @@ void lockupLongPress(); void lockupLongPressStop(); #endif /* BUTTONS_H_ */ + + diff --git a/Config.h b/Config.h index b7a8a25..76b33ae 100644 --- a/Config.h +++ b/Config.h @@ -1,8 +1,8 @@ /* * Config.h * - * Created on: 6 mars 2016 - * author: Sebastien CAPOU (neskweek@gmail.com) + * Created on: 21 Octber 2016 + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS */ @@ -10,17 +10,35 @@ #define CONFIG_H_ + /*!!!!!IMPORTANT IMPORTANT IMPORTANT IMPORTANT IMPORTANT!!! * * MPU6050 device ORIENTATION * Choose which MPU's axis is parallel * to your blade axis *************************************/ -#define BLADE_X -//#define BLADE_Y +//#define BLADE_X +#define BLADE_Y //#define BLADE_Z /************************************/ +/* + * MPU6050 calibrated offset values + * If defined, calibration values will be retrieved from EEPROM + * use this option if the MPU6050_calibration sketch wrote the calibrated offsets + * into the EEPROM (default address is 96) + * If not defined, you have to note down the calibrated offset values + * and assign them to the respective variables in the code. + *************************************/ +#define MPUCALOFFSETEEPROM +#ifdef MPUCALOFFSETEEPROM +#define MEMORYBASEMPUCALIBOFFSET 96 +#endif +/************************************/ + + +/************************************/ + /* * BLADE TYPE * @@ -29,24 +47,68 @@ * disable and remove all LEDSTRINGS * blocks from compile *************************************/ -#define LEDSTRINGS +//#define LEDSTRINGS //#define LUXEON -//#define NEOPIXEL +#define NEOPIXEL +/************************************/ +/* + * SABER TYPE + * currently in v1.3 only the CROSSGUARDSABER + * will have any effect on the code + * due to the fire blade effect + *************************************/ +#define SINGLEBLADE // i.e. Graflex +//#define SABERSTAFF // i.e. Darth Maul saber with dual blades +//#define CROSSGUARDSABER // i.e. Kylo Ren saber +/* + * DEFAULT CONFIG PARAMETERS + * Will be overriden by EEPROM settings + * once the first save will be done + *************************************/ +#define VOL 20 +#define SOUNDFONT 3 +#define SWING 1000 +/************************************/ + +/* + * DO NOT MODIFY + * Unless you know what you're doing + *************************************/ +#if defined LEDSTRINGS +#define CONFIG_VERSION "L01" +#endif +#if defined LUXEON +#define CONFIG_VERSION "L02" +#endif +#if defined NEOPIXEL +#define CONFIG_VERSION "L03" +#endif +#define MEMORYBASE 32 + +/************************************/ #if defined NEOPIXEL // How many leds in one strip? -#define NUMPIXELS 114 +#define NUMPIXELS 60 // can go up to 120, could lead to memory problems if further increased + +#ifdef CROSSGUARDSABER +// define how many pixels are used for the crossguard and how many for the main blade +#define CG_STRIPE 10 +#define MN_STRIPE 50 +#endif + +//#define FIREBLADE // Number of color defined #define COLORS 14 -static const uint8_t rgbFactor = 100; +static const uint8_t rgbFactor = 255; // For led chips like NEOPIXELs, which have a data line, ground, and power, you just // need to define DATA_PIN. For led chipsets that are SPI based (four wires - data, clock, // ground, and power), like the LPD8806 define both DATA_PIN and CLOCK_PIN -#define DATA_PIN 14 //A0 +#define DATA_PIN 13 //D13 #define STRING1 5 #define STRING2 6 #define STRING3 9 @@ -59,29 +121,15 @@ static const uint8_t rgbFactor = 100; #if defined LUXEON -/* - * MY_OWN_COLORS - * If you want to manually specify your own colors - */ -#define MY_OWN_COLORS -//#define FIXED_RANGE_COLORS static const uint8_t rgbFactor = 100; - -# if defined MY_OWN_COLORS -/* COLORS - * Number of colors YOU defined in getColor function - */ -#define COLORS 3 -#else /* COLORS * Number of colors to chose from * Range : 6<->600 * Default: 48 */ -#define COLORS 48 -#endif +#define COLORS 14 #endif /************************************/ // BLADE TYPE @@ -99,7 +147,7 @@ static const uint8_t rgbFactor = 100; * BE VERY CAREFULL WITH THIS ONE OR * YOU'LL BURN YOUR BLADE'S LED ************************************/ -#define MAX_BRIGHTNESS 100 +#define MAX_BRIGHTNESS 150 /* LIGHT_EFFECTS * @@ -108,13 +156,42 @@ static const uint8_t rgbFactor = 100; * If you a device with a CPU wich is not * an Atmega328 : COMMENT THIS ************************************/ -#define LIGHT_EFFECTS +//#define LIGHT_EFFECTS + +// How long do the light effect last for the different FX's +#define CLASH_FX_DURATION 200 +#define BLASTER_FX_DURATION 300 +#define SWING_FX_DURATION 400 #define BLASTER_FLASH_TIME 3 #define CLASH_FLASH_TIME 1 - +/* FX DURATIONS AND SUPRESS TIMES + * effects cannot be retriggered for the duration + * of their respective supress pareameters + * HUM_RELAUNCH will tell the state machine to relaunch + * hum sound after this time period elapses + */ +#define SWING_SUPPRESS 500 +#define CLASH_SUPRESS 400 // do not modify below 400, otherwise interlocking clash sounds can occur +#define BLASTERBLOCK_SUPRESS 400 +#define HUM_RELAUNCH 5000 + +/* BLASTER DEFLECT TYPE + * Define how a blaser bolt deflect is + * to be triggered + * Blaster deflect action is started with + * a single click on the lockup button. + * if BLASTERCLICKTRIGGER is defined, a blaster deflect + * will be triggered once on click. + * if BLATSTERMOVEMENTTRIGGER is defined, + * blaser deflect is triggered by ensuing swings/movements. + *************************************/ +#define BLASTERCLICKTRIGGER +#ifndef BLASTERCLICKTRIGGER +#define BLATSTERMOVEMENTTRIGGER +#endif /* WRIST_MOVEMENTS * If you want to enable/disable @@ -134,6 +211,7 @@ static const uint8_t rgbFactor = 100; #define SLEEP_TIMER 300000 //5min = 300000 millisecs #endif +#define VOLTAGEDIVIDER 2,36 @@ -158,6 +236,16 @@ static const uint8_t rgbFactor = 100; //#define FoCSTRING 14 #endif +#ifdef NEOPIXELS + +#define LS1 3 +#define LS2 5 +#define LS3 6 +#define LS4 9 +#define LS5 10 +#define LS6 11 + +#endif #if defined LUXEON #define LED_RED 3 @@ -184,14 +272,14 @@ static const uint8_t rgbFactor = 100; * LEDSTRINGS users have no choice : * your forced to use Software Accent LED *************************************/ -#define ACCENT_LED 15 //A1 +//#define ACCENT_LED 15 //A1 #if defined ACCENT_LED /* * Soft or Had PWM for Accent */ -#define SOFT_ACCENT +//#define SOFT_ACCENT #if not defined SOFT_ACCENT -#define HARD_ACCENT +//#define HARD_ACCENT #endif #endif //ACCENT_LED @@ -214,16 +302,39 @@ static const uint8_t rgbFactor = 100; #define DFPLAYER_RX 8 #define DFPLAYER_TX 7 -//#define SPK1 14 //A0 -//#define SPK2 15 //A1 -#define SPK1 A6 //A6 -#define SPK2 A7 //A7 +#define SPK1 20 //A6 +#define SPK2 21 //A7 #define MAIN_BUTTON 12 #define LOCKUP_BUTTON 4 +#define BUZZMOTOR 17 //A3 +#define BUTTONLEDPIN 16 //A2 + +/* + * CONFIG MENU PARAMETERS + */ +#define JUKEBOX +#if defined LUXEON +#define CONFIG_BLADE_MAIN_COLOR +#define CONFIG_BLADE_CLASH_COLOR +#endif + +#if defined NEOPIXELS +#define CONFIG_BLADE_MAIN_COLOR +#define CONFIG_BLADE_CLASH_COLOR +#define CONFIG_POWERON_EFFECT +#define CONFIG_POWEROFF_EFFECT +#define CONFIG_FLICKER_EFFECT +#endif + +#if defined LEDSTRINGS +#define CONFIG_POWERON_EFFECT +#define CONFIG_POWEROFF_EFFECT +#define CONFIG_FLICKER_EFFECT +#endif /* * DEBUG PARAMETERS */ @@ -231,15 +342,16 @@ static const uint8_t rgbFactor = 100; * For daily use I recommend you comment LS_INFO * When you plug your device to USB uncomment LS_INFO ! */ +#define LS_SERIAL //enable serial communication using Wire library +#if defined LS_SERIAL //#define LS_INFO -#if not defined LS_INFO //#define LS_DEBUG #endif #if defined LS_DEBUG -//#define LS_BUTTON_DEBUG -#define LS_MOTION_DEBUG -#define LS_MOTION_HEAVY_DEBUG +#define LS_BUTTON_DEBUG +//#define LS_MOTION_DEBUG +//#define LS_MOTION_HEAVY_DEBUG //#define LS_RELAUNCH_DEBUG //#define LS_DEBUG_SLEEP #endif @@ -254,4 +366,4 @@ static const uint8_t rgbFactor = 100; -#endif /* CONFIG_H_ */ +#endif /* CONFIG_H_ */ diff --git a/ConfigMenu.cpp b/ConfigMenu.cpp index 744b019..0e3b76d 100644 --- a/ConfigMenu.cpp +++ b/ConfigMenu.cpp @@ -1,7 +1,7 @@ /* * Config.cpp * - * author: Sebastien CAPOU (neskweek@gmail.com) + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS * Author: neskw */ @@ -12,13 +12,17 @@ extern int8_t modification; extern bool play; extern int16_t value; - +extern void SinglePlay_Sound(uint8_t track); +extern void LoopPlay_Sound(uint8_t track); // ==================================================================================== // === CONFIG MODE FUNCTIONS === // ==================================================================================== +// this function ensures that config menu items which have values between a min and a max value +// wrap back to min/max upon reaching max/min. It also plays a sound notifying the user if either min or max value has beeb reached. +// This function is also in charge of changing the actual value of a setting via the value global variable. void confParseValue(uint16_t variable, uint16_t min, uint16_t max, - short int multiplier, DFPlayer& dfplayer) { + short int multiplier) { value = variable + (multiplier * modification); @@ -28,16 +32,17 @@ void confParseValue(uint16_t variable, uint16_t min, uint16_t max, value = min; } else if (value == (int) min and play) { play = false; - dfplayer.playPhysicalTrack(15); + SinglePlay_Sound(10); delay(150); } else if (value == (int) max and play) { play = false; - dfplayer.playPhysicalTrack(14); + SinglePlay_Sound(9); delay(150); } } //confParseValue -void confMenuStart(uint16_t variable, uint16_t sound, DFPlayer& dfplayer) { +// this functions parses in the value of the config variable and based on it plays sounds or activates LEDs +void confMenuStart(uint16_t variable, uint16_t sound, uint8_t menu) { extern uint8_t ledPins[]; #if defined LUXEON extern uint8_t currentColor[]; @@ -47,39 +52,30 @@ void confMenuStart(uint16_t variable, uint16_t sound, DFPlayer& dfplayer) { #endif extern bool enterMenu; if (enterMenu) { - dfplayer.playPhysicalTrack(sound); + SinglePlay_Sound(sound); delay(500); - switch (sound) { - case 4: + switch (menu) { + case 0: #if defined LS_INFO - Serial.print(F("VOL\nCur:")); + Serial.print(F("SNDFT\nCur:")); #endif #if defined LEDSTRINGS - lightOff(); - lightOn(ledPins, 0); + lightOff(); + lightOn(ledPins, 1); #endif - break; - case 5: + break; + case 1: #if defined LS_INFO - Serial.print(F("SNDFT\nCur:")); -#endif -#if defined LEDSTRINGS - lightOff(); - lightOn(ledPins, 1); -#endif - break; - case 6: -#if defined LS_INFO - Serial.print(F("SWING\nCur:")); + Serial.print(F("VOL\nCur:")); #endif #if defined LEDSTRINGS lightOff(); - lightOn(ledPins, 5); + lightOn(ledPins, 0); #endif break; #if defined LUXEON - case 9: + case 2: lightOff(ledPins); #if defined LS_INFO Serial.print(F("COLOR1\nCur:")); @@ -87,7 +83,7 @@ void confMenuStart(uint16_t variable, uint16_t sound, DFPlayer& dfplayer) { getColor(currentColor, variable); lightOn(ledPins, currentColor); break; - case 10: + case 3: lightOff(ledPins); #if defined LS_INFO Serial.print(F("COLOR2\nCur:")); @@ -95,85 +91,52 @@ void confMenuStart(uint16_t variable, uint16_t sound, DFPlayer& dfplayer) { getColor(currentColor, variable); lightOn(ledPins, currentColor); break; - case 11: - lightOff(ledPins); -#if defined LS_INFO - Serial.println(F("SAVE?\n")); -#endif - break; -#endif -#if defined LEDSTRINGS - case 17: + case 4: + lightOff(ledPins); #if defined LS_INFO - Serial.print(F("PWRON\nCur:")); + Serial.print(F("COLOR3\nCur:")); #endif - lightOff(); - lightOn(ledPins, 2); - break; - case 18: -#if defined LS_INFO - Serial.print(F("PWROFF\nCur:")); -#endif - lightOff(); - lightOn(ledPins, 3); - break; - case 19: -#if defined LS_INFO - Serial.print(F("FLICK\nCur:")); + getColor(currentColor, variable); + lightOn(ledPins, currentColor); + break; #endif - lightOff(); - lightOn(ledPins, 4); - break; -#endif //LEDSTRINGS + #if defined NEOPIXEL - case 9: + case 2: + lightOff(); + +#if defined LS_INFO + Serial.print(F("COLOR1\nCur:")); +#endif + getColor(variable); + for (uint8_t i = 0; i < 6; i++) { + digitalWrite(ledPins[i], HIGH); + } + lightOn(currentColor); + break; + case 3: lightOff(); #if defined LS_INFO - Serial.print(F("COLOR1\nCur:")); + Serial.print(F("COLOR2\nCur:")); #endif getColor(variable); - for (uint8_t i = 0; i < 3; i++) { + for (uint8_t i = 0; i < 6; i++) { digitalWrite(ledPins[i], HIGH); } lightOn(currentColor); break; - case 10: + case 4: lightOff(); #if defined LS_INFO - Serial.print(F("COLOR2\nCur:")); + Serial.print(F("COLOR3\nCur:")); #endif getColor(variable); - lightOn(currentColor); - break; -// case 11: -// -//#if defined LS_INFO -// Serial.println(F("SAVE?\n")); -//#endif -// break; - case 17: - lightOff(); - for (uint8_t i = 0; i < 3; i++) { - digitalWrite(ledPins[i], LOW); - } -#if defined LS_INFO - Serial.print(F("PWRON\nCur:")); -#endif - break; - case 18: -#if defined LS_INFO - Serial.print(F("PWROFF\nCur:")); -#endif - break; - case 19: - -#if defined LS_INFO - Serial.print(F("FLICK\nCur:")); -#endif - lightOff(); - lightOn(currentColor); - break; + for (uint8_t i = 0; i < 6; i++) { + digitalWrite(ledPins[i], HIGH); + } + lightOn(currentColor); + break; #endif } @@ -185,3 +148,4 @@ void confMenuStart(uint16_t variable, uint16_t sound, DFPlayer& dfplayer) { delay(100); } } //confMenuStart + diff --git a/ConfigMenu.h b/ConfigMenu.h index 0e4dc63..6db739e 100644 --- a/ConfigMenu.h +++ b/ConfigMenu.h @@ -1,12 +1,11 @@ /* * Config.h * - * author: Sebastien CAPOU (neskweek@gmail.com) + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS */ #include -#include "DFPlayer.h" #if not defined CONFIGMENU_H_ #define CONFIGMENU_H_ @@ -15,8 +14,10 @@ // ==================================================================================== void confParseValue(uint16_t variable, uint16_t min, uint16_t max, - short int multiplier, DFPlayer& dfplayer); + short int multiplier); -void confMenuStart(uint16_t variable, uint16_t sound, DFPlayer& dfplayer); +void confMenuStart(uint16_t variable, uint16_t sound, uint8_t menu); #endif /* CONFIG_H_ */ + + diff --git a/Libraries/DFPlayer_LSOS/DFPlayer.h b/Libraries/DFPlayer_LSOS/DFPlayer.h new file mode 100644 index 0000000..19cfb54 --- /dev/null +++ b/Libraries/DFPlayer_LSOS/DFPlayer.h @@ -0,0 +1,649 @@ +/* + * DFPlayer V 0.3 + * + * Created on: 27 feb 2016 + * author: Sebastien CAPOU (neskweek@gmail.com) + * Source : https://github.com/neskweek/LightSaberOS + * Description: library for DFPlayer mini sound board + * + * This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. + * To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/. + */ + +#include "Arduino.h" +#include "DFPlayerSerial.h" + +#ifndef _DFPLAYER_H_ +#define _DFPLAYER_H_ + +class DFPlayer { +//#define DFPLAYER_INFO +//#define DFPLAYER_DEBUG +//#define DFPLAYER_HEAVY_DEBUG + +#define DFPLAYER_BUFFER_LENGTH 10 + /* + * MAX_FIFO_SIZE + * Actually the real size of the DFPlayer serial FIFO queue is 68 bits. + * As each message sent weights 10 bytes, this FIFO can only contains 6 + * full valid messages. + */ +#define DFPLAYER_FIFO_SIZE 60 + + /** + * The value below comes from different source. + * See the number right after the comment + * 1 => From datasheet PDF + * 2 => From DFRobot wiki http://www.dfrobot.com/wiki/index.php/DFPlayer_Mini_SKU:DFR0299 + * 3 => From original DFPlayer librarie V1.0 made by DFRobot + * 4 => From Personal reverse engineering + * + * + * All the value commented and noted UNKNOW : + * - were documented by DFRobot but doesn't produce what they are supposed to do + * OR + * - must exists but I don't know how to use them or what they do. + * + * Further reverse engineering to come on those. + */ + + /* OPERATING_DELAY + * Uncompressible delay needed for the DFPlayer to receive and apply commands correctly + * in milliseconds + * if this delay is reduced: + * - at best your DFPlayer won't handle the mp3Serial line correctly + * resulting in unpredictable behaviour. + * - at worse it may crash the DFPlayer that will require a reset + * */ +//#define OPERATING_DELAY 155 + + /* + * DFPLAYER COMMANDS + */ + +#define NEXT 0x01 //1 command "Next" +#define PREVIOUS 0x02 //1 command "Previous" +#define PLAY_TRACK 0x03 //1 command "Play specified track"(NUM) 0-2999 +#define VOL_UP 0x04 //1 command "Increase volume" +#define VOL_DWN 0x05 //1 command "Decrease volume" +#define VOLUME 0x06 //1 command "Set specified volume" range :0-30 +#define EQUALIZER 0x07 //1 command "Set Specified EQ" (0/1/2/3/4/5) Normal/Pop/Rock/Jazz/Classic/Base +#define PLAY_REPEAT 0x08 //1 command "Play loop the specified track +#define SOURCE 0x09 //1 command "Set Specified playback source"(0/1/2/3/4) U/TF/AUX/SLEEP/FLASH +#define STANDBY 0x0A //1 command "Enter into standby/low power loss" +#define NORMAL 0x0B //1 command "Normal working" +#define RESET 0x0C //1 command "Reset module" +#define PLAYBACK 0x0D //1 command "Play" +#define PAUSE 0x0E //1 command "Pause" +#define FOLDER 0x0F //1 command "Play track in specified folder" +#define VOL_ADJUST 0x10 //1 command "Volume adjust set" {DH:Open volume adjust} {DL: set volume gain 0~31} +#define LOOP_ALL 0x11 //1 command "Play loop all" {1:start repeat play} {0:stop play}2). +#define MP3_FOLDER 0x12 //2 play specified track from MP3 folder 0-9999 +#define ADVERT 0x13 //2 Commercials 0-9999 +#define FOLDER1000 0x14 //2 Allows you to play up to 1000 track from a single directory. Support 15 folder +#define BACKGROUND 0x15 //2 stop playback, play background +#define STOP 0x16 //2 Stop playback +#define FOLDER_LOOP 0x17 //4 +#define SHUFFLE 0x18 //4 +#define SINGLE_REPEAT 0x19 //3 Set/unset current playing track in single repeat mode +#define DAC 0x1A //4 DAC Open/close +#define PLAYLIST 0x30 // Not supported by YX5200-24SS. Only works on YX6300-24SS +#define ADVERT2 0x25 //4 Multi-folder commercials command +#define PLUG_IN 0x3A //4 +#define PLUG_OUT 0x3B //4 +#define U_END_PLAY 0x3C //2U device finished playing last track +#define TF_END_PLAY 0x3D //2TF device finished playing last track +#define FLASH_END_PLAY 0x3E //2STAY +#define INIT 0x3F //2Send initialization parameters 0 - 0x0F(each bit represent one device of the low-four bits) +#define ERROR 0x40 //2Returns an error, request retransmission +#define REPLY 0x41 //2Reply +#define STATUS 0x42 //2Query the current status +#define QUERY_VOLUME 0x43 //2Query the current volume +#define QUERY_EQ 0x44 //2Query the current EQ +#define QUERY_PLAYMODE 0x45 //2Query the current playback mode +#define QUERY_VERSION 0x46 //2Query the current software version +#define QUERY_U_FILES 0x47 //2Query the total number of TF card files +#define QUERY_TF_FILES 0x48 //2Query the total number of U-disk files +#define QUERY_F_FILES 0x49 //2Query the total number of flash files +#define QUERY_KEEPON 0x4A //2Keep on +#define QUERY_U_CUR 0x4B //2Queries the current track of U-Disk +#define QUERY_TF_CUR 0x4C //2Queries the current track of TF card +#define QUERY_F_CUR 0x4D //2Queries the current track of Flash + /***************************************/ + + /*** + * Corrected value from personal tests + */ + +public: + + inline DFPlayer() { + this->device = 1; // TFCard by default + this->noReceiveBit = false; + this->fifoCount = 0; + this->querying = false; + } + + inline ~DFPlayer() { + if (NULL != mp3Serial) { + delete this->mp3Serial; + } + } + + inline DFPlayerSerial * getSerial() { + return this->mp3Serial; + } + + inline void setSerial(uint8_t receivePin, uint8_t transmitPin, + bool inverse_logic = false) { + +#ifdef DFPLAYER_INFO + Serial.println(F("Connecting to DFPlayer...")); +#endif + + if (NULL != this->mp3Serial) { + delete this->mp3Serial; + } + + this->mp3Serial = new DFPlayerSerial(receivePin, transmitPin); + + this->mp3Serial->begin(9600); + // removed following lines, they work but cause a pop sound when the device starts + //reset(); + //while (not this->mp3Serial->available()) { + // //waiting for device to be ready + //} + //if (this->mp3Serial->available()) { +#ifdef DFPLAYER_INFO + Serial.println(F("Connected to DFPlayer !")); +#endif + + //} + + } + + inline uint8_t getDevice() const { + return this->device; + } + + inline void setDevice(uint8_t device) { + this->device = device; + } + + inline bool isNoReceiveBit() const { + return this->noReceiveBit; + } + + inline void setNoReceiveBit(bool noReceiveBit) { + this->noReceiveBit = noReceiveBit; + } + + inline uint8_t getFifoCount() const { + return this->fifoCount; + } + + inline uint8_t updateFifoCount() { + this->fifoCount = mp3Serial->available(); + return this->fifoCount; + } + + inline bool isQuerying() { + return querying; + } + + inline void setQuerying(bool querying) { + this->querying = querying; + } + + inline const uint8_t* getRecvBuffer() const { + return this->recvBuffer; + } + + inline void resetRecvBuffer() { + for (uint8_t i = 0; i < DFPLAYER_BUFFER_LENGTH; i++) { + this->recvBuffer[i] = 0x00; + } + } + + inline uint8_t* getSendBuffer() { + return this->sendBuffer; + } + + inline void setSendBuffer(uint8_t cmd, uint16_t bit6 = 0, + uint16_t bit5 = 0) { + if (not ((bit5 > 255) or (bit6 > 65535) or (bit6 > 255 and bit5 != 0))) { + this->sendBuffer[0] = 0x7E; // start code + this->sendBuffer[1] = 0xFF; // Version + this->sendBuffer[2] = 0x06; // Length (3+4+5+6+7+8) + this->sendBuffer[3] = cmd; // Command + this->sendBuffer[4] = this->noReceiveBit; // Command feedback + if (!(bit5 == 0 && bit6 > 255)) { //256 = 0x01 0x00 + this->sendBuffer[5] = bit5; // High data byte - Command Argument 1 (complement) or Argument 2 + this->sendBuffer[6] = bit6; // Low data byte Command Argument 1 + } else if (bit5 == 0 && bit6 > 255) { + fillData(this->sendBuffer + 5, bit6); + } + this->sendBuffer[7] = 0x00; // Cheksum + this->sendBuffer[8] = 0x00; // Cheksum + this->sendBuffer[9] = 0xEF; // End bit + checksum(); + } else { + /* + * We fill the sendBuffer with bullshit + * this will generate an error message on Serial + */ + for (uint8_t i = 0; i < DFPLAYER_BUFFER_LENGTH; i++) { + this->sendBuffer[i] = 0x00; + } + + } + } //setSendBuffer + + /* + *calculates checksum : 0 - (sum of bits 1 to 6) + */ + inline void checksum() { + uint16_t sum = 0; + for (uint8_t i = 1; i < 7; i++) { + sum += this->sendBuffer[i]; + } + fillData(this->sendBuffer + 7, -sum); + } //checksum + + /* + * + */ + inline void send() { + +#ifdef DFPLAYER_HEAVY_DEBUG + printSendBuffer(); +#endif + // if (this->isEmptyQueue()) { + this->mp3Serial->write(this->sendBuffer, DFPLAYER_BUFFER_LENGTH); + // delay(OPERATING_DELAY); + +#ifdef DFPLAYER_DEBUG + printHumanReadableSendBuffer(); +#endif + } //send + + /* + * + */ + inline void receive() { + resetRecvBuffer(); + while (not isNoReceiveBit() and this->mp3Serial->available() + and (this->mp3Serial->available() % DFPLAYER_BUFFER_LENGTH == 0) + /*and this->recvBuffer[0] != 0x7E*/) { + mp3Serial->readBytesUntil(0xEF, this->recvBuffer, + DFPLAYER_BUFFER_LENGTH); + +#ifdef DFPLAYER_DEBUG + this->printHumanReadableRecvBuffer(); +#endif +#ifdef DFPLAYER_HEAVY_DEBUG + this->printRecvBuffer(); +#endif + } + //for (uint8_t i = 0; i < DFPLAYER_BUFFER_LENGTH; i++) { + // Serial.print(this->recvBuffer[i], HEX); + // Serial.print(F(" ")); + //} + + this->fifoCount = this->mp3Serial->available(); + } //receive + + inline void reset() { + setSendBuffer(RESET); + send(); + //delay(3000); //Mandatory !! + } + inline void setVolume(uint8_t value) { + setSendBuffer(VOLUME, value); + // this command never receive an answer ! + send(); + } + + inline void setEqualizer(uint8_t value) { + //"Set Specified EQ" (0/1/2/3/4/5) Normal/Pop/Rock/Jazz/Classic/Base + setSendBuffer(EQUALIZER, value); + // this command never receive an answer ! + send(); + } + + inline void playTrackFromDir(uint8_t track, uint8_t folder) { + setSendBuffer(FOLDER, track, folder); + send(); + } + inline void playTrackFromMP3Folder(uint16_t track) { + setSendBuffer(MP3_FOLDER, track); + send(); + } + inline void playPhysicalTrack(uint16_t value = 0) { + setSendBuffer(PLAY_TRACK, value); + send(); + } + + inline void setSingleLoop(bool value) { + //delay(OPERATING_DELAY); + setSendBuffer(SINGLE_REPEAT, !value); // !value o_0 ? + // this command never receive an answer ! + send(); + //delay(OPERATING_DELAY); + + } + + inline void playSingleLoop(uint8_t track, uint8_t folder = 0) { + setSendBuffer(PLAY_REPEAT, track, folder); + send(); + + } + + inline uint16_t getCurrentTrack() { + while (this->querying) { + //Wait for serial line to be ready + } + this->querying = true; + uint8_t command; + uint16_t result = 0; + bool found = false; + switch (this->device) { + case 0: // U + command = QUERY_U_CUR; + break; + case 1: // TFCard + command = QUERY_TF_CUR; + break; + case 2: // SLEEP ???? + break; + case 3: // FLASH + command = QUERY_F_CUR; + break; + } + + setSendBuffer(command); + send(); + + while (this->mp3Serial->available() + and (this->mp3Serial->available() % DFPLAYER_BUFFER_LENGTH == 0)) { + + receive(); + + result = 256 * this->recvBuffer[5] + this->recvBuffer[6]; +#ifdef DFPLAYER_HEAVY_DEBUG + Serial.print(F("DFPLAYER getCurrentTrack searching; command=")); + Serial.print(this->recvBuffer[3], HEX); + Serial.print(F(" track=")); + Serial.print(result); + Serial.print(F(" millis=")); + Serial.print(millis()); + Serial.print(F("ms fifoCount=")); + Serial.println(this->fifoCount); + printRecvBuffer(); +#endif + if (this->recvBuffer[3] == command) { + found = true; + +#ifdef DFPLAYER_HEAVY_DEBUG + Serial.print(F("DFPLAYER getCurrentTrack command found=")); + Serial.print(this->recvBuffer[3], HEX); + Serial.print(F("! Will return =")); + Serial.print(256 * this->recvBuffer[5] + this->recvBuffer[6]); + Serial.print(F(" ")); + Serial.print(millis()); + Serial.print(F("ms fifoCount=")); + Serial.println(this->fifoCount); +#endif + + } + + } + if (!found) { +#ifdef DFPLAYER_HEAVY_DEBUG + Serial.println( + F( + "DFPLAYER getCurrentTrack didn't found matching receive code")); +#endif + // returning bullshit + } + this->querying = false; + return result; + } + + inline void folderLoop(uint8_t folder) { + setSendBuffer(FOLDER_LOOP, folder); + send(); + } + inline void loopAll(uint8_t track = 0, uint8_t folder = 0) { + setSendBuffer(LOOP_ALL, track, folder); + send(); + } + inline void test(uint8_t track = 0, uint8_t folder = 0) { + setSendBuffer(QUERY_KEEPON, track, folder); + send(); + } + +//#ifdef DFPLAYER_HEAVY_DEBUG + inline void printRecvBuffer() { + Serial.print(F("DFPLAYER Received at ")); + Serial.print(millis()); + Serial.print(F("ms:\t")); + for (uint8_t i = 0; i < DFPLAYER_BUFFER_LENGTH; i++) { + Serial.print(this->recvBuffer[i], HEX); + Serial.print(F(" ")); + } + Serial.print(F("\tfifoCount=")); + Serial.print(this->fifoCount); + Serial.println(); + + } + inline void printSendBuffer() { + Serial.print(F("DFPLAYER Sending at ")); + Serial.print(millis()); + Serial.print(F("ms:\t")); + for (uint8_t i = 0; i < DFPLAYER_BUFFER_LENGTH; i++) { + Serial.print(this->sendBuffer[i], HEX); + Serial.print(F(" ")); + } + Serial.println(); + } + +//#endif +//#ifdef DFPLAYER_DEBUG + + inline void printHumanReadableSendBuffer() { +// processing outgoing message + uint8_t command = this->sendBuffer[3]; + + uint8_t highBit = this->sendBuffer[5]; + uint8_t lowBit = this->sendBuffer[6]; + uint16_t data = 256 * this->sendBuffer[5] + this->sendBuffer[6]; + + switch (command) { + case SINGLE_REPEAT: + if (!lowBit) + Serial.println(F("\t\t\tDFPlayer: Enabling single play mode ")); + else + Serial.println(F("\t\t\tDFPlayer: Disabling single play mode ")); + break; + case FOLDER: + Serial.print(F("\t\t\tDFPlayer: Require play of file number ")); + Serial.print(lowBit); + Serial.print(F(" in directory number ")); + Serial.println(highBit); + break; + case VOLUME: + Serial.print(F("\t\t\tDFPlayer: Setting volume to ")); + Serial.println(lowBit); + break; + case RESET: + Serial.println(F("\t\t\tDFPlayer: Asking for Reset...")); + break; + case QUERY_TF_CUR: + case QUERY_U_CUR: + case QUERY_F_CUR: + Serial.println(F("\t\t\tDFPlayer: Query current playing track... ")); + break; + default: + // this recvBuffer needs to be added to the switch/case scenario + Serial.print(F("\t\t\tDFPlayer: Unhandled sended message: ")); + for (uint8_t i = 0; i < DFPLAYER_BUFFER_LENGTH; i++) { + Serial.print(this->sendBuffer[i], HEX); + Serial.print(F(" ")); + } + + break; + } + } // end printHumanReadableRecvBuffer + inline void printHumanReadableRecvBuffer() { +// processing incoming message + const char* errorText[] = {"Module busy", "Currently sleep mode", + "Serial receiving error", "Checksum incorrect", + "Specified track is out of current track scope", + "Specified track is not found", "Advertise error", + "SD card reading failed", "Entered into sleep mode"}; + + uint8_t answer = this->recvBuffer[3]; + + uint8_t highBit = this->recvBuffer[5]; + uint8_t lowBit = this->recvBuffer[6]; + uint16_t data = 256 * this->recvBuffer[5] + this->recvBuffer[6]; + + switch (answer) { + case ERROR: + // error occurs + Serial.print(F("\t\t\tDFPlayer: Error: ")); + Serial.println(errorText[lowBit - 1]); + break; + case PLUG_IN: + + if (lowBit == 2) { // sd card plugged in + Serial.println(F("\t\t\tDFPlayer: SD card plugged in!")); + } + break; + case PLUG_OUT: + if (lowBit == 2) { // sd card plugged out + Serial.println(F("\t\t\tDFPlayer: SD card plugged out!")); + } + break; + case TF_END_PLAY: + case U_END_PLAY: + // track finished + Serial.print(F("\t\t\tDFPlayer: Track finished playing: ")); + Serial.println(data); + break; + case INIT: + if (lowBit == 2) { // sd card online (startup) + Serial.println(F("\t\t\tDFPlayer: SD card online")); + } + break; + case QUERY_TF_CUR: + case QUERY_U_CUR: + case QUERY_F_CUR: + Serial.print(F("\t\t\tDFPlayer: Current track playing: ")); + Serial.println(data); + break; + default: + // this recvBuffer needs to be added to the switch/case scenario + Serial.print(F("\t\t\tDFPlayer: Unhandled received message: ")); + for (uint8_t i = 0; i < DFPLAYER_BUFFER_LENGTH; i++) { + Serial.print(this->recvBuffer[i], HEX); + Serial.print(F(" ")); + } + + break; + } + } // end printHumanReadableRecvBuffer +//#endif + + /* + * IMPLEMENTED BUT NOT TESTED YET !!! + * Be Aware those functions may not work as intended + */ + inline void next() { + setSendBuffer(NEXT); + send(); + } + inline void previous() { + setSendBuffer(PREVIOUS); + send(); + } + + inline void volumeUp() { + setSendBuffer(VOL_UP); + send(); + } + inline void volumeDown() { + setSendBuffer(VOL_DWN); + send(); + } + + inline void play() { + setSendBuffer(PLAYBACK); + send(); + } + inline void pause() { + setSendBuffer(PAUSE); + send(); + } + inline void stop() { + setSendBuffer(STOP); + send(); + } + inline uint8_t countTrackInDir(uint8_t folder) { + setSendBuffer(MP3_FOLDER); + send(); + } + + inline void playAdvert(uint16_t value) { + setSendBuffer(ADVERT); + send(); + } + inline void getStatus() { + setSendBuffer(STATUS); + send(); + receive(); + } + + inline void getVolume() { + setSendBuffer(QUERY_VOLUME); + send(); + receive(); + } + + inline uint16_t countTotalFiles() { + uint8_t command; + switch (this->device) { + case 0: // U + command = QUERY_U_FILES; + break; + case 1: // TFCard + command = QUERY_TF_FILES; + break; + case 2: // SLEEP ???? + break; + case 3: // FLASH + command = QUERY_F_FILES; + break; + } + + setSendBuffer(command); + send(); + } + +private: + +//Puts uint16_bigend at the specified memory address + inline void fillData(uint8_t *buffer, uint16_t data) { + *buffer = (uint8_t) (data >> 8); + *(buffer + 1) = (uint8_t) data; + } + + DFPlayerSerial * mp3Serial; + uint8_t sendBuffer[10]; + uint8_t recvBuffer[10]; + uint8_t device; + uint8_t fifoCount;bool noReceiveBit; + bool querying; +}; +#endif // _DFPLAYER_H_ diff --git a/Libraries/DFPlayer_LSOS/DFPlayerSerial.cpp b/Libraries/DFPlayer_LSOS/DFPlayerSerial.cpp new file mode 100644 index 0000000..a15b6b4 --- /dev/null +++ b/Libraries/DFPlayer_LSOS/DFPlayerSerial.cpp @@ -0,0 +1,466 @@ +/* + DFPlayerSerial.cpp (formerly NewSoftSerial.cpp) - + Multi-instance software serial library for Arduino/Wiring + -- Interrupt-driven receive and other improvements by ladyada + (http://ladyada.net) + -- Tuning, circular buffer, derivation from class Print/Stream, + multi-instance support, porting to 8MHz processors, + various optimizations, PROGMEM delay tables, inverse logic and + direct port writing by Mikal Hart (http://www.arduiniana.org) + -- Pin change interrupt macros by Paul Stoffregen (http://www.pjrc.com) + -- 20MHz processor support by Garrett Mace (http://www.macetech.com) + -- ATmega1280/2560 support by Brett Hagman (http://www.roguerobotics.com/) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + The latest version of this library can always be found at + http://arduiniana.org. + */ + +// When set, _DEBUG co-opts pins 11 and 13 for debugging with an +// oscilloscope or logic analyzer. Beware: it also slightly modifies +// the bit times, so don't rely on it too much at high baud rates +#define _DEBUG 0 +#define _DEBUG_PIN1 11 +#define _DEBUG_PIN2 13 +// +// Includes +// +#include +#include +#include +#include +#include + +// +// Statics +// +DFPlayerSerial *DFPlayerSerial::active_object = 0; +char DFPlayerSerial::_receive_buffer[_SS_MAX_RX_BUFF]; +volatile uint8_t DFPlayerSerial::_receive_buffer_tail = 0; +volatile uint8_t DFPlayerSerial::_receive_buffer_head = 0; + +// +// Debugging +// +// This function generates a brief pulse +// for debugging or measuring on an oscilloscope. +inline void DebugPulse(uint8_t pin, uint8_t count) { +#if _DEBUG + volatile uint8_t *pport = portOutputRegister(digitalPinToPort(pin)); + + uint8_t val = *pport; + while (count--) + { + *pport = val | digitalPinToBitMask(pin); + *pport = val; + } +#endif +} + +// +// Private methods +// + +/* static */ +inline void DFPlayerSerial::tunedDelay(uint16_t delay) { + _delay_loop_2(delay); +} + +// This function sets the current object as the "listening" +// one and returns true if it replaces another +bool DFPlayerSerial::listen() { + if (!_rx_delay_stopbit) + return false; + + if (active_object != this) { + if (active_object) + active_object->stopListening(); + + _buffer_overflow = false; + _receive_buffer_head = _receive_buffer_tail = 0; + active_object = this; + + setRxIntMsk(true); + return true; + } + + return false; +} + +// Stop listening. Returns true if we were actually listening. +bool DFPlayerSerial::stopListening() { + if (active_object == this) { + setRxIntMsk(false); + active_object = NULL; + return true; + } + return false; +} + +// +// The receive routine called by the interrupt handler +// +void DFPlayerSerial::recv() { +#if GCC_VERSION < 40302 +// Work-around for avr-gcc 4.3.0 OSX version bug +// Preserve the registers that the compiler misses +// (courtesy of Arduino forum user *etracer*) + asm volatile( + "push r18 \n\t" + "push r19 \n\t" + "push r20 \n\t" + "push r21 \n\t" + "push r22 \n\t" + "push r23 \n\t" + "push r26 \n\t" + "push r27 \n\t" + ::); +#endif + + uint8_t d = 0; + + // If RX line is high, then we don't see any start bit + // so interrupt is probably not for us + if (_inverse_logic ? rx_pin_read() : !rx_pin_read()) { + // Disable further interrupts during reception, this prevents + // triggering another interrupt directly after we return, which can + // cause problems at higher baudrates. + setRxIntMsk(false); + + // Wait approximately 1/2 of a bit width to "center" the sample + tunedDelay(_rx_delay_centering); + DebugPulse(_DEBUG_PIN2, 1); + + // Read each of the 8 bits + for (uint8_t i = 8; i > 0; --i) { + tunedDelay(_rx_delay_intrabit); + d >>= 1; + DebugPulse(_DEBUG_PIN2, 1); + if (rx_pin_read()) + d |= 0x80; + } + + if (_inverse_logic) + d = ~d; + + // if buffer full, set the overflow flag and return + uint8_t next = (_receive_buffer_tail + 1) % _SS_MAX_RX_BUFF; + if (next != _receive_buffer_head) { + // save new data in buffer: tail points to where byte goes + _receive_buffer[_receive_buffer_tail] = d; // save new byte + _receive_buffer_tail = next; + } else { + DebugPulse(_DEBUG_PIN1, 1); + _buffer_overflow = true; + } + + // skip the stop bit + tunedDelay(_rx_delay_stopbit); + DebugPulse(_DEBUG_PIN1, 1); + + // Re-enable interrupts when we're sure to be inside the stop bit + setRxIntMsk(true); + + } + +#if GCC_VERSION < 40302 +// Work-around for avr-gcc 4.3.0 OSX version bug +// Restore the registers that the compiler misses + asm volatile( + "pop r27 \n\t" + "pop r26 \n\t" + "pop r23 \n\t" + "pop r22 \n\t" + "pop r21 \n\t" + "pop r20 \n\t" + "pop r19 \n\t" + "pop r18 \n\t" + ::); +#endif +} + +uint8_t DFPlayerSerial::rx_pin_read() { + return *_receivePortRegister & _receiveBitMask; +} + +/* static */ + +inline void DFPlayerSerial::handle_interrupt() +//void DFPlayerSerial::handle_interrupt() +{ + if (active_object) + { + active_object->recv(); + } +} + + + + + + +/* + * Changed For LSOS: + * We use atmega328 so we don't need to reserve all PCINT to be sure to catch + * DFPlayer serial answer + * +*/ + +#if defined(PCINT0_vect) +ISR(PCINT0_vect) +{ + DFPlayerSerial::handle_interrupt(); +} +#endif + +#if defined(PCINT1_vect) +ISR(PCINT1_vect, ISR_ALIASOF(PCINT0_vect)); +#endif + +#if defined(PCINT2_vect) +ISR(PCINT2_vect, ISR_ALIASOF(PCINT0_vect)); +#endif + + + + +// +// Constructor +// +DFPlayerSerial::DFPlayerSerial(uint8_t receivePin, uint8_t transmitPin, + bool inverse_logic /* = false */) : + _rx_delay_centering(0), _rx_delay_intrabit(0), _rx_delay_stopbit(0), _tx_delay( + 0), _buffer_overflow(false), _inverse_logic(inverse_logic) { + setTX(transmitPin); + setRX(receivePin); +} + +// +// Destructor +// +DFPlayerSerial::~DFPlayerSerial() { + end(); +} + +void DFPlayerSerial::setTX(uint8_t tx) { + // First write, then set output. If we do this the other way around, + // the pin would be output low for a short while before switching to + // output hihg. Now, it is input with pullup for a short while, which + // is fine. With inverse logic, either order is fine. + digitalWrite(tx, _inverse_logic ? LOW : HIGH); + pinMode(tx, OUTPUT); + _transmitBitMask = digitalPinToBitMask(tx); + uint8_t port = digitalPinToPort(tx); + _transmitPortRegister = portOutputRegister(port); +} + +void DFPlayerSerial::setRX(uint8_t rx) { + pinMode(rx, INPUT); + if (!_inverse_logic) + digitalWrite(rx, HIGH); // pullup for normal logic! + _receivePin = rx; + _receiveBitMask = digitalPinToBitMask(rx); + uint8_t port = digitalPinToPort(rx); + _receivePortRegister = portInputRegister(port); +} + +uint16_t DFPlayerSerial::subtract_cap(uint16_t num, uint16_t sub) { + if (num > sub) + return num - sub; + else + return 1; +} + +// +// Public methods +// + +void DFPlayerSerial::begin(long speed) { + _rx_delay_centering = _rx_delay_intrabit = _rx_delay_stopbit = _tx_delay = + 0; + + // Precalculate the various delays, in number of 4-cycle delays + uint16_t bit_delay = (F_CPU / speed) / 4; + + // 12 (gcc 4.8.2) or 13 (gcc 4.3.2) cycles from start bit to first bit, + // 15 (gcc 4.8.2) or 16 (gcc 4.3.2) cycles between bits, + // 12 (gcc 4.8.2) or 14 (gcc 4.3.2) cycles from last bit to stop bit + // These are all close enough to just use 15 cycles, since the inter-bit + // timings are the most critical (deviations stack 8 times) + //_tx_delay = subtract_cap(bit_delay, 15 / 4); + _tx_delay = 417; + + // Only setup rx when we have a valid PCINT for this pin + if (digitalPinToPCICR(_receivePin)) { +#if GCC_VERSION > 40800 + // Timings counted from gcc 4.8.2 output. This works up to 115200 on + // 16Mhz and 57600 on 8Mhz. + // + // When the start bit occurs, there are 3 or 4 cycles before the + // interrupt flag is set, 4 cycles before the PC is set to the right + // interrupt vector address and the old PC is pushed on the stack, + // and then 75 cycles of instructions (including the RJMP in the + // ISR vector table) until the first delay. After the delay, there + // are 17 more cycles until the pin value is read (excluding the + // delay in the loop). + // We want to have a total delay of 1.5 bit time. Inside the loop, + // we already wait for 1 bit time - 23 cycles, so here we wait for + // 0.5 bit time - (71 + 18 - 22) cycles. + _rx_delay_centering = subtract_cap(bit_delay / 2, + (4 + 4 + 75 + 17 - 23) / 4); + + // There are 23 cycles in each loop iteration (excluding the delay) + _rx_delay_intrabit = subtract_cap(bit_delay, 23 / 4); + + // There are 37 cycles from the last bit read to the start of + // stopbit delay and 11 cycles from the delay until the interrupt + // mask is enabled again (which _must_ happen during the stopbit). + // This delay aims at 3/4 of a bit time, meaning the end of the + // delay will be at 1/4th of the stopbit. This allows some extra + // time for ISR cleanup, which makes 115200 baud at 16Mhz work more + // reliably + _rx_delay_stopbit = subtract_cap(bit_delay * 3 / 4, (37 + 11) / 4); +#else // Timings counted from gcc 4.3.2 output + // Note that this code is a _lot_ slower, mostly due to bad register + // allocation choices of gcc. This works up to 57600 on 16Mhz and + // 38400 on 8Mhz. + _rx_delay_centering = subtract_cap(bit_delay / 2, (4 + 4 + 97 + 29 - 11) / 4); + _rx_delay_intrabit = subtract_cap(bit_delay, 11 / 4); + _rx_delay_stopbit = subtract_cap(bit_delay * 3 / 4, (44 + 17) / 4); +#endif + + // Enable the PCINT for the entire port here, but never disable it + // (others might also need it, so we disable the interrupt by using + // the per-pin PCMSK register). + *digitalPinToPCICR(_receivePin) |= _BV( + digitalPinToPCICRbit(_receivePin)); + // Precalculate the pcint mask register and value, so setRxIntMask + // can be used inside the ISR without costing too much time. + _pcint_maskreg = digitalPinToPCMSK(_receivePin); + _pcint_maskvalue = _BV(digitalPinToPCMSKbit(_receivePin)); + + tunedDelay(_tx_delay); // if we were low this establishes the end + } + +#if _DEBUG + pinMode(_DEBUG_PIN1, OUTPUT); + pinMode(_DEBUG_PIN2, OUTPUT); +#endif + + listen(); +} + +void DFPlayerSerial::setRxIntMsk(bool enable) { + if (enable) { + *_pcint_maskreg |= _pcint_maskvalue; + } else { + *_pcint_maskreg &= ~_pcint_maskvalue; + } +} + +void DFPlayerSerial::end() { + stopListening(); +} + +// Read data from buffer +int DFPlayerSerial::read() { + if (!isListening()) + return -1; + + // Empty buffer? + if (_receive_buffer_head == _receive_buffer_tail) + return -1; + + // Read from "head" + uint8_t d = _receive_buffer[_receive_buffer_head]; // grab next byte + _receive_buffer_head = (_receive_buffer_head + 1) % _SS_MAX_RX_BUFF; + return d; +} + +int DFPlayerSerial::available() { + if (!isListening()) + return 0; + + return (_receive_buffer_tail + _SS_MAX_RX_BUFF - _receive_buffer_head) + % _SS_MAX_RX_BUFF; +} + +size_t DFPlayerSerial::write(uint8_t b) { + if (_tx_delay == 0) { + setWriteError(); + return 0; + } + + // By declaring these as local variables, the compiler will put them + // in registers _before_ disabling interrupts and entering the + // critical timing sections below, which makes it a lot easier to + // verify the cycle timings + volatile uint8_t *reg = _transmitPortRegister; + uint8_t reg_mask = _transmitBitMask; + uint8_t inv_mask = ~_transmitBitMask; + uint8_t oldSREG = SREG; + bool inv = _inverse_logic; + uint16_t delay = _tx_delay; + + cli(); + // turn off interrupts for a clean txmit + + // Write the start bit + *reg &= inv_mask; + + tunedDelay(delay); + + // Write each of the 8 bits + for (uint8_t i = 8; i > 0; --i) { + if (b & 1) // choose bit + *reg |= reg_mask; // send 1 + else + *reg &= inv_mask; // send 0 + + tunedDelay(delay); + b >>= 1; + } + + // restore pin to natural state + *reg |= reg_mask; + + SREG = oldSREG; // turn interrupts back on + tunedDelay(delay); + + return 1; +} + +void DFPlayerSerial::flush() { + if (!isListening()) + return; + + uint8_t oldSREG = SREG; + cli(); + _receive_buffer_head = _receive_buffer_tail = 0; + SREG = oldSREG; +} + +int DFPlayerSerial::peek() { + if (!isListening()) + return -1; + + // Empty buffer? + if (_receive_buffer_head == _receive_buffer_tail) + return -1; + + // Read from "head" + return _receive_buffer[_receive_buffer_head]; +} diff --git a/Libraries/DFPlayer_LSOS/DFPlayerSerial.h b/Libraries/DFPlayer_LSOS/DFPlayerSerial.h new file mode 100644 index 0000000..6f7f046 --- /dev/null +++ b/Libraries/DFPlayer_LSOS/DFPlayerSerial.h @@ -0,0 +1,129 @@ +/* +DFPlayerSerial.h (formerly NewSoftSerial.h) - +Multi-instance software serial library for Arduino/Wiring +-- Interrupt-driven receive and other improvements by ladyada + (http://ladyada.net) +-- Tuning, circular buffer, derivation from class Print/Stream, + multi-instance support, porting to 8MHz processors, + various optimizations, PROGMEM delay tables, inverse logic and + direct port writing by Mikal Hart (http://www.arduiniana.org) +-- Pin change interrupt macros by Paul Stoffregen (http://www.pjrc.com) +-- 20MHz processor support by Garrett Mace (http://www.macetech.com) +-- ATmega1280/2560 support by Brett Hagman (http://www.roguerobotics.com/) + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +The latest version of this library can always be found at +http://arduiniana.org. +*/ + +#ifndef SoftwareSerial_h +#define SoftwareSerial_h + +#include +#include + +/****************************************************************************** +* Definitions +******************************************************************************/ + +#define _SS_MAX_RX_BUFF 64 // RX buffer size +#ifndef GCC_VERSION +#define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) +#endif + +class DFPlayerSerial : public Stream +{ +private: + // per object data + uint8_t _receivePin; + uint8_t _receiveBitMask; + volatile uint8_t *_receivePortRegister; + uint8_t _transmitBitMask; + volatile uint8_t *_transmitPortRegister; + volatile uint8_t *_pcint_maskreg; + uint8_t _pcint_maskvalue; + + // Expressed as 4-cycle delays (must never be 0!) + uint16_t _rx_delay_centering; + uint16_t _rx_delay_intrabit; + uint16_t _rx_delay_stopbit; + uint16_t _tx_delay; + + uint16_t _buffer_overflow:1; + uint16_t _inverse_logic:1; + + // static data + static char _receive_buffer[_SS_MAX_RX_BUFF]; + static volatile uint8_t _receive_buffer_tail; + static volatile uint8_t _receive_buffer_head; + static DFPlayerSerial *active_object; + + // private methods + + uint8_t rx_pin_read(); + void tx_pin_write(uint8_t pin_state) __attribute__((__always_inline__)); + void setTX(uint8_t transmitPin); + void setRX(uint8_t receivePin); + void setRxIntMsk(bool enable) __attribute__((__always_inline__)); + + // Return num - sub, or 1 if the result would be < 1 + static uint16_t subtract_cap(uint16_t num, uint16_t sub); + + // private static method for timing + static inline void tunedDelay(uint16_t delay); + +public: + // public methods + DFPlayerSerial(uint8_t receivePin, uint8_t transmitPin, bool inverse_logic = false); + virtual ~DFPlayerSerial(); + void begin(long speed); + bool listen(); + void end(); + bool isListening() { return this == active_object; } + bool stopListening(); + bool overflow() { bool ret = _buffer_overflow; if (ret) _buffer_overflow = false; return ret; } + int peek(); + + virtual size_t write(uint8_t byte); + virtual int read(); + virtual int available(); + virtual void flush(); + operator bool() { return true; } + + using Print::write; + + // public only for easy access by interrupt handlers + void recv(); + + // public only for easy access by interrupt handlers + static inline void handle_interrupt() __attribute__((__always_inline__)); +// void handle_interrupt(); + + DFPlayerSerial* getActiveObject() { + return active_object; + } +}; + +// Arduino 0012 workaround +#undef int +#undef char +#undef long +#undef byte +#undef float +#undef abs +#undef round + +#endif diff --git a/Libraries/DFPlayer_Mini_Mp3/DFPlayer_Mini_Mp3.cpp b/Libraries/DFPlayer_Mini_Mp3/DFPlayer_Mini_Mp3.cpp new file mode 100644 index 0000000..fd2005c --- /dev/null +++ b/Libraries/DFPlayer_Mini_Mp3/DFPlayer_Mini_Mp3.cpp @@ -0,0 +1,258 @@ +/******************************************************************************* + * Copyright (C) 2014 DFRobot * + * * + * DFPlayer_Mini_Mp3, This library provides a quite complete function for * + * DFPlayer mini mp3 module. * + * www.github.com/dfrobot/DFPlayer_Mini_Mp3 (github as default source provider)* + * DFRobot-A great source for opensource hardware and robot. * + * * + * This file is part of the DFplayer_Mini_Mp3 library. * + * * + * DFPlayer_Mini_Mp3 is free software: you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 3 of * + * the License, or any later version. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with DFPlayer_Mini_Mp3. If not, see * + * . * + ******************************************************************************/ + +/* + * name: DFPlayer_Mini_Mp3 + * version: 1.0 + * Author: lisper + * Date: 2014-05-22 + * Description: mp3 library for DFPlayer mini board + * note: mp3 file must put into mp3 folder in your tf card + */ + + +#include +#include "SoftwareSerial.h" + +extern uint8_t send_buf[10]; +extern uint8_t recv_buf[10]; + +static void(*send_func)() = NULL; +//static HardwareSerial * _hardware_serial = NULL; +static SoftwareSerial * _software_serial = NULL; +static boolean is_reply = false; + +// +void mp3_set_reply (boolean state) { + is_reply = state; + send_buf[4] = is_reply; +} + +// +static void fill_uint16_bigend (uint8_t *thebuf, uint16_t data) { + *thebuf = (uint8_t)(data>>8); + *(thebuf+1) = (uint8_t)data; +} + + +//calc checksum (1~6 byte) +uint16_t mp3_get_checksum (uint8_t *thebuf) { + uint16_t sum = 0; + for (int i=1; i<7; i++) { + sum += thebuf[i]; + } + return -sum; +} + +//fill checksum to send_buf (7~8 byte) +void mp3_fill_checksum () { + uint16_t checksum = mp3_get_checksum (send_buf); + fill_uint16_bigend (send_buf+7, checksum); +} + +// +//void h_send_func () { +// for (int i=0; i<10; i++) { +// _hardware_serial->write (send_buf[i]); +// } +//} + +// +void s_send_func () { + for (int i=0; i<10; i++) { + _software_serial->write (send_buf[i]); + } +} + +// +//void mp3_set_serial (HardwareSerial *theSerial) { +//void mp3_set_serial (HardwareSerial &theSerial) { +// _hardware_serial = &theSerial; +// send_func = h_send_func; +//} + +// +void mp3_set_serial (SoftwareSerial &theSerial) { + _software_serial = &theSerial; + send_func = s_send_func; +} + +// +void mp3_send_cmd (uint8_t cmd, uint16_t arg) { + send_buf[3] = cmd; + fill_uint16_bigend ((send_buf+5), arg); + mp3_fill_checksum (); + send_func (); +} + +// +void mp3_send_cmd (uint8_t cmd) { + send_buf[3] = cmd; + fill_uint16_bigend ((send_buf+5), 0); + mp3_fill_checksum (); + send_func (); +} + + +// +void mp3_play_physical (uint16_t num) { + mp3_send_cmd (0x03, num); +} + +// +void mp3_play_physical () { + mp3_send_cmd (0x03); +} + +// +void mp3_next () { + mp3_send_cmd (0x01); +} + +// +void mp3_prev () { + mp3_send_cmd (0x02); +} + +//0x06 set volume 0-30 +void mp3_set_volume (uint16_t volume) { + mp3_send_cmd (0x06, volume); +} + +//0x07 set EQ0/1/2/3/4/5 Normal/Pop/Rock/Jazz/Classic/Bass +void mp3_set_EQ (uint16_t eq) { + mp3_send_cmd (0x07, eq); +} + +//0x09 set device 1/2/3/4/5 U/SD/AUX/SLEEP/FLASH +void mp3_set_device (uint16_t device) { + mp3_send_cmd (0x09, device); +} + +// +void mp3_sleep () { + mp3_send_cmd (0x0a); +} + +// +void mp3_reset () { + mp3_send_cmd (0x0c); +} + +// +void mp3_play () { + mp3_send_cmd (0x0d); +} + +// +void mp3_pause () { + mp3_send_cmd (0x0e); +} + +// +void mp3_stop () { + mp3_send_cmd (0x16); +} + +//play mp3 file in mp3 folder in your tf card +void mp3_play (uint16_t num) { + mp3_send_cmd (0x12, num); +} + +// +void mp3_get_state () { + mp3_send_cmd (0x42); +} + +// +void mp3_get_volume () { + mp3_send_cmd (0x43); +} + + +// +void mp3_get_u_sum () { + mp3_send_cmd (0x47); +} + +// +void mp3_get_tf_sum () { + mp3_send_cmd (0x48); +} + +// +void mp3_get_flash_sum () { + mp3_send_cmd (0x49); +} + + +// +void mp3_get_tf_current () { + mp3_send_cmd (0x4c); +} + +// +void mp3_get_u_current () { + mp3_send_cmd (0x4b); +} + + +// +void mp3_get_flash_current () { + mp3_send_cmd (0x4d); +} + +// +void mp3_single_loop (boolean state) { + mp3_send_cmd (0x19, !state); +} + +//add +void mp3_single_play (uint16_t num) { + mp3_play (num); + delay (10); + mp3_single_loop (true); + //mp3_send_cmd (0x19, !state); +} + +// +void mp3_DAC (boolean state) { + mp3_send_cmd (0x1a, !state); +} + +// +void mp3_random_play () { + mp3_send_cmd (0x18); +} + +// +void mp3_loop_play (uint16_t num) { + mp3_send_cmd (0x08, num); +} diff --git a/Libraries/DFPlayer_Mini_Mp3/DFPlayer_Mini_Mp3.h b/Libraries/DFPlayer_Mini_Mp3/DFPlayer_Mini_Mp3.h new file mode 100644 index 0000000..50f1a92 --- /dev/null +++ b/Libraries/DFPlayer_Mini_Mp3/DFPlayer_Mini_Mp3.h @@ -0,0 +1,174 @@ +/******************************************************************************* + * Copyright (C) 2014 DFRobot * + * * + * DFPlayer_Mini_Mp3, This library provides a quite complete function for * + * DFPlayer mini mp3 module. * + * www.github.com/dfrobot/DFPlayer_Mini_Mp3 (github as default source provider)* + * DFRobot-A great source for opensource hardware and robot. * + * * + * This file is part of the DFplayer_Mini_Mp3 library. * + * * + * DFPlayer_Mini_Mp3 is free software: you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 3 of * + * the License, or any later version. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with DFPlayer_Mini_Mp3. If not, see * + * . * + * * + ******************************************************************************/ + +/* + * name: DFPlayer_Mini_Mp3 + * version: 1.0 + * Author: lisper + * Date: 2014-05-22 + * official website: http://www.dfrobot.com + * Products page: http://www.dfrobot.com/index.php?route=product/product&product_id=1121#.U5Z_RYbUN8E + * Description: mp3 library for DFPlayer mini board + * note: mp3 file must put into mp3 folder in your tf card + */ +#include "Arduino.h" +#include "SoftwareSerial.h" + +uint8_t send_buf[10] = { + 0x7E, 0xFF, 06, 00, 00, 00, 00, 00, 00, 0xEF}; +uint8_t recv_buf[10]; + +//* void(*send_func)() = NULL; +//* HardwareSerial *hserial = NULL; +//* SoftwareSerial *sserial = NULL; +//* boolean is_reply = false; + +// 7E FF 06 0F 00 01 01 xx xx EF +// 0 -> 7E is start code +// 1 -> FF is version +// 2 -> 06 is length +// 3 -> 0F is command +// 4 -> 00 is no receive +// 5~6 -> 01 01 is argument +// 7~8 -> checksum = 0 - ( FF+06+0F+00+01+01 ) +// 9 -> EF is end code + +void mp3_set_reply (boolean state); + +void mp3_fill_cmd (uint8_t cmd, uint16_t arg); +void mp3_fill_cmd (uint8_t cmd); + +// +//void fill_uint16_bigend (uint8_t *thebuf, uint16_t data); + +//error because it is biggend mode in mp3 module +//void fill_uint16 (uint8_t *thebuf, uint16_t data) { +// *(uint16_t*)(thebuf) = data; +//} + +// +void mp3_set_serial (HardwareSerial &theSerial); + +// +void mp3_set_serial (SoftwareSerial &theSerial); + +// +//void h_send_func (); + +// +//void s_send_func (); + +// +void mp3_send_cmd (uint8_t cmd, uint16_t arg); + +// +void mp3_send_cmd (uint8_t cmd); + +// +uint16_t mp3_get_checksum (uint8_t *thebuf); + +// +void mp3_fill_checksum (); + +// +void mp3_play_physical (uint16_t num); +void mp3_play_physical (); + +// +void mp3_next (); + +// +void mp3_prev (); + +//0x06 set volume 0-30 +void mp3_set_volume (uint16_t volume); + +//0x07 set EQ0/1/2/3/4/5 Normal/Pop/Rock/Jazz/Classic/Bass +void mp3_set_EQ (uint16_t eq); + +//0x09 set device 1/2/3/4/5 U/SD/AUX/SLEEP/FLASH +void mp3_set_device (uint16_t device); + +// +void mp3_sleep (); + +// +void mp3_reset (); + +// +void mp3_pause (); + +// +void mp3_stop (); + +// +void mp3_play (); + +//specify a mp3 file in mp3 folder in your tf card, "mp3_play (1);" mean play "mp3/0001.mp3" +void mp3_play (uint16_t num); + +// +void mp3_get_state (); + +// +void mp3_get_volume (); + +// +void mp3_get_u_sum (); + +// +void mp3_get_tf_sum (); + +// +void mp3_get_flash_sum (); + +// +void mp3_get_tf_current (); + +// +void mp3_get_u_current (); + +// +void mp3_get_flash_current (); + +//set single loop +void mp3_single_loop (boolean state); + +void mp3_single_play (uint16_t num); + +// +void mp3_DAC (boolean state); + +// +void mp3_random_play (); + +// +void mp3_loop_play (uint16_t num); \ No newline at end of file diff --git a/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_Mini_Test/DFPlayer_Mini_Test.ino b/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_Mini_Test/DFPlayer_Mini_Test.ino new file mode 100644 index 0000000..36eb89c --- /dev/null +++ b/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_Mini_Test/DFPlayer_Mini_Test.ino @@ -0,0 +1,177 @@ +/******************************************************************************* + * DFPlayer_Mini_Mp3, This library provides a quite complete function for * + * DFPlayer mini mp3 module. * + * www.github.com/dfrobot/DFPlayer_Mini_Mp3 (github as default source provider)* + * DFRobot-A great source for opensource hardware and robot. * + * * + * This file is part of the DFplayer_Mini_Mp3 library. * + * * + * DFPlayer_Mini_Mp3 is free software: you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 3 of * + * the License, or any later version. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with DFPlayer_Mini_Mp3. If not, see * + * . * + * * + ******************************************************************************/ + +/* + * Copyright: DFRobot + * name: DFPlayer_Mini_Mp3 test code + * Author: lisper + * Date: 2014-05-22 + * Description: mp3 library for DFPlayer mini board + * + * this code is test on leonardo + * you can try input: + * play //play current music + * play 3 //play mp3/0003.mp3 + * next //play next + * prev //paly previous + * pause //pause current play + * stop //stop current play + * state //get current state of module + * current //get current track of tf card + * volume //get current volume + * volume 20 //set volume to 20 (0~30) + * single open/close //set single loop open or close + * reply open/close //set if need reply + */ + +#include +#include + +#include "DFPlayer_Mini_Mp3.h" + +#define BUFSIZE 20 //buf size +#define CMDNUM 8 //cmdbuf size + +uint8_t buf[BUFSIZE]; //data buffer for read from Serial +char *cmdbuf[CMDNUM]; //for split string from buf + +// +void setup () { + Serial1.begin (9600); + Serial.begin (9600); + while (!Serial); + + mp3_set_serial (Serial1); //set Serial1 for DFPlayer-mini mp3 module + mp3_set_volume (15); + mp3_get_tf_sum (); + print_info (); +} + +void print_info () { + Serial.println ("you send:"); + printHex (send_buf, 10); + delay (100); + int recv_leng = serialRead (Serial1, recv_buf, 10, 3); + if (recv_leng) { + Serial.println ("you get:"); + printHex (recv_buf, recv_leng); + } +} + +// +void loop () { + int leng; + leng = serialRead (Serial1, buf, BUFSIZE, 3); //first read data from Serial1 + if (leng) { + Serial.print ("=>"); + printHex (buf, leng); + } + leng = serialRead (Serial, buf, BUFSIZE, 3); //read command to buf from Serial (PC) + if (leng) { + buf[leng] = '\0'; + Serial.println ((char*)buf); + int cmdleng = split(cmdbuf, (char*)buf, 8); //split command string to cmdbuf + if (cmdleng >= 1) { + if (strcmp (cmdbuf[0], "next") == 0) { + mp3_next (); + print_info (); + } + else if (strcmp (cmdbuf[0], "physical") == 0) { + if (cmdleng == 2) { + mp3_play_physical (atoi (cmdbuf[1])); //get arguments + } else { + mp3_play_physical (); + } + print_info (); + } + else if (strcmp (cmdbuf[0], "prev") == 0) { + mp3_prev (); + print_info (); + } + else if (strcmp (cmdbuf[0], "stop") == 0) { + mp3_stop (); + print_info (); + } + else if (strcmp (cmdbuf[0], "pause") == 0) { + mp3_pause (); + print_info (); + } + else if (strcmp (cmdbuf[0], "state") == 0) { + mp3_get_state (); + print_info (); + } + else if (strcmp (cmdbuf[0], "sum") == 0) { + mp3_get_tf_sum (); + print_info (); + } + else if (strcmp (cmdbuf[0], "current") == 0) { + mp3_get_tf_current (); + print_info (); + } + else if (strcmp (cmdbuf[0], "volume") == 0) { + if (cmdleng == 2) { + mp3_set_volume (atoi (cmdbuf[1])); + } else { + mp3_get_volume (); + } + print_info (); + } + else if (strcmp (cmdbuf[0], "reply") == 0 && cmdleng == 2) { + if (strcmp (cmdbuf[1], "open") == 0) + mp3_set_reply (true); + else if (strcmp (cmdbuf[1], "close") == 0) + mp3_set_reply (false); + print_info (); + } + else if (strcmp (cmdbuf[0], "play") == 0 && cmdleng == 2) { + if (cmdleng == 2) { + mp3_play (atoi (cmdbuf[1])); + } else { + mp3_play (); + } + print_info (); + } + else if (strcmp (cmdbuf[0], "eq") == 0 && cmdleng == 2) { + mp3_set_EQ (atoi (cmdbuf[1])); + print_info (); + } + else if (strcmp (cmdbuf[0], "single") == 0 && cmdleng == 2) { + if (strcmp (cmdbuf[1], "open") == 0) + mp3_single_loop (true); + else if (strcmp (cmdbuf[1], "close") == 0) + mp3_single_loop (false); + print_info (); + } else { + Serial.println ("error! no this command"); + } + } + } +} + + diff --git a/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_SoftwareSerial/DFPlayer_SoftwareSerial.ino b/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_SoftwareSerial/DFPlayer_SoftwareSerial.ino new file mode 100644 index 0000000..2e46e50 --- /dev/null +++ b/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_SoftwareSerial/DFPlayer_SoftwareSerial.ino @@ -0,0 +1,85 @@ +/******************************************************************************* + * DFPlayer_Mini_Mp3, This library provides a quite complete function for * + * DFPlayer mini mp3 module. * + * www.github.com/dfrobot/DFPlayer_Mini_Mp3 (github as default source provider)* + * DFRobot-A great source for opensource hardware and robot. * + * * + * This file is part of the DFplayer_Mini_Mp3 library. * + * * + * DFPlayer_Mini_Mp3 is free software: you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 3 of * + * the License, or any later version. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with DFPlayer_Mini_Mp3. If not, see * + * . * + * * + ******************************************************************************/ + +/* + * Copyright: DFRobot + * name: DFPlayer_Mini_Mp3 sample code + * Author: lisper + * Date: 2014-05-30 + * Description: connect DFPlayer Mini by SoftwareSerial, this code is test on Uno + * Note: the mp3 files must put into mp3 folder in your tf card + */ +#include +#include + +SoftwareSerial mySerial(10, 11); // RX, TX + +// +void setup () { + Serial.begin (9600); + mySerial.begin (9600); + mp3_set_serial (mySerial); //set softwareSerial for DFPlayer-mini mp3 module + mp3_set_volume (15); +} + + +// +void loop () { + mp3_play (1); + delay (6000); + mp3_next (); + delay (6000); + mp3_prev (); + delay (6000); + mp3_play (4); + delay (6000); +} + +/* + mp3_play (); //start play + mp3_play (5); //play "mp3/0005.mp3" + mp3_next (); //play next + mp3_prev (); //play previous + mp3_set_volume (uint16_t volume); //0~30 + mp3_set_EQ (); //0~5 + mp3_pause (); + mp3_stop (); + void mp3_get_state (); //send get state command + void mp3_get_volume (); + void mp3_get_u_sum (); + void mp3_get_tf_sum (); + void mp3_get_flash_sum (); + void mp3_get_tf_current (); + void mp3_get_u_current (); + void mp3_get_flash_current (); + void mp3_single_loop (boolean state); //set single loop + void mp3_DAC (boolean state); + void mp3_random_play (); + */ + diff --git a/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_sample/DFPlayer_sample.ino b/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_sample/DFPlayer_sample.ino new file mode 100644 index 0000000..2177182 --- /dev/null +++ b/Libraries/DFPlayer_Mini_Mp3/examples/DFPlayer_sample/DFPlayer_sample.ino @@ -0,0 +1,82 @@ +/******************************************************************************* + * DFPlayer_Mini_Mp3, This library provides a quite complete function for * + * DFPlayer mini mp3 module. * + * www.github.com/dfrobot/DFPlayer_Mini_Mp3 (github as default source provider)* + * DFRobot-A great source for opensource hardware and robot. * + * * + * This file is part of the DFplayer_Mini_Mp3 library. * + * * + * DFPlayer_Mini_Mp3 is free software: you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 3 of * + * the License, or any later version. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * DFPlayer_Mini_Mp3 is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with DFPlayer_Mini_Mp3. If not, see * + * . * + * * + ******************************************************************************/ + +/* + * Copyright: DFRobot + * name: DFPlayer_Mini_Mp3 sample code + * Author: lisper + * Date: 2014-05-30 + * Description: sample code for DFPlayer Mini, this code is test on Uno + * note: mp3 file must put into mp3 folder in your tf card + */ + +#include +#include + +// +void setup () { + Serial.begin (9600); + mp3_set_serial (Serial); //set Serial for DFPlayer-mini mp3 module + mp3_set_volume (15); +} + + +// +void loop () { + mp3_play (1); + delay (6000); + mp3_next (); + delay (6000); + mp3_prev (); + delay (6000); + mp3_play (4); + delay (6000); +} + +/* + mp3_play (); //start play + mp3_play (5); //play "mp3/0005.mp3" + mp3_next (); //play next + mp3_prev (); //play previous + mp3_set_volume (uint16_t volume); //0~30 + mp3_set_EQ (); //0~5 + mp3_pause (); + mp3_stop (); + void mp3_get_state (); //send get state command + void mp3_get_volume (); + void mp3_get_u_sum (); + void mp3_get_tf_sum (); + void mp3_get_flash_sum (); + void mp3_get_tf_current (); + void mp3_get_u_current (); + void mp3_get_flash_current (); + void mp3_single_loop (boolean state); //set single loop + void mp3_DAC (boolean state); + void mp3_random_play (); + */ diff --git a/Libraries/DFPlayer_Mini_Mp3/keywords.txt b/Libraries/DFPlayer_Mini_Mp3/keywords.txt new file mode 100644 index 0000000..03a4dbc --- /dev/null +++ b/Libraries/DFPlayer_Mini_Mp3/keywords.txt @@ -0,0 +1,44 @@ +####################################### +# Syntax Coloring Map for DFPlayer_Mini_Mp3 +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +mp3_set_reply KEYWORD2 +fill_uint16_bigend KEYWORD2 +mp3_set_serial KEYWORD2 +get_mp3_checksum KEYWORD2 +mp3_fill_checksum KEYWORD2 +mp3_play KEYWORD2 +mp3_next KEYWORD2 +mp3_prev KEYWORD2 +mp3_set_volume KEYWORD2 +mp3_set_EQ KEYWORD2 +mp3_set_device KEYWORD2 +mp3_sleep KEYWORD2 +mp3_reset KEYWORD2 +mp3_pause KEYWORD2 +mp3_stop KEYWORD2 +mp3_play_mp3 KEYWORD2 +mp3_get_state KEYWORD2 +mp3_get_volume KEYWORD2 +mp3_get_u_sum KEYWORD2 +mp3_get_tf_sum KEYWORD2 +mp3_get_flash_sum KEYWORD2 +mp3_get_tf_current KEYWORD2 +mp3_get_u_current KEYWORD2 +mp3_get_flash_current KEYWORD2 +mp3_single_loop KEYWORD2 +mp3_DAC KEYWORD2 +mp3_random_play KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/Libraries/DFPlayer_Mini_Mp3/license.txt b/Libraries/DFPlayer_Mini_Mp3/license.txt new file mode 100644 index 0000000..65c5ca8 --- /dev/null +++ b/Libraries/DFPlayer_Mini_Mp3/license.txt @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/Libraries/MPU6050/MPU6050.h b/Libraries/MPU6050/MPU6050.h index 6e3b9a7..6bbd2fe 100644 --- a/Libraries/MPU6050/MPU6050.h +++ b/Libraries/MPU6050/MPU6050.h @@ -1,1032 +1,1033 @@ -// I2Cdev library collection - MPU6050 I2C device class -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 10/3/2011 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// ... - ongoing debug release - -// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE -// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF -// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _MPU6050_H_ -#define _MPU6050_H_ - -#include "I2Cdev.h" - -// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 -// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s - -#ifdef __AVR__ -#include -#else -//#define PROGMEM /* empty */ -//#define pgm_read_byte(x) (*(x)) -//#define pgm_read_word(x) (*(x)) -//#define pgm_read_float(x) (*(x)) -//#define PSTR(STR) STR -#endif - - -#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board -#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) -#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW - -#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN -#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN -#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN -#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS -#define MPU6050_RA_XA_OFFS_L_TC 0x07 -#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS -#define MPU6050_RA_YA_OFFS_L_TC 0x09 -#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS -#define MPU6050_RA_ZA_OFFS_L_TC 0x0B -#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] -#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] -#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] -#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] -#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR -#define MPU6050_RA_XG_OFFS_USRL 0x14 -#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR -#define MPU6050_RA_YG_OFFS_USRL 0x16 -#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR -#define MPU6050_RA_ZG_OFFS_USRL 0x18 -#define MPU6050_RA_SMPLRT_DIV 0x19 -#define MPU6050_RA_CONFIG 0x1A -#define MPU6050_RA_GYRO_CONFIG 0x1B -#define MPU6050_RA_ACCEL_CONFIG 0x1C -#define MPU6050_RA_FF_THR 0x1D -#define MPU6050_RA_FF_DUR 0x1E -#define MPU6050_RA_MOT_THR 0x1F -#define MPU6050_RA_MOT_DUR 0x20 -#define MPU6050_RA_ZRMOT_THR 0x21 -#define MPU6050_RA_ZRMOT_DUR 0x22 -#define MPU6050_RA_FIFO_EN 0x23 -#define MPU6050_RA_I2C_MST_CTRL 0x24 -#define MPU6050_RA_I2C_SLV0_ADDR 0x25 -#define MPU6050_RA_I2C_SLV0_REG 0x26 -#define MPU6050_RA_I2C_SLV0_CTRL 0x27 -#define MPU6050_RA_I2C_SLV1_ADDR 0x28 -#define MPU6050_RA_I2C_SLV1_REG 0x29 -#define MPU6050_RA_I2C_SLV1_CTRL 0x2A -#define MPU6050_RA_I2C_SLV2_ADDR 0x2B -#define MPU6050_RA_I2C_SLV2_REG 0x2C -#define MPU6050_RA_I2C_SLV2_CTRL 0x2D -#define MPU6050_RA_I2C_SLV3_ADDR 0x2E -#define MPU6050_RA_I2C_SLV3_REG 0x2F -#define MPU6050_RA_I2C_SLV3_CTRL 0x30 -#define MPU6050_RA_I2C_SLV4_ADDR 0x31 -#define MPU6050_RA_I2C_SLV4_REG 0x32 -#define MPU6050_RA_I2C_SLV4_DO 0x33 -#define MPU6050_RA_I2C_SLV4_CTRL 0x34 -#define MPU6050_RA_I2C_SLV4_DI 0x35 -#define MPU6050_RA_I2C_MST_STATUS 0x36 -#define MPU6050_RA_INT_PIN_CFG 0x37 -#define MPU6050_RA_INT_ENABLE 0x38 -#define MPU6050_RA_DMP_INT_STATUS 0x39 -#define MPU6050_RA_INT_STATUS 0x3A -#define MPU6050_RA_ACCEL_XOUT_H 0x3B -#define MPU6050_RA_ACCEL_XOUT_L 0x3C -#define MPU6050_RA_ACCEL_YOUT_H 0x3D -#define MPU6050_RA_ACCEL_YOUT_L 0x3E -#define MPU6050_RA_ACCEL_ZOUT_H 0x3F -#define MPU6050_RA_ACCEL_ZOUT_L 0x40 -#define MPU6050_RA_TEMP_OUT_H 0x41 -#define MPU6050_RA_TEMP_OUT_L 0x42 -#define MPU6050_RA_GYRO_XOUT_H 0x43 -#define MPU6050_RA_GYRO_XOUT_L 0x44 -#define MPU6050_RA_GYRO_YOUT_H 0x45 -#define MPU6050_RA_GYRO_YOUT_L 0x46 -#define MPU6050_RA_GYRO_ZOUT_H 0x47 -#define MPU6050_RA_GYRO_ZOUT_L 0x48 -#define MPU6050_RA_EXT_SENS_DATA_00 0x49 -#define MPU6050_RA_EXT_SENS_DATA_01 0x4A -#define MPU6050_RA_EXT_SENS_DATA_02 0x4B -#define MPU6050_RA_EXT_SENS_DATA_03 0x4C -#define MPU6050_RA_EXT_SENS_DATA_04 0x4D -#define MPU6050_RA_EXT_SENS_DATA_05 0x4E -#define MPU6050_RA_EXT_SENS_DATA_06 0x4F -#define MPU6050_RA_EXT_SENS_DATA_07 0x50 -#define MPU6050_RA_EXT_SENS_DATA_08 0x51 -#define MPU6050_RA_EXT_SENS_DATA_09 0x52 -#define MPU6050_RA_EXT_SENS_DATA_10 0x53 -#define MPU6050_RA_EXT_SENS_DATA_11 0x54 -#define MPU6050_RA_EXT_SENS_DATA_12 0x55 -#define MPU6050_RA_EXT_SENS_DATA_13 0x56 -#define MPU6050_RA_EXT_SENS_DATA_14 0x57 -#define MPU6050_RA_EXT_SENS_DATA_15 0x58 -#define MPU6050_RA_EXT_SENS_DATA_16 0x59 -#define MPU6050_RA_EXT_SENS_DATA_17 0x5A -#define MPU6050_RA_EXT_SENS_DATA_18 0x5B -#define MPU6050_RA_EXT_SENS_DATA_19 0x5C -#define MPU6050_RA_EXT_SENS_DATA_20 0x5D -#define MPU6050_RA_EXT_SENS_DATA_21 0x5E -#define MPU6050_RA_EXT_SENS_DATA_22 0x5F -#define MPU6050_RA_EXT_SENS_DATA_23 0x60 -#define MPU6050_RA_MOT_DETECT_STATUS 0x61 -#define MPU6050_RA_I2C_SLV0_DO 0x63 -#define MPU6050_RA_I2C_SLV1_DO 0x64 -#define MPU6050_RA_I2C_SLV2_DO 0x65 -#define MPU6050_RA_I2C_SLV3_DO 0x66 -#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 -#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 -#define MPU6050_RA_MOT_DETECT_CTRL 0x69 -#define MPU6050_RA_USER_CTRL 0x6A -#define MPU6050_RA_PWR_MGMT_1 0x6B -#define MPU6050_RA_PWR_MGMT_2 0x6C -#define MPU6050_RA_BANK_SEL 0x6D -#define MPU6050_RA_MEM_START_ADDR 0x6E -#define MPU6050_RA_MEM_R_W 0x6F -#define MPU6050_RA_DMP_CFG_1 0x70 -#define MPU6050_RA_DMP_CFG_2 0x71 -#define MPU6050_RA_FIFO_COUNTH 0x72 -#define MPU6050_RA_FIFO_COUNTL 0x73 -#define MPU6050_RA_FIFO_R_W 0x74 -#define MPU6050_RA_WHO_AM_I 0x75 - -#define MPU6050_SELF_TEST_XA_1_BIT 0x07 -#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 -#define MPU6050_SELF_TEST_XA_2_BIT 0x05 -#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 -#define MPU6050_SELF_TEST_YA_1_BIT 0x07 -#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 -#define MPU6050_SELF_TEST_YA_2_BIT 0x03 -#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 -#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 -#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 -#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 -#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 - -#define MPU6050_SELF_TEST_XG_1_BIT 0x04 -#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 -#define MPU6050_SELF_TEST_YG_1_BIT 0x04 -#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 -#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 -#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 - -#define MPU6050_TC_PWR_MODE_BIT 7 -#define MPU6050_TC_OFFSET_BIT 6 -#define MPU6050_TC_OFFSET_LENGTH 6 -#define MPU6050_TC_OTP_BNK_VLD_BIT 0 - -#define MPU6050_VDDIO_LEVEL_VLOGIC 0 -#define MPU6050_VDDIO_LEVEL_VDD 1 - -#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 -#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 -#define MPU6050_CFG_DLPF_CFG_BIT 2 -#define MPU6050_CFG_DLPF_CFG_LENGTH 3 - -#define MPU6050_EXT_SYNC_DISABLED 0x0 -#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 -#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 -#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 -#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 -#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 -#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 -#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 - -#define MPU6050_DLPF_BW_256 0x00 -#define MPU6050_DLPF_BW_188 0x01 -#define MPU6050_DLPF_BW_98 0x02 -#define MPU6050_DLPF_BW_42 0x03 -#define MPU6050_DLPF_BW_20 0x04 -#define MPU6050_DLPF_BW_10 0x05 -#define MPU6050_DLPF_BW_5 0x06 - -#define MPU6050_GCONFIG_FS_SEL_BIT 4 -#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 - -#define MPU6050_GYRO_FS_250 0x00 -#define MPU6050_GYRO_FS_500 0x01 -#define MPU6050_GYRO_FS_1000 0x02 -#define MPU6050_GYRO_FS_2000 0x03 - -#define MPU6050_ACONFIG_XA_ST_BIT 7 -#define MPU6050_ACONFIG_YA_ST_BIT 6 -#define MPU6050_ACONFIG_ZA_ST_BIT 5 -#define MPU6050_ACONFIG_AFS_SEL_BIT 4 -#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 -#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 -#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 - -#define MPU6050_ACCEL_FS_2 0x00 -#define MPU6050_ACCEL_FS_4 0x01 -#define MPU6050_ACCEL_FS_8 0x02 -#define MPU6050_ACCEL_FS_16 0x03 - -#define MPU6050_DHPF_RESET 0x00 -#define MPU6050_DHPF_5 0x01 -#define MPU6050_DHPF_2P5 0x02 -#define MPU6050_DHPF_1P25 0x03 -#define MPU6050_DHPF_0P63 0x04 -#define MPU6050_DHPF_HOLD 0x07 - -#define MPU6050_TEMP_FIFO_EN_BIT 7 -#define MPU6050_XG_FIFO_EN_BIT 6 -#define MPU6050_YG_FIFO_EN_BIT 5 -#define MPU6050_ZG_FIFO_EN_BIT 4 -#define MPU6050_ACCEL_FIFO_EN_BIT 3 -#define MPU6050_SLV2_FIFO_EN_BIT 2 -#define MPU6050_SLV1_FIFO_EN_BIT 1 -#define MPU6050_SLV0_FIFO_EN_BIT 0 - -#define MPU6050_MULT_MST_EN_BIT 7 -#define MPU6050_WAIT_FOR_ES_BIT 6 -#define MPU6050_SLV_3_FIFO_EN_BIT 5 -#define MPU6050_I2C_MST_P_NSR_BIT 4 -#define MPU6050_I2C_MST_CLK_BIT 3 -#define MPU6050_I2C_MST_CLK_LENGTH 4 - -#define MPU6050_CLOCK_DIV_348 0x0 -#define MPU6050_CLOCK_DIV_333 0x1 -#define MPU6050_CLOCK_DIV_320 0x2 -#define MPU6050_CLOCK_DIV_308 0x3 -#define MPU6050_CLOCK_DIV_296 0x4 -#define MPU6050_CLOCK_DIV_286 0x5 -#define MPU6050_CLOCK_DIV_276 0x6 -#define MPU6050_CLOCK_DIV_267 0x7 -#define MPU6050_CLOCK_DIV_258 0x8 -#define MPU6050_CLOCK_DIV_500 0x9 -#define MPU6050_CLOCK_DIV_471 0xA -#define MPU6050_CLOCK_DIV_444 0xB -#define MPU6050_CLOCK_DIV_421 0xC -#define MPU6050_CLOCK_DIV_400 0xD -#define MPU6050_CLOCK_DIV_381 0xE -#define MPU6050_CLOCK_DIV_364 0xF - -#define MPU6050_I2C_SLV_RW_BIT 7 -#define MPU6050_I2C_SLV_ADDR_BIT 6 -#define MPU6050_I2C_SLV_ADDR_LENGTH 7 -#define MPU6050_I2C_SLV_EN_BIT 7 -#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 -#define MPU6050_I2C_SLV_REG_DIS_BIT 5 -#define MPU6050_I2C_SLV_GRP_BIT 4 -#define MPU6050_I2C_SLV_LEN_BIT 3 -#define MPU6050_I2C_SLV_LEN_LENGTH 4 - -#define MPU6050_I2C_SLV4_RW_BIT 7 -#define MPU6050_I2C_SLV4_ADDR_BIT 6 -#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 -#define MPU6050_I2C_SLV4_EN_BIT 7 -#define MPU6050_I2C_SLV4_INT_EN_BIT 6 -#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 -#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 -#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 - -#define MPU6050_MST_PASS_THROUGH_BIT 7 -#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 -#define MPU6050_MST_I2C_LOST_ARB_BIT 5 -#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 -#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 -#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 -#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 -#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 - -#define MPU6050_INTCFG_INT_LEVEL_BIT 7 -#define MPU6050_INTCFG_INT_OPEN_BIT 6 -#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 -#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 -#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 -#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 -#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 -#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 - -#define MPU6050_INTMODE_ACTIVEHIGH 0x00 -#define MPU6050_INTMODE_ACTIVELOW 0x01 - -#define MPU6050_INTDRV_PUSHPULL 0x00 -#define MPU6050_INTDRV_OPENDRAIN 0x01 - -#define MPU6050_INTLATCH_50USPULSE 0x00 -#define MPU6050_INTLATCH_WAITCLEAR 0x01 - -#define MPU6050_INTCLEAR_STATUSREAD 0x00 -#define MPU6050_INTCLEAR_ANYREAD 0x01 - -#define MPU6050_INTERRUPT_FF_BIT 7 -#define MPU6050_INTERRUPT_MOT_BIT 6 -#define MPU6050_INTERRUPT_ZMOT_BIT 5 -#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 -#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 -#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 -#define MPU6050_INTERRUPT_DMP_INT_BIT 1 -#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 - -// TODO: figure out what these actually do -// UMPL source code is not very obivous -#define MPU6050_DMPINT_5_BIT 5 -#define MPU6050_DMPINT_4_BIT 4 -#define MPU6050_DMPINT_3_BIT 3 -#define MPU6050_DMPINT_2_BIT 2 -#define MPU6050_DMPINT_1_BIT 1 -#define MPU6050_DMPINT_0_BIT 0 - -#define MPU6050_MOTION_MOT_XNEG_BIT 7 -#define MPU6050_MOTION_MOT_XPOS_BIT 6 -#define MPU6050_MOTION_MOT_YNEG_BIT 5 -#define MPU6050_MOTION_MOT_YPOS_BIT 4 -#define MPU6050_MOTION_MOT_ZNEG_BIT 3 -#define MPU6050_MOTION_MOT_ZPOS_BIT 2 -#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 - -#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 -#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 -#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 -#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 -#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 -#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 - -#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 -#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 -#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 - -#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 -#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 -#define MPU6050_DETECT_FF_COUNT_BIT 3 -#define MPU6050_DETECT_FF_COUNT_LENGTH 2 -#define MPU6050_DETECT_MOT_COUNT_BIT 1 -#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 - -#define MPU6050_DETECT_DECREMENT_RESET 0x0 -#define MPU6050_DETECT_DECREMENT_1 0x1 -#define MPU6050_DETECT_DECREMENT_2 0x2 -#define MPU6050_DETECT_DECREMENT_4 0x3 - -#define MPU6050_USERCTRL_DMP_EN_BIT 7 -#define MPU6050_USERCTRL_FIFO_EN_BIT 6 -#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 -#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 -#define MPU6050_USERCTRL_DMP_RESET_BIT 3 -#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 -#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 -#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 - -#define MPU6050_PWR1_DEVICE_RESET_BIT 7 -#define MPU6050_PWR1_SLEEP_BIT 6 -#define MPU6050_PWR1_CYCLE_BIT 5 -#define MPU6050_PWR1_TEMP_DIS_BIT 3 -#define MPU6050_PWR1_CLKSEL_BIT 2 -#define MPU6050_PWR1_CLKSEL_LENGTH 3 - -#define MPU6050_CLOCK_INTERNAL 0x00 -#define MPU6050_CLOCK_PLL_XGYRO 0x01 -#define MPU6050_CLOCK_PLL_YGYRO 0x02 -#define MPU6050_CLOCK_PLL_ZGYRO 0x03 -#define MPU6050_CLOCK_PLL_EXT32K 0x04 -#define MPU6050_CLOCK_PLL_EXT19M 0x05 -#define MPU6050_CLOCK_KEEP_RESET 0x07 - -#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 -#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 -#define MPU6050_PWR2_STBY_XA_BIT 5 -#define MPU6050_PWR2_STBY_YA_BIT 4 -#define MPU6050_PWR2_STBY_ZA_BIT 3 -#define MPU6050_PWR2_STBY_XG_BIT 2 -#define MPU6050_PWR2_STBY_YG_BIT 1 -#define MPU6050_PWR2_STBY_ZG_BIT 0 - -#define MPU6050_WAKE_FREQ_1P25 0x0 -#define MPU6050_WAKE_FREQ_2P5 0x1 -#define MPU6050_WAKE_FREQ_5 0x2 -#define MPU6050_WAKE_FREQ_10 0x3 - -#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 -#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 -#define MPU6050_BANKSEL_MEM_SEL_BIT 4 -#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 - -#define MPU6050_WHO_AM_I_BIT 6 -#define MPU6050_WHO_AM_I_LENGTH 6 - -#define MPU6050_DMP_MEMORY_BANKS 8 -#define MPU6050_DMP_MEMORY_BANK_SIZE 256 -#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 - -// note: DMP code memory blocks defined at end of header file - -class MPU6050 { - public: - MPU6050(); - MPU6050(uint8_t address); - - void initialize(); - bool testConnection(); - - // AUX_VDDIO register - uint8_t getAuxVDDIOLevel(); - void setAuxVDDIOLevel(uint8_t level); - - // SMPLRT_DIV register - uint8_t getRate(); - void setRate(uint8_t rate); - - // CONFIG register - uint8_t getExternalFrameSync(); - void setExternalFrameSync(uint8_t sync); - uint8_t getDLPFMode(); - void setDLPFMode(uint8_t bandwidth); - - // GYRO_CONFIG register - uint8_t getFullScaleGyroRange(); - void setFullScaleGyroRange(uint8_t range); - - // SELF_TEST registers - uint8_t getAccelXSelfTestFactoryTrim(); - uint8_t getAccelYSelfTestFactoryTrim(); - uint8_t getAccelZSelfTestFactoryTrim(); - - uint8_t getGyroXSelfTestFactoryTrim(); - uint8_t getGyroYSelfTestFactoryTrim(); - uint8_t getGyroZSelfTestFactoryTrim(); - - // ACCEL_CONFIG register - bool getAccelXSelfTest(); - void setAccelXSelfTest(bool enabled); - bool getAccelYSelfTest(); - void setAccelYSelfTest(bool enabled); - bool getAccelZSelfTest(); - void setAccelZSelfTest(bool enabled); - uint8_t getFullScaleAccelRange(); - void setFullScaleAccelRange(uint8_t range); - uint8_t getDHPFMode(); - void setDHPFMode(uint8_t mode); - - // FF_THR register - uint8_t getFreefallDetectionThreshold(); - void setFreefallDetectionThreshold(uint8_t threshold); - - // FF_DUR register - uint8_t getFreefallDetectionDuration(); - void setFreefallDetectionDuration(uint8_t duration); - - // MOT_THR register - uint8_t getMotionDetectionThreshold(); - void setMotionDetectionThreshold(uint8_t threshold); - - // MOT_DUR register - uint8_t getMotionDetectionDuration(); - void setMotionDetectionDuration(uint8_t duration); - - // ZRMOT_THR register - uint8_t getZeroMotionDetectionThreshold(); - void setZeroMotionDetectionThreshold(uint8_t threshold); - - // ZRMOT_DUR register - uint8_t getZeroMotionDetectionDuration(); - void setZeroMotionDetectionDuration(uint8_t duration); - - // FIFO_EN register - bool getTempFIFOEnabled(); - void setTempFIFOEnabled(bool enabled); - bool getXGyroFIFOEnabled(); - void setXGyroFIFOEnabled(bool enabled); - bool getYGyroFIFOEnabled(); - void setYGyroFIFOEnabled(bool enabled); - bool getZGyroFIFOEnabled(); - void setZGyroFIFOEnabled(bool enabled); - bool getAccelFIFOEnabled(); - void setAccelFIFOEnabled(bool enabled); - bool getSlave2FIFOEnabled(); - void setSlave2FIFOEnabled(bool enabled); - bool getSlave1FIFOEnabled(); - void setSlave1FIFOEnabled(bool enabled); - bool getSlave0FIFOEnabled(); - void setSlave0FIFOEnabled(bool enabled); - - // I2C_MST_CTRL register - bool getMultiMasterEnabled(); - void setMultiMasterEnabled(bool enabled); - bool getWaitForExternalSensorEnabled(); - void setWaitForExternalSensorEnabled(bool enabled); - bool getSlave3FIFOEnabled(); - void setSlave3FIFOEnabled(bool enabled); - bool getSlaveReadWriteTransitionEnabled(); - void setSlaveReadWriteTransitionEnabled(bool enabled); - uint8_t getMasterClockSpeed(); - void setMasterClockSpeed(uint8_t speed); - - // I2C_SLV* registers (Slave 0-3) - uint8_t getSlaveAddress(uint8_t num); - void setSlaveAddress(uint8_t num, uint8_t address); - uint8_t getSlaveRegister(uint8_t num); - void setSlaveRegister(uint8_t num, uint8_t reg); - bool getSlaveEnabled(uint8_t num); - void setSlaveEnabled(uint8_t num, bool enabled); - bool getSlaveWordByteSwap(uint8_t num); - void setSlaveWordByteSwap(uint8_t num, bool enabled); - bool getSlaveWriteMode(uint8_t num); - void setSlaveWriteMode(uint8_t num, bool mode); - bool getSlaveWordGroupOffset(uint8_t num); - void setSlaveWordGroupOffset(uint8_t num, bool enabled); - uint8_t getSlaveDataLength(uint8_t num); - void setSlaveDataLength(uint8_t num, uint8_t length); - - // I2C_SLV* registers (Slave 4) - uint8_t getSlave4Address(); - void setSlave4Address(uint8_t address); - uint8_t getSlave4Register(); - void setSlave4Register(uint8_t reg); - void setSlave4OutputByte(uint8_t data); - bool getSlave4Enabled(); - void setSlave4Enabled(bool enabled); - bool getSlave4InterruptEnabled(); - void setSlave4InterruptEnabled(bool enabled); - bool getSlave4WriteMode(); - void setSlave4WriteMode(bool mode); - uint8_t getSlave4MasterDelay(); - void setSlave4MasterDelay(uint8_t delay); - uint8_t getSlate4InputByte(); - - // I2C_MST_STATUS register - bool getPassthroughStatus(); - bool getSlave4IsDone(); - bool getLostArbitration(); - bool getSlave4Nack(); - bool getSlave3Nack(); - bool getSlave2Nack(); - bool getSlave1Nack(); - bool getSlave0Nack(); - - // INT_PIN_CFG register - bool getInterruptMode(); - void setInterruptMode(bool mode); - bool getInterruptDrive(); - void setInterruptDrive(bool drive); - bool getInterruptLatch(); - void setInterruptLatch(bool latch); - bool getInterruptLatchClear(); - void setInterruptLatchClear(bool clear); - bool getFSyncInterruptLevel(); - void setFSyncInterruptLevel(bool level); - bool getFSyncInterruptEnabled(); - void setFSyncInterruptEnabled(bool enabled); - bool getI2CBypassEnabled(); - void setI2CBypassEnabled(bool enabled); - bool getClockOutputEnabled(); - void setClockOutputEnabled(bool enabled); - - // INT_ENABLE register - uint8_t getIntEnabled(); - void setIntEnabled(uint8_t enabled); - bool getIntFreefallEnabled(); - void setIntFreefallEnabled(bool enabled); - bool getIntMotionEnabled(); - void setIntMotionEnabled(bool enabled); - bool getIntZeroMotionEnabled(); - void setIntZeroMotionEnabled(bool enabled); - bool getIntFIFOBufferOverflowEnabled(); - void setIntFIFOBufferOverflowEnabled(bool enabled); - bool getIntI2CMasterEnabled(); - void setIntI2CMasterEnabled(bool enabled); - bool getIntDataReadyEnabled(); - void setIntDataReadyEnabled(bool enabled); - - // INT_STATUS register - uint8_t getIntStatus(); - bool getIntFreefallStatus(); - bool getIntMotionStatus(); - bool getIntZeroMotionStatus(); - bool getIntFIFOBufferOverflowStatus(); - bool getIntI2CMasterStatus(); - bool getIntDataReadyStatus(); - - // ACCEL_*OUT_* registers - 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); - void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); - void getAcceleration(int16_t* x, int16_t* y, int16_t* z); - int16_t getAccelerationX(); - int16_t getAccelerationY(); - int16_t getAccelerationZ(); - - // TEMP_OUT_* registers - int16_t getTemperature(); - - // GYRO_*OUT_* registers - void getRotation(int16_t* x, int16_t* y, int16_t* z); - int16_t getRotationX(); - int16_t getRotationY(); - int16_t getRotationZ(); - - // EXT_SENS_DATA_* registers - uint8_t getExternalSensorByte(int position); - uint16_t getExternalSensorWord(int position); - uint32_t getExternalSensorDWord(int position); - - // MOT_DETECT_STATUS register - uint8_t getMotionStatus(); - bool getXNegMotionDetected(); - bool getXPosMotionDetected(); - bool getYNegMotionDetected(); - bool getYPosMotionDetected(); - bool getZNegMotionDetected(); - bool getZPosMotionDetected(); - bool getZeroMotionDetected(); - - // I2C_SLV*_DO register - void setSlaveOutputByte(uint8_t num, uint8_t data); - - // I2C_MST_DELAY_CTRL register - bool getExternalShadowDelayEnabled(); - void setExternalShadowDelayEnabled(bool enabled); - bool getSlaveDelayEnabled(uint8_t num); - void setSlaveDelayEnabled(uint8_t num, bool enabled); - - // SIGNAL_PATH_RESET register - void resetGyroscopePath(); - void resetAccelerometerPath(); - void resetTemperaturePath(); - - // MOT_DETECT_CTRL register - uint8_t getAccelerometerPowerOnDelay(); - void setAccelerometerPowerOnDelay(uint8_t delay); - uint8_t getFreefallDetectionCounterDecrement(); - void setFreefallDetectionCounterDecrement(uint8_t decrement); - uint8_t getMotionDetectionCounterDecrement(); - void setMotionDetectionCounterDecrement(uint8_t decrement); - - // USER_CTRL register - bool getFIFOEnabled(); - void setFIFOEnabled(bool enabled); - bool getI2CMasterModeEnabled(); - void setI2CMasterModeEnabled(bool enabled); - void switchSPIEnabled(bool enabled); - void resetFIFO(); - void resetI2CMaster(); - void resetSensors(); - - // PWR_MGMT_1 register - void reset(); - bool getSleepEnabled(); - void setSleepEnabled(bool enabled); - bool getWakeCycleEnabled(); - void setWakeCycleEnabled(bool enabled); - bool getTempSensorEnabled(); - void setTempSensorEnabled(bool enabled); - uint8_t getClockSource(); - void setClockSource(uint8_t source); - - // PWR_MGMT_2 register - uint8_t getWakeFrequency(); - void setWakeFrequency(uint8_t frequency); - bool getStandbyXAccelEnabled(); - void setStandbyXAccelEnabled(bool enabled); - bool getStandbyYAccelEnabled(); - void setStandbyYAccelEnabled(bool enabled); - bool getStandbyZAccelEnabled(); - void setStandbyZAccelEnabled(bool enabled); - bool getStandbyXGyroEnabled(); - void setStandbyXGyroEnabled(bool enabled); - bool getStandbyYGyroEnabled(); - void setStandbyYGyroEnabled(bool enabled); - bool getStandbyZGyroEnabled(); - void setStandbyZGyroEnabled(bool enabled); - - // FIFO_COUNT_* registers - uint16_t getFIFOCount(); - - // FIFO_R_W register - uint8_t getFIFOByte(); - void setFIFOByte(uint8_t data); - void getFIFOBytes(uint8_t *data, uint8_t length); - - // WHO_AM_I register - uint8_t getDeviceID(); - void setDeviceID(uint8_t id); - - // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== - - // XG_OFFS_TC register - uint8_t getOTPBankValid(); - void setOTPBankValid(bool enabled); - int8_t getXGyroOffsetTC(); - void setXGyroOffsetTC(int8_t offset); - - // YG_OFFS_TC register - int8_t getYGyroOffsetTC(); - void setYGyroOffsetTC(int8_t offset); - - // ZG_OFFS_TC register - int8_t getZGyroOffsetTC(); - void setZGyroOffsetTC(int8_t offset); - - // X_FINE_GAIN register - int8_t getXFineGain(); - void setXFineGain(int8_t gain); - - // Y_FINE_GAIN register - int8_t getYFineGain(); - void setYFineGain(int8_t gain); - - // Z_FINE_GAIN register - int8_t getZFineGain(); - void setZFineGain(int8_t gain); - - // XA_OFFS_* registers - int16_t getXAccelOffset(); - void setXAccelOffset(int16_t offset); - - // YA_OFFS_* register - int16_t getYAccelOffset(); - void setYAccelOffset(int16_t offset); - - // ZA_OFFS_* register - int16_t getZAccelOffset(); - void setZAccelOffset(int16_t offset); - - // XG_OFFS_USR* registers - int16_t getXGyroOffset(); - void setXGyroOffset(int16_t offset); - - // YG_OFFS_USR* register - int16_t getYGyroOffset(); - void setYGyroOffset(int16_t offset); - - // ZG_OFFS_USR* register - int16_t getZGyroOffset(); - void setZGyroOffset(int16_t offset); - - // INT_ENABLE register (DMP functions) - bool getIntPLLReadyEnabled(); - void setIntPLLReadyEnabled(bool enabled); - bool getIntDMPEnabled(); - void setIntDMPEnabled(bool enabled); - - // DMP_INT_STATUS - bool getDMPInt5Status(); - bool getDMPInt4Status(); - bool getDMPInt3Status(); - bool getDMPInt2Status(); - bool getDMPInt1Status(); - bool getDMPInt0Status(); - - // INT_STATUS register (DMP functions) - bool getIntPLLReadyStatus(); - bool getIntDMPStatus(); - - // USER_CTRL register (DMP functions) - bool getDMPEnabled(); - void setDMPEnabled(bool enabled); - void resetDMP(); - - // BANK_SEL register - void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); - - // MEM_START_ADDR register - void setMemoryStartAddress(uint8_t address); - - // MEM_R_W register - uint8_t readMemoryByte(); - void writeMemoryByte(uint8_t data); - void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); - bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); - bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); - - bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); - bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); - - // DMP_CFG_1 register - uint8_t getDMPConfig1(); - void setDMPConfig1(uint8_t config); - - // DMP_CFG_2 register - uint8_t getDMPConfig2(); - void setDMPConfig2(uint8_t config); - - // special methods for MotionApps 2.0 implementation - #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 - uint8_t *dmpPacketBuffer; - uint16_t dmpPacketSize; - - uint8_t dmpInitialize(); - bool dmpPacketAvailable(); - - uint8_t dmpSetFIFORate(uint8_t fifoRate); - uint8_t dmpGetFIFORate(); - uint8_t dmpGetSampleStepSizeMS(); - uint8_t dmpGetSampleFrequency(); - int32_t dmpDecodeTemperature(int8_t tempReg); - - // Register callbacks after a packet of FIFO data is processed - //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); - //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); - uint8_t dmpRunFIFORateProcesses(); - - // Setup FIFO for various output - uint8_t dmpSendQuaternion(uint_fast16_t accuracy); - uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); - uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - - // Get Fixed Point data from FIFO - uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpSetLinearAccelFilterCoefficient(float coef); - uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); - uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); - uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); - uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); - uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); - - uint8_t dmpGetEuler(float *data, Quaternion *q); - uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); - - // Get Floating Point data from FIFO - uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); - - uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); - uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); - - uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); - - uint8_t dmpInitFIFOParam(); - uint8_t dmpCloseFIFO(); - uint8_t dmpSetGyroDataSource(uint8_t source); - uint8_t dmpDecodeQuantizedAccel(); - uint32_t dmpGetGyroSumOfSquare(); - uint32_t dmpGetAccelSumOfSquare(); - void dmpOverrideQuaternion(long *q); - uint16_t dmpGetFIFOPacketSize(); - #endif - - // special methods for MotionApps 4.1 implementation - #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 - uint8_t *dmpPacketBuffer; - uint16_t dmpPacketSize; - - uint8_t dmpInitialize(); - bool dmpPacketAvailable(); - - uint8_t dmpSetFIFORate(uint8_t fifoRate); - uint8_t dmpGetFIFORate(); - uint8_t dmpGetSampleStepSizeMS(); - uint8_t dmpGetSampleFrequency(); - int32_t dmpDecodeTemperature(int8_t tempReg); - - // Register callbacks after a packet of FIFO data is processed - //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); - //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); - uint8_t dmpRunFIFORateProcesses(); - - // Setup FIFO for various output - uint8_t dmpSendQuaternion(uint_fast16_t accuracy); - uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); - uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - - // Get Fixed Point data from FIFO - uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); - uint8_t dmpSetLinearAccelFilterCoefficient(float coef); - uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); - uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); - uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); - uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); - uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); - - uint8_t dmpGetEuler(float *data, Quaternion *q); - uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); - - // Get Floating Point data from FIFO - uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); - - uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); - uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); - - uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); - - uint8_t dmpInitFIFOParam(); - uint8_t dmpCloseFIFO(); - uint8_t dmpSetGyroDataSource(uint8_t source); - uint8_t dmpDecodeQuantizedAccel(); - uint32_t dmpGetGyroSumOfSquare(); - uint32_t dmpGetAccelSumOfSquare(); - void dmpOverrideQuaternion(long *q); - uint16_t dmpGetFIFOPacketSize(); - #endif - - private: - uint8_t devAddr; - uint8_t buffer[14]; -}; - -#endif /* _MPU6050_H_ */ +// I2Cdev library collection - MPU6050 I2C device class +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 10/3/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_H_ +#define _MPU6050_H_ + +#include "I2Cdev.h" + +// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 +// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s + +#ifdef __AVR__ +#include +#else +//#define PROGMEM /* empty */ +//#define pgm_read_byte(x) (*(x)) +//#define pgm_read_word(x) (*(x)) +//#define pgm_read_float(x) (*(x)) +//#define PSTR(STR) STR +#endif + + +#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW + +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +#define MPU6050_SELF_TEST_XA_1_BIT 0x07 +#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_XA_2_BIT 0x05 +#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_YA_1_BIT 0x07 +#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_YA_2_BIT 0x03 +#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 +#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 +#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 + +#define MPU6050_SELF_TEST_XG_1_BIT 0x04 +#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_YG_1_BIT 0x04 +#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 +#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 + +#define MPU6050_TC_PWR_MODE_BIT 7 +#define MPU6050_TC_OFFSET_BIT 6 +#define MPU6050_TC_OFFSET_LENGTH 6 +#define MPU6050_TC_OTP_BNK_VLD_BIT 0 + +#define MPU6050_VDDIO_LEVEL_VLOGIC 0 +#define MPU6050_VDDIO_LEVEL_VDD 1 + +#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 +#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 +#define MPU6050_CFG_DLPF_CFG_BIT 2 +#define MPU6050_CFG_DLPF_CFG_LENGTH 3 + +#define MPU6050_EXT_SYNC_DISABLED 0x0 +#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 +#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 +#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 +#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 +#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 +#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 +#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 + +#define MPU6050_DLPF_BW_256 0x00 +#define MPU6050_DLPF_BW_188 0x01 +#define MPU6050_DLPF_BW_98 0x02 +#define MPU6050_DLPF_BW_42 0x03 +#define MPU6050_DLPF_BW_20 0x04 +#define MPU6050_DLPF_BW_10 0x05 +#define MPU6050_DLPF_BW_5 0x06 + +#define MPU6050_GCONFIG_FS_SEL_BIT 4 +#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 + +#define MPU6050_GYRO_FS_250 0x00 +#define MPU6050_GYRO_FS_500 0x01 +#define MPU6050_GYRO_FS_1000 0x02 +#define MPU6050_GYRO_FS_2000 0x03 + +#define MPU6050_ACONFIG_XA_ST_BIT 7 +#define MPU6050_ACONFIG_YA_ST_BIT 6 +#define MPU6050_ACONFIG_ZA_ST_BIT 5 +#define MPU6050_ACONFIG_AFS_SEL_BIT 4 +#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 +#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 +#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 + +#define MPU6050_ACCEL_FS_2 0x00 +#define MPU6050_ACCEL_FS_4 0x01 +#define MPU6050_ACCEL_FS_8 0x02 +#define MPU6050_ACCEL_FS_16 0x03 + +#define MPU6050_DHPF_RESET 0x00 +#define MPU6050_DHPF_5 0x01 +#define MPU6050_DHPF_2P5 0x02 +#define MPU6050_DHPF_1P25 0x03 +#define MPU6050_DHPF_0P63 0x04 +#define MPU6050_DHPF_HOLD 0x07 + +#define MPU6050_TEMP_FIFO_EN_BIT 7 +#define MPU6050_XG_FIFO_EN_BIT 6 +#define MPU6050_YG_FIFO_EN_BIT 5 +#define MPU6050_ZG_FIFO_EN_BIT 4 +#define MPU6050_ACCEL_FIFO_EN_BIT 3 +#define MPU6050_SLV2_FIFO_EN_BIT 2 +#define MPU6050_SLV1_FIFO_EN_BIT 1 +#define MPU6050_SLV0_FIFO_EN_BIT 0 + +#define MPU6050_MULT_MST_EN_BIT 7 +#define MPU6050_WAIT_FOR_ES_BIT 6 +#define MPU6050_SLV_3_FIFO_EN_BIT 5 +#define MPU6050_I2C_MST_P_NSR_BIT 4 +#define MPU6050_I2C_MST_CLK_BIT 3 +#define MPU6050_I2C_MST_CLK_LENGTH 4 + +#define MPU6050_CLOCK_DIV_348 0x0 +#define MPU6050_CLOCK_DIV_333 0x1 +#define MPU6050_CLOCK_DIV_320 0x2 +#define MPU6050_CLOCK_DIV_308 0x3 +#define MPU6050_CLOCK_DIV_296 0x4 +#define MPU6050_CLOCK_DIV_286 0x5 +#define MPU6050_CLOCK_DIV_276 0x6 +#define MPU6050_CLOCK_DIV_267 0x7 +#define MPU6050_CLOCK_DIV_258 0x8 +#define MPU6050_CLOCK_DIV_500 0x9 +#define MPU6050_CLOCK_DIV_471 0xA +#define MPU6050_CLOCK_DIV_444 0xB +#define MPU6050_CLOCK_DIV_421 0xC +#define MPU6050_CLOCK_DIV_400 0xD +#define MPU6050_CLOCK_DIV_381 0xE +#define MPU6050_CLOCK_DIV_364 0xF + +#define MPU6050_I2C_SLV_RW_BIT 7 +#define MPU6050_I2C_SLV_ADDR_BIT 6 +#define MPU6050_I2C_SLV_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV_EN_BIT 7 +#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 +#define MPU6050_I2C_SLV_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV_GRP_BIT 4 +#define MPU6050_I2C_SLV_LEN_BIT 3 +#define MPU6050_I2C_SLV_LEN_LENGTH 4 + +#define MPU6050_I2C_SLV4_RW_BIT 7 +#define MPU6050_I2C_SLV4_ADDR_BIT 6 +#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV4_EN_BIT 7 +#define MPU6050_I2C_SLV4_INT_EN_BIT 6 +#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 +#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 + +#define MPU6050_MST_PASS_THROUGH_BIT 7 +#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 +#define MPU6050_MST_I2C_LOST_ARB_BIT 5 +#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 +#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 +#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 +#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 +#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 + +#define MPU6050_INTCFG_INT_LEVEL_BIT 7 +#define MPU6050_INTCFG_INT_OPEN_BIT 6 +#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 +#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 +#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 +#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 +#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 +#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 + +#define MPU6050_INTMODE_ACTIVEHIGH 0x00 +#define MPU6050_INTMODE_ACTIVELOW 0x01 + +#define MPU6050_INTDRV_PUSHPULL 0x00 +#define MPU6050_INTDRV_OPENDRAIN 0x01 + +#define MPU6050_INTLATCH_50USPULSE 0x00 +#define MPU6050_INTLATCH_WAITCLEAR 0x01 + +#define MPU6050_INTCLEAR_STATUSREAD 0x00 +#define MPU6050_INTCLEAR_ANYREAD 0x01 + +#define MPU6050_INTERRUPT_FF_BIT 7 +#define MPU6050_INTERRUPT_MOT_BIT 6 +#define MPU6050_INTERRUPT_ZMOT_BIT 5 +#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 +#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 +#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 +#define MPU6050_INTERRUPT_DMP_INT_BIT 1 +#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 + +// TODO: figure out what these actually do +// UMPL source code is not very obivous +#define MPU6050_DMPINT_5_BIT 5 +#define MPU6050_DMPINT_4_BIT 4 +#define MPU6050_DMPINT_3_BIT 3 +#define MPU6050_DMPINT_2_BIT 2 +#define MPU6050_DMPINT_1_BIT 1 +#define MPU6050_DMPINT_0_BIT 0 + +#define MPU6050_MOTION_MOT_XNEG_BIT 7 +#define MPU6050_MOTION_MOT_XPOS_BIT 6 +#define MPU6050_MOTION_MOT_YNEG_BIT 5 +#define MPU6050_MOTION_MOT_YPOS_BIT 4 +#define MPU6050_MOTION_MOT_ZNEG_BIT 3 +#define MPU6050_MOTION_MOT_ZPOS_BIT 2 +#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 + +#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 +#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 +#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 +#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 +#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 +#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 + +#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 +#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 +#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 + +#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 +#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 +#define MPU6050_DETECT_FF_COUNT_BIT 3 +#define MPU6050_DETECT_FF_COUNT_LENGTH 2 +#define MPU6050_DETECT_MOT_COUNT_BIT 1 +#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 + +#define MPU6050_DETECT_DECREMENT_RESET 0x0 +#define MPU6050_DETECT_DECREMENT_1 0x1 +#define MPU6050_DETECT_DECREMENT_2 0x2 +#define MPU6050_DETECT_DECREMENT_4 0x3 + +#define MPU6050_USERCTRL_DMP_EN_BIT 7 +#define MPU6050_USERCTRL_FIFO_EN_BIT 6 +#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 +#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 +#define MPU6050_USERCTRL_DMP_RESET_BIT 3 +#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 +#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 +#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 + +#define MPU6050_PWR1_DEVICE_RESET_BIT 7 +#define MPU6050_PWR1_SLEEP_BIT 6 +#define MPU6050_PWR1_CYCLE_BIT 5 +#define MPU6050_PWR1_TEMP_DIS_BIT 3 +#define MPU6050_PWR1_CLKSEL_BIT 2 +#define MPU6050_PWR1_CLKSEL_LENGTH 3 + +#define MPU6050_CLOCK_INTERNAL 0x00 +#define MPU6050_CLOCK_PLL_XGYRO 0x01 +#define MPU6050_CLOCK_PLL_YGYRO 0x02 +#define MPU6050_CLOCK_PLL_ZGYRO 0x03 +#define MPU6050_CLOCK_PLL_EXT32K 0x04 +#define MPU6050_CLOCK_PLL_EXT19M 0x05 +#define MPU6050_CLOCK_KEEP_RESET 0x07 + +#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 +#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 +#define MPU6050_PWR2_STBY_XA_BIT 5 +#define MPU6050_PWR2_STBY_YA_BIT 4 +#define MPU6050_PWR2_STBY_ZA_BIT 3 +#define MPU6050_PWR2_STBY_XG_BIT 2 +#define MPU6050_PWR2_STBY_YG_BIT 1 +#define MPU6050_PWR2_STBY_ZG_BIT 0 + +#define MPU6050_WAKE_FREQ_1P25 0x0 +#define MPU6050_WAKE_FREQ_2P5 0x1 +#define MPU6050_WAKE_FREQ_5 0x2 +#define MPU6050_WAKE_FREQ_10 0x3 + +#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 +#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 +#define MPU6050_BANKSEL_MEM_SEL_BIT 4 +#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 + +#define MPU6050_WHO_AM_I_BIT 6 +#define MPU6050_WHO_AM_I_LENGTH 6 + +#define MPU6050_DMP_MEMORY_BANKS 8 +#define MPU6050_DMP_MEMORY_BANK_SIZE 256 +#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 + +// note: DMP code memory blocks defined at end of header file + +class MPU6050 { + public: + MPU6050(); + MPU6050(uint8_t address); + + void initialize(); + bool testConnection(); + + // AUX_VDDIO register + uint8_t getAuxVDDIOLevel(); + void setAuxVDDIOLevel(uint8_t level); + + // SMPLRT_DIV register + uint8_t getRate(); + void setRate(uint8_t rate); + + // CONFIG register + uint8_t getExternalFrameSync(); + void setExternalFrameSync(uint8_t sync); + uint8_t getDLPFMode(); + void setDLPFMode(uint8_t bandwidth); + + // GYRO_CONFIG register + uint8_t getFullScaleGyroRange(); + void setFullScaleGyroRange(uint8_t range); + + // SELF_TEST registers + uint8_t getAccelXSelfTestFactoryTrim(); + uint8_t getAccelYSelfTestFactoryTrim(); + uint8_t getAccelZSelfTestFactoryTrim(); + + uint8_t getGyroXSelfTestFactoryTrim(); + uint8_t getGyroYSelfTestFactoryTrim(); + uint8_t getGyroZSelfTestFactoryTrim(); + + // ACCEL_CONFIG register + bool getAccelXSelfTest(); + void setAccelXSelfTest(bool enabled); + bool getAccelYSelfTest(); + void setAccelYSelfTest(bool enabled); + bool getAccelZSelfTest(); + void setAccelZSelfTest(bool enabled); + uint8_t getFullScaleAccelRange(); + void setFullScaleAccelRange(uint8_t range); + uint8_t getDHPFMode(); + void setDHPFMode(uint8_t mode); + + // FF_THR register + uint8_t getFreefallDetectionThreshold(); + void setFreefallDetectionThreshold(uint8_t threshold); + + // FF_DUR register + uint8_t getFreefallDetectionDuration(); + void setFreefallDetectionDuration(uint8_t duration); + + // MOT_THR register + uint8_t getMotionDetectionThreshold(); + void setMotionDetectionThreshold(uint8_t threshold); + + // MOT_DUR register + uint8_t getMotionDetectionDuration(); + void setMotionDetectionDuration(uint8_t duration); + + // ZRMOT_THR register + uint8_t getZeroMotionDetectionThreshold(); + void setZeroMotionDetectionThreshold(uint8_t threshold); + + // ZRMOT_DUR register + uint8_t getZeroMotionDetectionDuration(); + void setZeroMotionDetectionDuration(uint8_t duration); + + // FIFO_EN register + bool getTempFIFOEnabled(); + void setTempFIFOEnabled(bool enabled); + bool getXGyroFIFOEnabled(); + void setXGyroFIFOEnabled(bool enabled); + bool getYGyroFIFOEnabled(); + void setYGyroFIFOEnabled(bool enabled); + bool getZGyroFIFOEnabled(); + void setZGyroFIFOEnabled(bool enabled); + bool getAccelFIFOEnabled(); + void setAccelFIFOEnabled(bool enabled); + bool getSlave2FIFOEnabled(); + void setSlave2FIFOEnabled(bool enabled); + bool getSlave1FIFOEnabled(); + void setSlave1FIFOEnabled(bool enabled); + bool getSlave0FIFOEnabled(); + void setSlave0FIFOEnabled(bool enabled); + + // I2C_MST_CTRL register + bool getMultiMasterEnabled(); + void setMultiMasterEnabled(bool enabled); + bool getWaitForExternalSensorEnabled(); + void setWaitForExternalSensorEnabled(bool enabled); + bool getSlave3FIFOEnabled(); + void setSlave3FIFOEnabled(bool enabled); + bool getSlaveReadWriteTransitionEnabled(); + void setSlaveReadWriteTransitionEnabled(bool enabled); + uint8_t getMasterClockSpeed(); + void setMasterClockSpeed(uint8_t speed); + + // I2C_SLV* registers (Slave 0-3) + uint8_t getSlaveAddress(uint8_t num); + void setSlaveAddress(uint8_t num, uint8_t address); + uint8_t getSlaveRegister(uint8_t num); + void setSlaveRegister(uint8_t num, uint8_t reg); + bool getSlaveEnabled(uint8_t num); + void setSlaveEnabled(uint8_t num, bool enabled); + bool getSlaveWordByteSwap(uint8_t num); + void setSlaveWordByteSwap(uint8_t num, bool enabled); + bool getSlaveWriteMode(uint8_t num); + void setSlaveWriteMode(uint8_t num, bool mode); + bool getSlaveWordGroupOffset(uint8_t num); + void setSlaveWordGroupOffset(uint8_t num, bool enabled); + uint8_t getSlaveDataLength(uint8_t num); + void setSlaveDataLength(uint8_t num, uint8_t length); + + // I2C_SLV* registers (Slave 4) + uint8_t getSlave4Address(); + void setSlave4Address(uint8_t address); + uint8_t getSlave4Register(); + void setSlave4Register(uint8_t reg); + void setSlave4OutputByte(uint8_t data); + bool getSlave4Enabled(); + void setSlave4Enabled(bool enabled); + bool getSlave4InterruptEnabled(); + void setSlave4InterruptEnabled(bool enabled); + bool getSlave4WriteMode(); + void setSlave4WriteMode(bool mode); + uint8_t getSlave4MasterDelay(); + void setSlave4MasterDelay(uint8_t delay); + uint8_t getSlate4InputByte(); + + // I2C_MST_STATUS register + bool getPassthroughStatus(); + bool getSlave4IsDone(); + bool getLostArbitration(); + bool getSlave4Nack(); + bool getSlave3Nack(); + bool getSlave2Nack(); + bool getSlave1Nack(); + bool getSlave0Nack(); + + // INT_PIN_CFG register + bool getInterruptMode(); + void setInterruptMode(bool mode); + bool getInterruptDrive(); + void setInterruptDrive(bool drive); + bool getInterruptLatch(); + void setInterruptLatch(bool latch); + bool getInterruptLatchClear(); + void setInterruptLatchClear(bool clear); + bool getFSyncInterruptLevel(); + void setFSyncInterruptLevel(bool level); + bool getFSyncInterruptEnabled(); + void setFSyncInterruptEnabled(bool enabled); + bool getI2CBypassEnabled(); + void setI2CBypassEnabled(bool enabled); + bool getClockOutputEnabled(); + void setClockOutputEnabled(bool enabled); + + // INT_ENABLE register + uint8_t getIntEnabled(); + void setIntEnabled(uint8_t enabled); + bool getIntFreefallEnabled(); + void setIntFreefallEnabled(bool enabled); + bool getIntMotionEnabled(); + void setIntMotionEnabled(bool enabled); + bool getIntZeroMotionEnabled(); + void setIntZeroMotionEnabled(bool enabled); + bool getIntFIFOBufferOverflowEnabled(); + void setIntFIFOBufferOverflowEnabled(bool enabled); + bool getIntI2CMasterEnabled(); + void setIntI2CMasterEnabled(bool enabled); + bool getIntDataReadyEnabled(); + void setIntDataReadyEnabled(bool enabled); + + // INT_STATUS register + uint8_t getIntStatus(); + bool getIntFreefallStatus(); + bool getIntMotionStatus(); + bool getIntZeroMotionStatus(); + bool getIntFIFOBufferOverflowStatus(); + bool getIntI2CMasterStatus(); + bool getIntDataReadyStatus(); + + // ACCEL_*OUT_* registers + 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); + void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); + void getAcceleration(int16_t* x, int16_t* y, int16_t* z); + int16_t getAccelerationX(); + int16_t getAccelerationY(); + int16_t getAccelerationZ(); + + // TEMP_OUT_* registers + int16_t getTemperature(); + + // GYRO_*OUT_* registers + void getRotation(int16_t* x, int16_t* y, int16_t* z); + int16_t getRotationX(); + int16_t getRotationY(); + int16_t getRotationZ(); + + // EXT_SENS_DATA_* registers + uint8_t getExternalSensorByte(int position); + uint16_t getExternalSensorWord(int position); + uint32_t getExternalSensorDWord(int position); + + // MOT_DETECT_STATUS register + uint8_t getMotionStatus(); + bool getXNegMotionDetected(); + bool getXPosMotionDetected(); + bool getYNegMotionDetected(); + bool getYPosMotionDetected(); + bool getZNegMotionDetected(); + bool getZPosMotionDetected(); + bool getZeroMotionDetected(); + + // I2C_SLV*_DO register + void setSlaveOutputByte(uint8_t num, uint8_t data); + + // I2C_MST_DELAY_CTRL register + bool getExternalShadowDelayEnabled(); + void setExternalShadowDelayEnabled(bool enabled); + bool getSlaveDelayEnabled(uint8_t num); + void setSlaveDelayEnabled(uint8_t num, bool enabled); + + // SIGNAL_PATH_RESET register + void resetGyroscopePath(); + void resetAccelerometerPath(); + void resetTemperaturePath(); + + // MOT_DETECT_CTRL register + uint8_t getAccelerometerPowerOnDelay(); + void setAccelerometerPowerOnDelay(uint8_t delay); + uint8_t getFreefallDetectionCounterDecrement(); + void setFreefallDetectionCounterDecrement(uint8_t decrement); + uint8_t getMotionDetectionCounterDecrement(); + void setMotionDetectionCounterDecrement(uint8_t decrement); + + // USER_CTRL register + bool getFIFOEnabled(); + void setFIFOEnabled(bool enabled); + bool getI2CMasterModeEnabled(); + void setI2CMasterModeEnabled(bool enabled); + void switchSPIEnabled(bool enabled); + void resetFIFO(); + void resetI2CMaster(); + void resetSensors(); + + // PWR_MGMT_1 register + void reset(); + bool getSleepEnabled(); + void setSleepEnabled(bool enabled); + bool getWakeCycleEnabled(); + void setWakeCycleEnabled(bool enabled); + bool getTempSensorEnabled(); + void setTempSensorEnabled(bool enabled); + uint8_t getClockSource(); + void setClockSource(uint8_t source); + + // PWR_MGMT_2 register + uint8_t getWakeFrequency(); + void setWakeFrequency(uint8_t frequency); + bool getStandbyXAccelEnabled(); + void setStandbyXAccelEnabled(bool enabled); + bool getStandbyYAccelEnabled(); + void setStandbyYAccelEnabled(bool enabled); + bool getStandbyZAccelEnabled(); + void setStandbyZAccelEnabled(bool enabled); + bool getStandbyXGyroEnabled(); + void setStandbyXGyroEnabled(bool enabled); + bool getStandbyYGyroEnabled(); + void setStandbyYGyroEnabled(bool enabled); + bool getStandbyZGyroEnabled(); + void setStandbyZGyroEnabled(bool enabled); + + // FIFO_COUNT_* registers + uint16_t getFIFOCount(); + + // FIFO_R_W register + uint8_t getFIFOByte(); + void setFIFOByte(uint8_t data); + void getFIFOBytes(uint8_t *data, uint8_t length); + + // WHO_AM_I register + uint8_t getDeviceID(); + void setDeviceID(uint8_t id); + + // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== + + // XG_OFFS_TC register + uint8_t getOTPBankValid(); + void setOTPBankValid(bool enabled); + int8_t getXGyroOffsetTC(); + void setXGyroOffsetTC(int8_t offset); + + // YG_OFFS_TC register + int8_t getYGyroOffsetTC(); + void setYGyroOffsetTC(int8_t offset); + + // ZG_OFFS_TC register + int8_t getZGyroOffsetTC(); + void setZGyroOffsetTC(int8_t offset); + + // X_FINE_GAIN register + int8_t getXFineGain(); + void setXFineGain(int8_t gain); + + // Y_FINE_GAIN register + int8_t getYFineGain(); + void setYFineGain(int8_t gain); + + // Z_FINE_GAIN register + int8_t getZFineGain(); + void setZFineGain(int8_t gain); + + // XA_OFFS_* registers + int16_t getXAccelOffset(); + void setXAccelOffset(int16_t offset); + + // YA_OFFS_* register + int16_t getYAccelOffset(); + void setYAccelOffset(int16_t offset); + + // ZA_OFFS_* register + int16_t getZAccelOffset(); + void setZAccelOffset(int16_t offset); + + // XG_OFFS_USR* registers + int16_t getXGyroOffset(); + void setXGyroOffset(int16_t offset); + + // YG_OFFS_USR* register + int16_t getYGyroOffset(); + void setYGyroOffset(int16_t offset); + + // ZG_OFFS_USR* register + int16_t getZGyroOffset(); + void setZGyroOffset(int16_t offset); + + // INT_ENABLE register (DMP functions) + bool getIntPLLReadyEnabled(); + void setIntPLLReadyEnabled(bool enabled); + bool getIntDMPEnabled(); + void setIntDMPEnabled(bool enabled); + + // DMP_INT_STATUS + bool getDMPInt5Status(); + bool getDMPInt4Status(); + bool getDMPInt3Status(); + bool getDMPInt2Status(); + bool getDMPInt1Status(); + bool getDMPInt0Status(); + + // INT_STATUS register (DMP functions) + bool getIntPLLReadyStatus(); + bool getIntDMPStatus(); + + // USER_CTRL register (DMP functions) + bool getDMPEnabled(); + void setDMPEnabled(bool enabled); + void resetDMP(); + + // BANK_SEL register + void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); + + // MEM_START_ADDR register + void setMemoryStartAddress(uint8_t address); + + // MEM_R_W register + uint8_t readMemoryByte(); + void writeMemoryByte(uint8_t data); + void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); + bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); + bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); + + bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); + bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); + + // DMP_CFG_1 register + uint8_t getDMPConfig1(); + void setDMPConfig1(uint8_t config); + + // DMP_CFG_2 register + uint8_t getDMPConfig2(); + void setDMPConfig2(uint8_t config); + + // special methods for MotionApps 2.0 implementation + #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; + + uint8_t dmpInitialize(); + uint8_t dmpInitialize_light(); // reduced init sequence for LSOS + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + #endif + + // special methods for MotionApps 4.1 implementation + #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; + + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + #endif + + private: + uint8_t devAddr; + uint8_t buffer[14]; +}; + +#endif /* _MPU6050_H_ */ diff --git a/Libraries/MPU6050/MPU6050_6Axis_MotionApps20.h b/Libraries/MPU6050/MPU6050_6Axis_MotionApps20.h index 1c05543..5af4492 100644 --- a/Libraries/MPU6050/MPU6050_6Axis_MotionApps20.h +++ b/Libraries/MPU6050/MPU6050_6Axis_MotionApps20.h @@ -1,745 +1,980 @@ -// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 5/20/2013 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// ... - ongoing debug release - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ -#define _MPU6050_6AXIS_MOTIONAPPS20_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" - -// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 - -#include "MPU6050.h" - -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifdef __AVR__ - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include - - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x - - typedef void prog_void; - typedef char prog_char; - typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; - - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) - - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) - - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -/* Source is from the InvenSense MotionApps v2 demo code. Original source is - * unavailable, unless you happen to be amazing as decompiling binary by - * hand (in which case, please contact me, and I'm totally serious). - * - * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the - * DMP reverse-engineering he did to help make this bit of wizardry - * possible. - */ - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] -#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] -#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] - -/* ================================================================================================ * - | Default MotionApps v2.0 42-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | - | | - | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | - | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | - * ================================================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) -const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { - // bank 0, 256 bytes - 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, - 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, - 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, - - // bank 1, 256 bytes - 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, - 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, - 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, - - // bank 2, 256 bytes - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - // bank 3, 256 bytes - 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, - 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, - 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, - 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, - 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, - 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, - 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, - 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, - 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, - 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, - 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, - 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, - 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, - 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, - 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, - 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, - - // bank 4, 256 bytes - 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, - 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, - 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, - 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, - 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, - 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, - 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, - 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, - 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, - 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, - 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, - 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, - - // bank 5, 256 bytes - 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, - 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, - 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, - 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, - 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, - 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, - 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, - 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, - 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, - 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, - 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, - 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, - 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, - 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, - 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, - 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, - - // bank 6, 256 bytes - 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, - 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, - 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, - 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, - 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, - 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, - 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, - 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, - 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, - 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, - 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, - 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, - 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, - 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, - 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, - 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, - - // bank 7, 138 bytes (remainder) - 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, - 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, - 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, - 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, - 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, - 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, - 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, - 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, - 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF -}; - -// thanks to Noah Zerkin for piecing this stuff together! -const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { -// BANK OFFSET LENGTH [DATA] - 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration - 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration - 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration - 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration - 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration - 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration - 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration - 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration - 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration - 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 - 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 - 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 - 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 - 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 - 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 - 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 - 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 - 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel - 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors - 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion - 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update - 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone - // SPECIAL 0x01 = enable interrupts - 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION - 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt - 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion - 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer - 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro - 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo - 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo - 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate - - // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, - // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. - // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) - - // It is important to make sure the host processor can keep up with reading and processing - // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. -}; - -const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { - 0x01, 0xB2, 0x02, 0xFF, 0xFF, - 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, - 0x01, 0x6A, 0x02, 0x06, 0x00, - 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x01, 0x62, 0x02, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 -}; - -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - reset(); - delay(30); // wait after reset - - // enable sleep mode and wake cycle - /*Serial.println(F("Enabling sleep mode...")); - setSleepEnabled(true); - Serial.println(F("Enabling wake cycle...")); - setWakeCycleEnabled(true);*/ - - // disable sleep mode - DEBUG_PRINTLN(F("Disabling sleep mode...")); - setSleepEnabled(false); - - // get MPU hardware revision - DEBUG_PRINTLN(F("Selecting user bank 16...")); - setMemoryBank(0x10, true, true); - DEBUG_PRINTLN(F("Selecting memory byte 6...")); - setMemoryStartAddress(0x06); - DEBUG_PRINTLN(F("Checking hardware revision...")); - DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLNF(readMemoryByte(), HEX); - DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); - setMemoryBank(0, false, false); - - // check OTP bank valid - DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); - DEBUG_PRINT(F("OTP bank is ")); - DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); - - // get X/Y/Z gyro offsets - DEBUG_PRINTLN(F("Reading gyro offset TC values...")); - int8_t xgOffsetTC = getXGyroOffsetTC(); - int8_t ygOffsetTC = getYGyroOffsetTC(); - int8_t zgOffsetTC = getZGyroOffsetTC(); - DEBUG_PRINT(F("X gyro offset = ")); - DEBUG_PRINTLN(xgOffsetTC); - DEBUG_PRINT(F("Y gyro offset = ")); - DEBUG_PRINTLN(ygOffsetTC); - DEBUG_PRINT(F("Z gyro offset = ")); - DEBUG_PRINTLN(zgOffsetTC); - - // setup weird slave stuff (?) - DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); - setSlaveAddress(0, 0x7F); - DEBUG_PRINTLN(F("Disabling I2C Master mode...")); - setI2CMasterModeEnabled(false); - DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); - setSlaveAddress(0, 0x68); - DEBUG_PRINTLN(F("Resetting I2C Master control...")); - resetI2CMaster(); - delay(20); - - // load DMP code into memory banks - DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP code written and verified.")); - - // write DMP configuration - DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); - DEBUG_PRINTLN(F(" bytes in config def)")); - if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); - - DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); - setClockSource(MPU6050_CLOCK_PLL_ZGYRO); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - setIntEnabled(0x12); - - DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); - setRate(4); // 1khz / (1 + 4) = 200 Hz - - DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); - setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); - - DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); - setDLPFMode(MPU6050_DLPF_BW_42); - - DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); - setFullScaleGyroRange(MPU6050_GYRO_FS_2000); - - DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); - setDMPConfig1(0x03); - setDMPConfig2(0x00); - - DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); - setOTPBankValid(false); - - DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); - setXGyroOffsetTC(xgOffsetTC); - setYGyroOffsetTC(ygOffsetTC); - setZGyroOffsetTC(zgOffsetTC); - - //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); - //setXGyroOffset(0); - //setYGyroOffset(0); - //setZGyroOffset(0); - - DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); - uint8_t dmpUpdate[16], j; - uint16_t pos = 0; - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Resetting FIFO...")); - resetFIFO(); - - DEBUG_PRINTLN(F("Reading FIFO count...")); - uint16_t fifoCount = getFIFOCount(); - uint8_t fifoBuffer[128]; - - DEBUG_PRINT(F("Current FIFO count=")); - DEBUG_PRINTLN(fifoCount); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); - setMotionDetectionThreshold(2); - - DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); - setZeroMotionDetectionThreshold(156); - - DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); - setMotionDetectionDuration(80); - - DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); - setZeroMotionDetectionDuration(0); - - DEBUG_PRINTLN(F("Resetting FIFO...")); - resetFIFO(); - - DEBUG_PRINTLN(F("Enabling FIFO...")); - setFIFOEnabled(true); - - DEBUG_PRINTLN(F("Enabling DMP...")); - setDMPEnabled(true); - - DEBUG_PRINTLN(F("Resetting DMP...")); - resetDMP(); - - DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); - while ((fifoCount = getFIFOCount()) < 3); - - DEBUG_PRINT(F("Current FIFO count=")); - DEBUG_PRINTLN(fifoCount); - DEBUG_PRINTLN(F("Reading FIFO data...")); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Reading interrupt status...")); - - DEBUG_PRINT(F("Current interrupt status=")); - DEBUG_PRINTLNF(getIntStatus(), HEX); - - DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); - while ((fifoCount = getFIFOCount()) < 3); - - DEBUG_PRINT(F("Current FIFO count=")); - DEBUG_PRINTLN(fifoCount); - - DEBUG_PRINTLN(F("Reading FIFO data...")); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Reading interrupt status...")); - - DEBUG_PRINT(F("Current interrupt status=")); - DEBUG_PRINTLNF(getIntStatus(), HEX); - - DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("DMP is good to go! Finally.")); - - DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); - setDMPEnabled(false); - - DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); - dmpPacketSize = 42; - /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { - return 3; // TODO: proper error code for no memory - }*/ - - DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); - resetFIFO(); - getIntStatus(); - } else { - DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); - return 2; // configuration block loading failed - } - } else { - DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); - return 1; // main binary block loading failed - } - return 0; // success -} - -bool MPU6050::dmpPacketAvailable() { - return getFIFOCount() >= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); - data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); - data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) | packet[29]; - data[1] = (packet[32] << 8) | packet[33]; - data[2] = (packet[36] << 8) | packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[28] << 8) | packet[29]; - v -> y = (packet[32] << 8) | packet[33]; - v -> z = (packet[36] << 8) | packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); - data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); - data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[20] << 8) | packet[21]; - data[2] = (packet[24] << 8) | packet[25]; - return 0; -} -uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[16] << 8) | packet[17]; - v -> y = (packet[20] << 8) | packet[21]; - v -> z = (packet[24] << 8) | packet[25]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) - v -> x = vRaw -> x - gravity -> x*8192; - v -> y = vRaw -> y - gravity -> y*8192; - v -> z = vRaw -> z - gravity -> z*8192; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - 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 - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - 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 - return 0; -} -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) (*processed)++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} - -#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ +#define _MPU6050_6AXIS_MOTIONAPPS20_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 + +#include "MPU6050.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v2.0 42-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + // bank 0, 256 bytes + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + + // bank 1, 256 bytes + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + + // bank 2, 256 bytes + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + // bank 3, 256 bytes + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + + // bank 4, 256 bytes + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + + // bank 5, 256 bytes + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + + // bank 6, 256 bytes + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + + // bank 7, 138 bytes (remainder) + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF +}; + +// thanks to Noah Zerkin for piecing this stuff together! +const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { +// BANK OFFSET LENGTH [DATA] + 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration + 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration + 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration + 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration + 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration + 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration + 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration + 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration + 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration + 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 + 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 + 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 + 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 + 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 + 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 + 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 + 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 + 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel + 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors + 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion + 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update + 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone + // SPECIAL 0x01 = enable interrupts + 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION + 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt + 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion + 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer + 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro + 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo + 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo + 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate + + // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, + // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. + // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) + + // It is important to make sure the host processor can keep up with reading and processing + // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. +}; + +const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { + 0x01, 0xB2, 0x02, 0xFF, 0xFF, + 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, + 0x01, 0x6A, 0x02, 0x06, 0x00, + 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x01, 0x62, 0x02, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 +}; + +uint8_t MPU6050::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + DEBUG_PRINTLN(F("Disabling sleep mode...")); + setSleepEnabled(false); + + // get MPU hardware revision + DEBUG_PRINTLN(F("Selecting user bank 16...")); + setMemoryBank(0x10, true, true); + DEBUG_PRINTLN(F("Selecting memory byte 6...")); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLNF(readMemoryByte(), HEX); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); + + // get X/Y/Z gyro offsets + DEBUG_PRINTLN(F("Reading gyro offset TC values...")); + int8_t xgOffsetTC = getXGyroOffsetTC(); + int8_t ygOffsetTC = getYGyroOffsetTC(); + int8_t zgOffsetTC = getZGyroOffsetTC(); + DEBUG_PRINT(F("X gyro offset = ")); + DEBUG_PRINTLN(xgOffsetTC); + DEBUG_PRINT(F("Y gyro offset = ")); + DEBUG_PRINTLN(ygOffsetTC); + DEBUG_PRINT(F("Z gyro offset = ")); + DEBUG_PRINTLN(zgOffsetTC); + + // setup weird slave stuff (?) + DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); + setSlaveAddress(0, 0x7F); + DEBUG_PRINTLN(F("Disabling I2C Master mode...")); + setI2CMasterModeEnabled(false); + DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); + setSlaveAddress(0, 0x68); + DEBUG_PRINTLN(F("Resetting I2C Master control...")); + resetI2CMaster(); + delay(20); + + // load DMP code into memory banks + DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); + DEBUG_PRINTLN(F(" bytes)")); + if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP code written and verified.")); + + // write DMP configuration + DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); + DEBUG_PRINTLN(F(" bytes in config def)")); + if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); + + DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + setClockSource(MPU6050_CLOCK_PLL_ZGYRO); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(0x12); + + DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); + setRate(4); // 1khz / (1 + 4) = 200 Hz + + DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); + setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); + + DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); + setDLPFMode(MPU6050_DLPF_BW_42); + + DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); + setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + + DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); + setDMPConfig1(0x03); + setDMPConfig2(0x00); + + DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); + setOTPBankValid(false); + + DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); + setXGyroOffsetTC(xgOffsetTC); + setYGyroOffsetTC(ygOffsetTC); + setZGyroOffsetTC(zgOffsetTC); + + //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); + //setXGyroOffset(0); + //setYGyroOffset(0); + //setZGyroOffset(0); + + DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); + uint8_t dmpUpdate[16], j; + uint16_t pos = 0; + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Resetting FIFO...")); + resetFIFO(); + + DEBUG_PRINTLN(F("Reading FIFO count...")); + uint16_t fifoCount = getFIFOCount(); + uint8_t fifoBuffer[128]; + + DEBUG_PRINT(F("Current FIFO count=")); + DEBUG_PRINTLN(fifoCount); + getFIFOBytes(fifoBuffer, fifoCount); + + DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); + setMotionDetectionThreshold(2); + + DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); + setZeroMotionDetectionThreshold(156); + + DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); + setMotionDetectionDuration(80); + + DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); + setZeroMotionDetectionDuration(0); + + DEBUG_PRINTLN(F("Resetting FIFO...")); + resetFIFO(); + + DEBUG_PRINTLN(F("Enabling FIFO...")); + setFIFOEnabled(true); + + DEBUG_PRINTLN(F("Enabling DMP...")); + setDMPEnabled(true); + + DEBUG_PRINTLN(F("Resetting DMP...")); + resetDMP(); + + DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); + while ((fifoCount = getFIFOCount()) < 3); + + DEBUG_PRINT(F("Current FIFO count=")); + DEBUG_PRINTLN(fifoCount); + DEBUG_PRINTLN(F("Reading FIFO data...")); + getFIFOBytes(fifoBuffer, fifoCount); + + DEBUG_PRINTLN(F("Reading interrupt status...")); + + DEBUG_PRINT(F("Current interrupt status=")); + DEBUG_PRINTLNF(getIntStatus(), HEX); + + DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); + while ((fifoCount = getFIFOCount()) < 3); + + DEBUG_PRINT(F("Current FIFO count=")); + DEBUG_PRINTLN(fifoCount); + + DEBUG_PRINTLN(F("Reading FIFO data...")); + getFIFOBytes(fifoBuffer, fifoCount); + + DEBUG_PRINTLN(F("Reading interrupt status...")); + + DEBUG_PRINT(F("Current interrupt status=")); + DEBUG_PRINTLNF(getIntStatus(), HEX); + + DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("DMP is good to go! Finally.")); + + DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); + setDMPEnabled(false); + + DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); + dmpPacketSize = 42; + /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { + return 3; // TODO: proper error code for no memory + }*/ + + DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); + resetFIFO(); + getIntStatus(); + } else { + DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); + return 2; // configuration block loading failed + } + } else { + DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); + return 1; // main binary block loading failed + } + return 0; // success +} +// ****************************************************** + +uint8_t MPU6050::dmpInitialize_light() { +// do not remove!!! AK - start +// reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + //Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + //DEBUG_PRINTLN(F("Disabling sleep mode...")); + setSleepEnabled(false); +// do not remove!!! AK - stop + +// could be removed AK - start + // get MPU hardware revision + //DEBUG_PRINTLN(F("Selecting user bank 16...")); + //setMemoryBank(0x10, true, true); + //DEBUG_PRINTLN(F("Selecting memory byte 6...")); + //setMemoryStartAddress(0x06); + //DEBUG_PRINTLN(F("Checking hardware revision...")); + //DEBUG_PRINT(F("Revision @ user[16][6] = ")); + //DEBUG_PRINTLNF(readMemoryByte(), HEX); + //DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + //setMemoryBank(0, false, false); + + // check OTP bank valid + //DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + //DEBUG_PRINT(F("OTP bank is ")); + //DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); + + // get X/Y/Z gyro offsets + //DEBUG_PRINTLN(F("Reading gyro offset TC values...")); + //int8_t xgOffsetTC = getXGyroOffsetTC(); + //int8_t ygOffsetTC = getYGyroOffsetTC(); + //int8_t zgOffsetTC = getZGyroOffsetTC(); + //DEBUG_PRINT(F("X gyro offset = ")); + //DEBUG_PRINTLN(xgOffsetTC); + //DEBUG_PRINT(F("Y gyro offset = ")); + //DEBUG_PRINTLN(ygOffsetTC); + //DEBUG_PRINT(F("Z gyro offset = ")); + //DEBUG_PRINTLN(zgOffsetTC); + + // setup weird slave stuff (?) + //DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); + //setSlaveAddress(0, 0x7F); + //DEBUG_PRINTLN(F("Disabling I2C Master mode...")); + //setI2CMasterModeEnabled(false); + //DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); + //setSlaveAddress(0, 0x68); + //DEBUG_PRINTLN(F("Resetting I2C Master control...")); + //resetI2CMaster(); + //delay(20); +// could be removed AK - stop + + // load DMP code into memory banks + DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); + DEBUG_PRINTLN(F(" bytes)")); + if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP code written and verified.")); + + // write DMP configuration + DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); + DEBUG_PRINTLN(F(" bytes in config def)")); + if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); + + DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + setClockSource(MPU6050_CLOCK_PLL_ZGYRO); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(0x12); + + DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); + setRate(4); // 1khz / (1 + 4) = 200 Hz + + DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); + setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); + + DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); + setDLPFMode(MPU6050_DLPF_BW_42); + + DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); + setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + + DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); + setDMPConfig1(0x03); + setDMPConfig2(0x00); + + DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); + setOTPBankValid(false); +// could be removed AK - start + //DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); + //setXGyroOffsetTC(xgOffsetTC); + //setYGyroOffsetTC(ygOffsetTC); + //setZGyroOffsetTC(zgOffsetTC); + + //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); + //setXGyroOffset(0); + //setYGyroOffset(0); + //setZGyroOffset(0); +// could be removed AK - stop + +// do not remove!!! AK - start + DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); + uint8_t dmpUpdate[16], j; + uint16_t pos = 0; + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); +// do not remove!!! AK - stop +// could be removed AK - start + + //DEBUG_PRINTLN(F("Resetting FIFO...")); + //resetFIFO(); + + //DEBUG_PRINTLN(F("Reading FIFO count...")); + //uint16_t fifoCount = getFIFOCount(); + //uint8_t fifoBuffer[32]; // code works with this reduced size as well...with 64 and also with 32 + //uint8_t fifoBuffer[128]; // original size + + //DEBUG_PRINT(F("Current FIFO count=")); + //DEBUG_PRINTLN(fifoCount); + //getFIFOBytes(fifoBuffer, fifoCount); + //DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); + //setMotionDetectionThreshold(2); + + //DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); + //setZeroMotionDetectionThreshold(156); + + //DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); + //setMotionDetectionDuration(80); + + //DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); + //setZeroMotionDetectionDuration(0); +// could be removed AK - stop + + //DEBUG_PRINTLN(F("Resetting FIFO...")); + //resetFIFO(); +// do not remove!!! AK - start + DEBUG_PRINTLN(F("Enabling FIFO...")); + setFIFOEnabled(true); + + DEBUG_PRINTLN(F("Enabling DMP...")); + setDMPEnabled(true); + + DEBUG_PRINTLN(F("Resetting DMP...")); + resetDMP(); +// do not remove!!! AK - stop + +// could be removed AK - start + //DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); + //for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + //writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); + //for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + //writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); + //for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + //writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); + //while ((fifoCount = getFIFOCount()) < 3); + + //DEBUG_PRINT(F("Current FIFO count=")); + //DEBUG_PRINTLN(fifoCount); + //DEBUG_PRINTLN(F("Reading FIFO data...")); + //getFIFOBytes(fifoBuffer, fifoCount); + + //DEBUG_PRINTLN(F("Reading interrupt status...")); + + //DEBUG_PRINT(F("Current interrupt status=")); + //DEBUG_PRINTLNF(getIntStatus(), HEX); + + //DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); + //for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + //readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); + //while ((fifoCount = getFIFOCount()) < 3); + + //DEBUG_PRINT(F("Current FIFO count=")); + //DEBUG_PRINTLN(fifoCount); + + //DEBUG_PRINTLN(F("Reading FIFO data...")); + //getFIFOBytes(fifoBuffer, fifoCount); + + //DEBUG_PRINTLN(F("Reading interrupt status...")); + + //DEBUG_PRINT(F("Current interrupt status=")); + //DEBUG_PRINTLNF(getIntStatus(), HEX); +// could be removed AK - stop + + DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("DMP is good to go! Finally.")); + + DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); + setDMPEnabled(false); + + DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); + dmpPacketSize = 42; + /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { + return 3; // TODO: proper error code for no memory + }*/ + + DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); + resetFIFO(); + getIntStatus(); + } else { + DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); + return 2; // configuration block loading failed + } + } else { + DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); + return 1; // main binary block loading failed + } + return 0; // success +} +// ****************************************************** + +bool MPU6050::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); + data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); + data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[32] << 8) | packet[33]; + data[2] = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[28] << 8) | packet[29]; + v -> y = (packet[32] << 8) | packet[33]; + v -> z = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[20] << 8) | packet[21]; + v -> z = (packet[24] << 8) | packet[25]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + 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 + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + 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 + return 0; +} +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Libraries/Wire/Wire.cpp b/Libraries/Wire/Wire.cpp new file mode 100644 index 0000000..4e7a17c --- /dev/null +++ b/Libraries/Wire/Wire.cpp @@ -0,0 +1,298 @@ +/* + TwoWire.cpp - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts +*/ + +extern "C" { + #include + #include + #include + #include "twi.h" +} + +#include "Wire.h" + +// Initialize Class Variables ////////////////////////////////////////////////// + +uint8_t TwoWire::rxBuffer[BUFFER_LENGTH]; +uint8_t TwoWire::rxBufferIndex = 0; +uint8_t TwoWire::rxBufferLength = 0; + +uint8_t TwoWire::txAddress = 0; +uint8_t TwoWire::txBuffer[BUFFER_LENGTH]; +uint8_t TwoWire::txBufferIndex = 0; +uint8_t TwoWire::txBufferLength = 0; + +uint8_t TwoWire::transmitting = 0; +void (*TwoWire::user_onRequest)(void); +void (*TwoWire::user_onReceive)(int); + +// Constructors //////////////////////////////////////////////////////////////// + +TwoWire::TwoWire() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// + +void TwoWire::begin(void) +{ + rxBufferIndex = 0; + rxBufferLength = 0; + + txBufferIndex = 0; + txBufferLength = 0; + + twi_init(); +} + +void TwoWire::begin(uint8_t address) +{ + twi_setAddress(address); + twi_attachSlaveTxEvent(onRequestService); + twi_attachSlaveRxEvent(onReceiveService); + begin(); +} + +void TwoWire::begin(int address) +{ + begin((uint8_t)address); +} + +uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop) +{ + // clamp to buffer length + if(quantity > BUFFER_LENGTH){ + quantity = BUFFER_LENGTH; + } + // perform blocking read into buffer + uint8_t read = twi_readFrom(address, rxBuffer, quantity, sendStop); + // set rx buffer iterator vars + rxBufferIndex = 0; + rxBufferLength = read; + + return read; +} + +uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity) +{ + return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)true); +} + +uint8_t TwoWire::requestFrom(int address, int quantity) +{ + return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)true); +} + +uint8_t TwoWire::requestFrom(int address, int quantity, int sendStop) +{ + return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)sendStop); +} + +void TwoWire::beginTransmission(uint8_t address) +{ + // indicate that we are transmitting + transmitting = 1; + // set address of targeted slave + txAddress = address; + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; +} + +void TwoWire::beginTransmission(int address) +{ + beginTransmission((uint8_t)address); +} + +// +// Originally, 'endTransmission' was an f(void) function. +// It has been modified to take one parameter indicating +// whether or not a STOP should be performed on the bus. +// Calling endTransmission(false) allows a sketch to +// perform a repeated start. +// +// WARNING: Nothing in the library keeps track of whether +// the bus tenure has been properly ended with a STOP. It +// is very possible to leave the bus in a hung state if +// no call to endTransmission(true) is made. Some I2C +// devices will behave oddly if they do not see a STOP. +// +uint8_t TwoWire::endTransmission(uint8_t sendStop) +{ + // transmit buffer (blocking) + int8_t ret = twi_writeTo(txAddress, txBuffer, txBufferLength, 1, sendStop); + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + // indicate that we are done transmitting + transmitting = 0; + return ret; +} + +// This provides backwards compatibility with the original +// definition, and expected behaviour, of endTransmission +// +uint8_t TwoWire::endTransmission(void) +{ + return endTransmission(true); +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +size_t TwoWire::write(uint8_t data) +{ + if(transmitting){ + // in master transmitter mode + // don't bother if buffer is full + if(txBufferLength >= BUFFER_LENGTH){ + setWriteError(); + return 0; + } + // put byte in tx buffer + txBuffer[txBufferIndex] = data; + ++txBufferIndex; + // update amount in buffer + txBufferLength = txBufferIndex; + }else{ + // in slave send mode + // reply to master + twi_transmit(&data, 1); + } + return 1; +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +size_t TwoWire::write(const uint8_t *data, size_t quantity) +{ + if(transmitting){ + // in master transmitter mode + for(size_t i = 0; i < quantity; ++i){ + write(data[i]); + } + }else{ + // in slave send mode + // reply to master + twi_transmit(data, quantity); + } + return quantity; +} + +// must be called in: +// slave rx event callback +// or after requestFrom(address, numBytes) +int TwoWire::available(void) +{ + return rxBufferLength - rxBufferIndex; +} + +// must be called in: +// slave rx event callback +// or after requestFrom(address, numBytes) +int TwoWire::read(void) +{ + int value = -1; + + // get each successive byte on each call + if(rxBufferIndex < rxBufferLength){ + value = rxBuffer[rxBufferIndex]; + ++rxBufferIndex; + } + + return value; +} + +// must be called in: +// slave rx event callback +// or after requestFrom(address, numBytes) +int TwoWire::peek(void) +{ + int value = -1; + + if(rxBufferIndex < rxBufferLength){ + value = rxBuffer[rxBufferIndex]; + } + + return value; +} + +void TwoWire::flush(void) +{ + // XXX: to be implemented. +} + +// behind the scenes function that is called when data is received +void TwoWire::onReceiveService(uint8_t* inBytes, int numBytes) +{ + // don't bother if user hasn't registered a callback + if(!user_onReceive){ + return; + } + // don't bother if rx buffer is in use by a master requestFrom() op + // i know this drops data, but it allows for slight stupidity + // meaning, they may not have read all the master requestFrom() data yet + if(rxBufferIndex < rxBufferLength){ + return; + } + // copy twi rx buffer into local read buffer + // this enables new reads to happen in parallel + for(uint8_t i = 0; i < numBytes; ++i){ + rxBuffer[i] = inBytes[i]; + } + // set rx iterator vars + rxBufferIndex = 0; + rxBufferLength = numBytes; + // alert user program + user_onReceive(numBytes); +} + +// behind the scenes function that is called when data is requested +void TwoWire::onRequestService(void) +{ + // don't bother if user hasn't registered a callback + if(!user_onRequest){ + return; + } + // reset tx buffer iterator vars + // !!! this will kill any pending pre-master sendTo() activity + txBufferIndex = 0; + txBufferLength = 0; + // alert user program + user_onRequest(); +} + +// sets function called on slave write +void TwoWire::onReceive( void (*function)(int) ) +{ + user_onReceive = function; +} + +// sets function called on slave read +void TwoWire::onRequest( void (*function)(void) ) +{ + user_onRequest = function; +} + +// Preinstantiate Objects ////////////////////////////////////////////////////// + +TwoWire Wire = TwoWire(); + diff --git a/Libraries/Wire/Wire.h b/Libraries/Wire/Wire.h new file mode 100644 index 0000000..a93d0f5 --- /dev/null +++ b/Libraries/Wire/Wire.h @@ -0,0 +1,79 @@ +/* + TwoWire.h - TWI/I2C library for Arduino & Wiring + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts +*/ + +#ifndef TwoWire_h +#define TwoWire_h + +#include +#include "Stream.h" + +#define BUFFER_LENGTH 32 + +class TwoWire : public Stream +{ + private: + static uint8_t rxBuffer[]; + static uint8_t rxBufferIndex; + static uint8_t rxBufferLength; + + static uint8_t txAddress; + static uint8_t txBuffer[]; + static uint8_t txBufferIndex; + static uint8_t txBufferLength; + + static uint8_t transmitting; + static void (*user_onRequest)(void); + static void (*user_onReceive)(int); + static void onRequestService(void); + static void onReceiveService(uint8_t*, int); + public: + TwoWire(); + void begin(); + void begin(uint8_t); + void begin(int); + void beginTransmission(uint8_t); + void beginTransmission(int); + uint8_t endTransmission(void); + uint8_t endTransmission(uint8_t); + uint8_t requestFrom(uint8_t, uint8_t); + uint8_t requestFrom(uint8_t, uint8_t, uint8_t); + uint8_t requestFrom(int, int); + uint8_t requestFrom(int, int, int); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *, size_t); + virtual int available(void); + virtual int read(void); + virtual int peek(void); + virtual void flush(void); + void onReceive( void (*)(int) ); + void onRequest( void (*)(void) ); + + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + using Print::write; +}; + +extern TwoWire Wire; + +#endif + diff --git a/Libraries/Wire/examples/SFRRanger_reader/SFRRanger_reader.ino b/Libraries/Wire/examples/SFRRanger_reader/SFRRanger_reader.ino new file mode 100644 index 0000000..9c41c18 --- /dev/null +++ b/Libraries/Wire/examples/SFRRanger_reader/SFRRanger_reader.ino @@ -0,0 +1,87 @@ +// I2C SRF10 or SRF08 Devantech Ultrasonic Ranger Finder +// by Nicholas Zambetti +// and James Tichenor + +// Demonstrates use of the Wire library reading data from the +// Devantech Utrasonic Rangers SFR08 and SFR10 + +// Created 29 April 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) + Serial.begin(9600); // start serial communication at 9600bps +} + +int reading = 0; + +void loop() +{ + // step 1: instruct sensor to read echoes + Wire.beginTransmission(112); // transmit to device #112 (0x70) + // the address specified in the datasheet is 224 (0xE0) + // but i2c adressing uses the high 7 bits so it's 112 + Wire.write(byte(0x00)); // sets register pointer to the command register (0x00) + Wire.write(byte(0x50)); // command sensor to measure in "inches" (0x50) + // use 0x51 for centimeters + // use 0x52 for ping microseconds + Wire.endTransmission(); // stop transmitting + + // step 2: wait for readings to happen + delay(70); // datasheet suggests at least 65 milliseconds + + // step 3: instruct sensor to return a particular echo reading + Wire.beginTransmission(112); // transmit to device #112 + Wire.write(byte(0x02)); // sets register pointer to echo #1 register (0x02) + Wire.endTransmission(); // stop transmitting + + // step 4: request reading from sensor + Wire.requestFrom(112, 2); // request 2 bytes from slave device #112 + + // step 5: receive reading from sensor + if(2 <= Wire.available()) // if two bytes were received + { + reading = Wire.read(); // receive high byte (overwrites previous reading) + reading = reading << 8; // shift high byte to be high 8 bits + reading |= Wire.read(); // receive low byte as lower 8 bits + Serial.println(reading); // print the reading + } + + delay(250); // wait a bit since people have to read the output :) +} + + +/* + +// The following code changes the address of a Devantech Ultrasonic Range Finder (SRF10 or SRF08) +// usage: changeAddress(0x70, 0xE6); + +void changeAddress(byte oldAddress, byte newAddress) +{ + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(byte(0xA0)); + Wire.endTransmission(); + + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(byte(0xAA)); + Wire.endTransmission(); + + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(byte(0xA5)); + Wire.endTransmission(); + + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(newAddress); + Wire.endTransmission(); +} + +*/ diff --git a/Libraries/Wire/examples/digital_potentiometer/digital_potentiometer.ino b/Libraries/Wire/examples/digital_potentiometer/digital_potentiometer.ino new file mode 100644 index 0000000..38da1c5 --- /dev/null +++ b/Libraries/Wire/examples/digital_potentiometer/digital_potentiometer.ino @@ -0,0 +1,39 @@ +// I2C Digital Potentiometer +// by Nicholas Zambetti +// and Shawn Bonkowski + +// Demonstrates use of the Wire library +// Controls AD5171 digital potentiometer via I2C/TWI + +// Created 31 March 2006 + +// This example code is in the public domain. + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) +} + +byte val = 0; + +void loop() +{ + Wire.beginTransmission(44); // transmit to device #44 (0x2c) + // device address is specified in datasheet + Wire.write(byte(0x00)); // sends instruction byte + Wire.write(val); // sends potentiometer value byte + Wire.endTransmission(); // stop transmitting + + val++; // increment value + if(val == 64) // if reached 64th position (max) + { + val = 0; // start over from lowest value + } + delay(500); +} + diff --git a/Libraries/Wire/examples/master_reader/master_reader.ino b/Libraries/Wire/examples/master_reader/master_reader.ino new file mode 100644 index 0000000..4124d7d --- /dev/null +++ b/Libraries/Wire/examples/master_reader/master_reader.ino @@ -0,0 +1,32 @@ +// Wire Master Reader +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Reads data from an I2C/TWI slave device +// Refer to the "Wire Slave Sender" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) + Serial.begin(9600); // start serial for output +} + +void loop() +{ + Wire.requestFrom(2, 6); // request 6 bytes from slave device #2 + + while(Wire.available()) // slave may send less than requested + { + char c = Wire.read(); // receive a byte as character + Serial.print(c); // print the character + } + + delay(500); +} diff --git a/Libraries/Wire/examples/master_writer/master_writer.ino b/Libraries/Wire/examples/master_writer/master_writer.ino new file mode 100644 index 0000000..ccaa036 --- /dev/null +++ b/Libraries/Wire/examples/master_writer/master_writer.ino @@ -0,0 +1,31 @@ +// Wire Master Writer +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Writes data to an I2C/TWI slave device +// Refer to the "Wire Slave Receiver" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) +} + +byte x = 0; + +void loop() +{ + Wire.beginTransmission(4); // transmit to device #4 + Wire.write("x is "); // sends five bytes + Wire.write(x); // sends one byte + Wire.endTransmission(); // stop transmitting + + x++; + delay(500); +} diff --git a/Libraries/Wire/examples/slave_receiver/slave_receiver.ino b/Libraries/Wire/examples/slave_receiver/slave_receiver.ino new file mode 100644 index 0000000..60dd4bd --- /dev/null +++ b/Libraries/Wire/examples/slave_receiver/slave_receiver.ino @@ -0,0 +1,38 @@ +// Wire Slave Receiver +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Receives data as an I2C/TWI slave device +// Refer to the "Wire Master Writer" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(4); // join i2c bus with address #4 + Wire.onReceive(receiveEvent); // register event + Serial.begin(9600); // start serial for output +} + +void loop() +{ + delay(100); +} + +// function that executes whenever data is received from master +// this function is registered as an event, see setup() +void receiveEvent(int howMany) +{ + while(1 < Wire.available()) // loop through all but the last + { + char c = Wire.read(); // receive byte as a character + Serial.print(c); // print the character + } + int x = Wire.read(); // receive byte as an integer + Serial.println(x); // print the integer +} diff --git a/Libraries/Wire/examples/slave_sender/slave_sender.ino b/Libraries/Wire/examples/slave_sender/slave_sender.ino new file mode 100644 index 0000000..d3b238a --- /dev/null +++ b/Libraries/Wire/examples/slave_sender/slave_sender.ino @@ -0,0 +1,32 @@ +// Wire Slave Sender +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Sends data as an I2C/TWI slave device +// Refer to the "Wire Master Reader" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(2); // join i2c bus with address #2 + Wire.onRequest(requestEvent); // register event +} + +void loop() +{ + delay(100); +} + +// function that executes whenever data is requested by master +// this function is registered as an event, see setup() +void requestEvent() +{ + Wire.write("hello "); // respond with message of 6 bytes + // as expected by master +} diff --git a/Libraries/Wire/keywords.txt b/Libraries/Wire/keywords.txt new file mode 100644 index 0000000..37765af --- /dev/null +++ b/Libraries/Wire/keywords.txt @@ -0,0 +1,31 @@ +####################################### +# Syntax Coloring Map For Wire +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +begin KEYWORD2 +beginTransmission KEYWORD2 +endTransmission KEYWORD2 +requestFrom KEYWORD2 +send KEYWORD2 +receive KEYWORD2 +onReceive KEYWORD2 +onRequest KEYWORD2 + +####################################### +# Instances (KEYWORD2) +####################################### + +Wire KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/Libraries/Wire/utility/twi.c b/Libraries/Wire/utility/twi.c new file mode 100644 index 0000000..201d7d1 --- /dev/null +++ b/Libraries/Wire/utility/twi.c @@ -0,0 +1,527 @@ +/* + twi.c - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts +*/ + +#include +#include +#include +#include +#include +#include +#include "Arduino.h" // for digitalWrite + +#ifndef cbi +#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif + +#ifndef sbi +#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +#include "pins_arduino.h" +#include "twi.h" + +static volatile uint8_t twi_state; +static volatile uint8_t twi_slarw; +static volatile uint8_t twi_sendStop; // should the transaction end with a stop +static volatile uint8_t twi_inRepStart; // in the middle of a repeated start + +static void (*twi_onSlaveTransmit)(void); +static void (*twi_onSlaveReceive)(uint8_t*, int); + +static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_masterBufferIndex; +static volatile uint8_t twi_masterBufferLength; + +static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_txBufferIndex; +static volatile uint8_t twi_txBufferLength; + +static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_rxBufferIndex; + +static volatile uint8_t twi_error; + +/* + * Function twi_init + * Desc readys twi pins and sets twi bitrate + * Input none + * Output none + */ +void twi_init(void) +{ + // initialize state + twi_state = TWI_READY; + twi_sendStop = true; // default value + twi_inRepStart = false; + + // activate internal pullups for twi. + digitalWrite(SDA, 1); + digitalWrite(SCL, 1); + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); + cbi(TWSR, TWPS1); + TWBR = ((F_CPU / TWI_FREQ) - 16) / 2; + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + // enable twi module, acks, and twi interrupt + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); +} + +/* + * Function twi_slaveInit + * Desc sets slave address and enables interrupt + * Input none + * Output none + */ +void twi_setAddress(uint8_t address) +{ + // set twi slave address (skip over TWGCE bit) + TWAR = address << 1; +} + +/* + * Function twi_readFrom + * Desc attempts to become twi bus master and read a + * series of bytes from a device on the bus + * Input address: 7bit i2c device address + * data: pointer to byte array + * length: number of bytes to read into array + * sendStop: Boolean indicating whether to send a stop at the end + * Output number of bytes read + */ +uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 0; + } + + // wait until twi is ready, become master receiver + while(TWI_READY != twi_state){ + continue; + } + twi_state = TWI_MRX; + twi_sendStop = sendStop; + // reset error state (0xFF.. no error occured) + twi_error = 0xFF; + + // initialize buffer iteration vars + twi_masterBufferIndex = 0; + twi_masterBufferLength = length-1; // This is not intuitive, read on... + // On receive, the previously configured ACK/NACK setting is transmitted in + // response to the received byte before the interrupt is signalled. + // Therefor we must actually set NACK when the _next_ to last byte is + // received, causing that NACK to be sent in response to receiving the last + // expected byte of data. + + // build sla+w, slave device address + w bit + twi_slarw = TW_READ; + twi_slarw |= address << 1; + + if (true == twi_inRepStart) { + // if we're in the repeated start state, then we've already sent the start, + // (@@@ we hope), and the TWI statemachine is just waiting for the address byte. + // We need to remove ourselves from the repeated start state before we enable interrupts, + // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning + // up. Also, don't enable the START interrupt. There may be one pending from the + // repeated start that we sent outselves, and that would really confuse things. + twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR + TWDR = twi_slarw; + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START + } + else + // send start condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + + // wait for read operation to complete + while(TWI_MRX == twi_state){ + continue; + } + + if (twi_masterBufferIndex < length) + length = twi_masterBufferIndex; + + // copy twi buffer to data + for(i = 0; i < length; ++i){ + data[i] = twi_masterBuffer[i]; + } + + return length; +} + +/* + * Function twi_writeTo + * Desc attempts to become twi bus master and write a + * series of bytes to a device on the bus + * Input address: 7bit i2c device address + * data: pointer to byte array + * length: number of bytes in array + * wait: boolean indicating to wait for write or not + * sendStop: boolean indicating whether or not to send a stop at the end + * Output 0 .. success + * 1 .. length to long for buffer + * 2 .. address send, NACK received + * 3 .. data send, NACK received + * 4 .. other twi error (lost bus arbitration, bus error, ..) + */ +uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 1; + } + + // wait until twi is ready, become master transmitter + while(TWI_READY != twi_state){ + continue; + } + twi_state = TWI_MTX; + twi_sendStop = sendStop; + // reset error state (0xFF.. no error occured) + twi_error = 0xFF; + + // initialize buffer iteration vars + twi_masterBufferIndex = 0; + twi_masterBufferLength = length; + + // copy data to twi buffer + for(i = 0; i < length; ++i){ + twi_masterBuffer[i] = data[i]; + } + + // build sla+w, slave device address + w bit + twi_slarw = TW_WRITE; + twi_slarw |= address << 1; + + // if we're in a repeated start, then we've already sent the START + // in the ISR. Don't do it again. + // + if (true == twi_inRepStart) { + // if we're in the repeated start state, then we've already sent the start, + // (@@@ we hope), and the TWI statemachine is just waiting for the address byte. + // We need to remove ourselves from the repeated start state before we enable interrupts, + // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning + // up. Also, don't enable the START interrupt. There may be one pending from the + // repeated start that we sent outselves, and that would really confuse things. + twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR + TWDR = twi_slarw; + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START + } + else + // send start condition + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA); // enable INTs + + // wait for write operation to complete + while(wait && (TWI_MTX == twi_state)){ + continue; + } + + if (twi_error == 0xFF) + return 0; // success + else if (twi_error == TW_MT_SLA_NACK) + return 2; // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + return 3; // error: data send, nack received + else + return 4; // other twi error +} + +/* + * Function twi_transmit + * Desc fills slave tx buffer with data + * must be called in slave tx event callback + * Input data: pointer to byte array + * length: number of bytes in array + * Output 1 length too long for buffer + * 2 not slave transmitter + * 0 ok + */ +uint8_t twi_transmit(const uint8_t* data, uint8_t length) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 1; + } + + // ensure we are currently a slave transmitter + if(TWI_STX != twi_state){ + return 2; + } + + // set length and copy data into tx buffer + twi_txBufferLength = length; + for(i = 0; i < length; ++i){ + twi_txBuffer[i] = data[i]; + } + + return 0; +} + +/* + * Function twi_attachSlaveRxEvent + * Desc sets function called before a slave read operation + * Input function: callback function to use + * Output none + */ +void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) ) +{ + twi_onSlaveReceive = function; +} + +/* + * Function twi_attachSlaveTxEvent + * Desc sets function called before a slave write operation + * Input function: callback function to use + * Output none + */ +void twi_attachSlaveTxEvent( void (*function)(void) ) +{ + twi_onSlaveTransmit = function; +} + +/* + * Function twi_reply + * Desc sends byte or readys receive line + * Input ack: byte indicating to ack or to nack + * Output none + */ +void twi_reply(uint8_t ack) +{ + // transmit master read ready signal, with or without ack + if(ack){ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + }else{ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + } +} + +/* + * Function twi_stop + * Desc relinquishes bus master status + * Input none + * Output none + */ +void twi_stop(void) +{ + // send stop condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + + // wait for stop condition to be exectued on bus + // TWINT is not set after a stop condition! + while(TWCR & _BV(TWSTO)){ + continue; + } + + // update twi state + twi_state = TWI_READY; +} + +/* + * Function twi_releaseBus + * Desc releases bus control + * Input none + * Output none + */ +void twi_releaseBus(void) +{ + // release bus + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + + // update twi state + twi_state = TWI_READY; +} + +ISR(TWI_vect) +{ + switch(TW_STATUS){ + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if(twi_masterBufferIndex < twi_masterBufferLength){ + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + }else{ + if (twi_sendStop) + twi_stop(); + else { + twi_inRepStart = true; // we're gonna send the START + // don't enable the interrupt. We'll generate the start, but we + // avoid handling the interrupt until we're in the next transaction, + // at the point where we would normally issue the start. + TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ; + twi_state = TWI_READY; + } + } + break; + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if(twi_masterBufferIndex < twi_masterBufferLength){ + twi_reply(1); + }else{ + twi_reply(0); + } + break; + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + if (twi_sendStop) + twi_stop(); + else { + twi_inRepStart = true; // we're gonna send the START + // don't enable the interrupt. We'll generate the start, but we + // avoid handling the interrupt until we're in the next transaction, + // at the point where we would normally issue the start. + TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ; + twi_state = TWI_READY; + } + break; + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){ + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + }else{ + // otherwise nack + twi_reply(0); + } + break; + case TW_SR_STOP: // stop or repeated start condition received + // put a null char after data if there's room + if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){ + twi_rxBuffer[twi_rxBufferIndex] = '\0'; + } + // sends ack and stops interface for clock stretching + twi_stop(); + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + // ack future responses and leave slave receiver state + twi_releaseBus(); + break; + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + // if they didn't change buffer & length, initialize it + if(0 == twi_txBufferLength){ + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + // transmit first byte from buffer, fall + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + // if there is more to send, ack, otherwise nack + if(twi_txBufferIndex < twi_txBufferLength){ + twi_reply(1); + }else{ + twi_reply(0); + } + break; + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + + // All + case TW_NO_INFO: // no state information + break; + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } +} + diff --git a/Libraries/Wire/utility/twi.h b/Libraries/Wire/utility/twi.h new file mode 100644 index 0000000..6526593 --- /dev/null +++ b/Libraries/Wire/utility/twi.h @@ -0,0 +1,53 @@ +/* + twi.h - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef twi_h +#define twi_h + + #include + + //#define ATMEGA8 + + #ifndef TWI_FREQ + #define TWI_FREQ 100000L + #endif + + #ifndef TWI_BUFFER_LENGTH + #define TWI_BUFFER_LENGTH 32 + #endif + + #define TWI_READY 0 + #define TWI_MRX 1 + #define TWI_MTX 2 + #define TWI_SRX 3 + #define TWI_STX 4 + + void twi_init(void); + void twi_setAddress(uint8_t); + uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t); + uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t); + uint8_t twi_transmit(const uint8_t*, uint8_t); + void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) ); + void twi_attachSlaveTxEvent( void (*)(void) ); + void twi_reply(uint8_t); + void twi_stop(void); + void twi_releaseBus(void); + +#endif + diff --git a/Libraries/light_WS2812/WS2812.cpp b/Libraries/light_WS2812/WS2812.cpp index 1d7f87b..b5064c8 100644 --- a/Libraries/light_WS2812/WS2812.cpp +++ b/Libraries/light_WS2812/WS2812.cpp @@ -1,100 +1,106 @@ -/* -* light weight WS2812 lib V2.1 - Arduino support -* -* Controls WS2811/WS2812/WS2812B RGB-LEDs -* Author: Matthias Riegler -* -* Mar 07 2014: Added Arduino and C++ Library -* -* September 6, 2014: Added option to switch between most popular color orders -* (RGB, GRB, and BRG) -- Windell H. Oskay -* -* License: GNU GPL v2 (see License.txt) -*/ - -#include "WS2812.h" -#include - -WS2812::WS2812(uint16_t num_leds) { - count_led = num_leds; - - pixels = (uint8_t*)malloc(count_led*3); - offsetGreen = 0; - offsetRed = 1; - offsetBlue = 2; -} - -cRGB WS2812::get_crgb_at(uint16_t index) { - - cRGB px_value; - - if(index < count_led) { - - uint16_t tmp; - tmp = index * 3; - - px_value.r = pixels[tmp+offsetRed]; - px_value.g = pixels[tmp+offsetGreen]; - px_value.b = pixels[tmp+offsetBlue]; - } - - return px_value; -} - -uint8_t WS2812::set_crgb_at(uint16_t index, cRGB px_value) { - - if(index < count_led) { - - uint16_t tmp; - tmp = index * 3; - - pixels[tmp+offsetGreen] = px_value.g; - pixels[tmp+offsetRed] = px_value.r; - pixels[tmp+offsetBlue] = px_value.b; - - return 0; - } - return 1; -} - -void WS2812::sync() { - *ws2812_port_reg |= pinMask; // Enable DDR - ws2812_sendarray_mask(pixels,3*count_led,pinMask,(uint8_t*) ws2812_port,(uint8_t*) ws2812_port_reg ); -} - -void WS2812::setColorOrderGRB() { // Default color order - offsetGreen = 0; - offsetRed = 1; - offsetBlue = 2; -} - -void WS2812::setColorOrderRGB() { - offsetRed = 0; - offsetGreen = 1; - offsetBlue = 2; -} - -void WS2812::setColorOrderBRG() { - offsetBlue = 0; - offsetRed = 1; - offsetGreen = 2; -} - -WS2812::~WS2812() { - - -} - -#ifndef ARDUINO -void WS2812::setOutput(const volatile uint8_t* port, volatile uint8_t* reg, uint8_t pin) { - pinMask = (1< + +WS2812::WS2812(uint16_t num_leds) { + count_led = num_leds; + + pixels = (uint8_t*)malloc(count_led*3); + offsetGreen = 0; + offsetRed = 1; + offsetBlue = 2; +} + +cRGB WS2812::get_crgb_at(uint16_t index) { + + cRGB px_value; + + if(index < count_led) { + + uint16_t tmp; + tmp = index * 3; + + px_value.r = pixels[tmp+offsetRed]; + px_value.g = pixels[tmp+offsetGreen]; + px_value.b = pixels[tmp+offsetBlue]; + } + + return px_value; +} + +uint8_t WS2812::set_crgb_at(uint16_t index, cRGB px_value) { + + if(index < count_led) { + + uint16_t tmp; + tmp = index * 3; + + pixels[tmp+offsetGreen] = px_value.g; + pixels[tmp+offsetRed] = px_value.r; + pixels[tmp+offsetBlue] = px_value.b; + + return 0; + } + return 1; +} + +void WS2812::sync() { + *ws2812_port_reg |= pinMask; // Enable DDR + ws2812_sendarray_mask(pixels,3*count_led,pinMask,(uint8_t*) ws2812_port,(uint8_t*) ws2812_port_reg ); +} + +void WS2812::setColorOrderGRB() { // Default color order + offsetGreen = 0; + offsetRed = 1; + offsetBlue = 2; +} + +void WS2812::setColorOrderRGB() { + offsetRed = 0; + offsetGreen = 1; + offsetBlue = 2; +} + +void WS2812::setColorOrderBRG() { + offsetBlue = 0; + offsetRed = 1; + offsetGreen = 2; +} + +void WS2812::setColorOrderRBG() { + offsetRed = 0; + offsetBlue = 1; + offsetGreen = 2; +} + +WS2812::~WS2812() { + + +} + +#ifndef ARDUINO +void WS2812::setOutput(const volatile uint8_t* port, volatile uint8_t* reg, uint8_t pin) { + pinMask = (1< -#include -#ifndef F_CPU -#define F_CPU 16000000UL -#endif -#include -#include - -#ifdef ARDUINO -#if (ARDUINO >= 100) -#include -#else -#include -#include -#endif -#endif - -struct cRGB { uint8_t g; uint8_t r; uint8_t b; }; - -class WS2812 { -public: - WS2812(uint16_t num_led); - ~WS2812(); - - #ifndef ARDUINO - void setOutput(const volatile uint8_t* port, volatile uint8_t* reg, uint8_t pin); - #else - void setOutput(uint8_t pin); - #endif - - cRGB get_crgb_at(uint16_t index); - uint8_t set_crgb_at(uint16_t index, cRGB px_value); - void sync(); - void setColorOrderRGB(); - void setColorOrderGRB(); - void setColorOrderBRG(); - - -private: - uint16_t count_led; - uint8_t *pixels; - uint8_t offsetRed; - uint8_t offsetGreen; - uint8_t offsetBlue; - - void ws2812_sendarray_mask(uint8_t *array,uint16_t length, uint8_t pinmask,uint8_t *port, uint8_t *portreg); - - const volatile uint8_t *ws2812_port; - volatile uint8_t *ws2812_port_reg; - uint8_t pinMask; -}; - - - +/* +* light weight WS2812 lib V2.1 - Arduino support +* +* Controls WS2811/WS2812/WS2812B RGB-LEDs +* Author: Matthias Riegler +* +* Mar 07 2014: Added Arduino and C++ Library +* +* September 6, 2014 Added option to switch between most popular color orders +* (RGB, GRB, and BRG) -- Windell H. Oskay +* +* License: GNU GPL v2 (see License.txt) +*/ + +#ifndef WS2812_H_ +#define WS2812_H_ + +#include +#include +#ifndef F_CPU +#define F_CPU 16000000UL +#endif +#include +#include + +#ifdef ARDUINO +#if (ARDUINO >= 100) +#include +#else +#include +#include +#endif +#endif + +struct cRGB { uint8_t g; uint8_t r; uint8_t b; }; + +class WS2812 { +public: + WS2812(uint16_t num_led); + ~WS2812(); + + #ifndef ARDUINO + void setOutput(const volatile uint8_t* port, volatile uint8_t* reg, uint8_t pin); + #else + void setOutput(uint8_t pin); + #endif + + cRGB get_crgb_at(uint16_t index); + uint8_t set_crgb_at(uint16_t index, cRGB px_value); + void sync(); + void setColorOrderRGB(); + void setColorOrderGRB(); + void setColorOrderBRG(); + void setColorOrderRBG(); + + +private: + uint16_t count_led; + uint8_t *pixels; + uint8_t offsetRed; + uint8_t offsetGreen; + uint8_t offsetBlue; + + void ws2812_sendarray_mask(uint8_t *array,uint16_t length, uint8_t pinmask,uint8_t *port, uint8_t *portreg); + + const volatile uint8_t *ws2812_port; + volatile uint8_t *ws2812_port_reg; + uint8_t pinMask; +}; + + + #endif /* WS2812_H_ */ \ No newline at end of file diff --git a/Libraries/light_WS2812/examples/Blinky/Blinky.ino b/Libraries/light_WS2812/examples/Blinky/Blinky.ino index 3308fa0..afdb724 100644 --- a/Libraries/light_WS2812/examples/Blinky/Blinky.ino +++ b/Libraries/light_WS2812/examples/Blinky/Blinky.ino @@ -12,7 +12,7 @@ WS2812 LED(1); // 1 LED cRGB value; void setup() { - LED.setOutput(14); // Digital Pin 9 + LED.setOutput(9); // Digital Pin 9 } void loop() { diff --git a/Libraries/light_WS2812/examples/fade_rgb/fade_rgb.ino b/Libraries/light_WS2812/examples/fade_rgb/fade_rgb.ino index 4c9ed88..da9e4d5 100644 --- a/Libraries/light_WS2812/examples/fade_rgb/fade_rgb.ino +++ b/Libraries/light_WS2812/examples/fade_rgb/fade_rgb.ino @@ -8,8 +8,8 @@ #include -#define outputPin 14 // Digital output pin (default: 7) -#define LEDCount 4 // Number of LEDs to drive (default: 9) +#define outputPin 7 // Digital output pin (default: 7) +#define LEDCount 9 // Number of LEDs to drive (default: 9) WS2812 LED(LEDCount); diff --git a/Libraries/light_WS2812/light_ws2812.cpp b/Libraries/light_WS2812/light_ws2812.cpp index c36678b..2440d23 100644 --- a/Libraries/light_WS2812/light_ws2812.cpp +++ b/Libraries/light_WS2812/light_ws2812.cpp @@ -26,7 +26,7 @@ // Fixed cycles used by the inner loop #define w_fixedlow 3 #define w_fixedhigh 6 -#define w_fixedtotal 10 +#define w_fixedtotal 10 // Insert NOPs to match the timing, if possible #define w_zerocycles (((F_CPU/1000)*w_zeropulse )/1000000) diff --git a/Light.cpp b/Light.cpp index 663154a..14e4c22 100644 --- a/Light.cpp +++ b/Light.cpp @@ -1,7 +1,7 @@ /* * Light.cpp * - * author: Sebastien CAPOU (neskweek@gmail.com) + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS */ #include "Light.h" @@ -11,13 +11,50 @@ #include #endif +enum SaberStateEnum {S_STANDBY, S_SABERON, S_CONFIG, S_SLEEP, S_JUKEBOX}; +enum ActionModeSubStatesEnum {AS_HUM, AS_IGNITION, AS_RETRACTION, AS_BLADELOCKUP, AS_BLASTERDEFLECTMOTION, AS_BLASTERDEFLECTPRESS, AS_CLASH, AS_SWING, AS_SPIN, AS_FORCE}; +extern SaberStateEnum SaberState; +extern SaberStateEnum PrevSaberState; +extern ActionModeSubStatesEnum ActionModeSubStates; + + +#ifdef JUKEBOX +#define SAMPLESIZEAVERAGE 30 +#endif + +#ifdef FIREBLADE +bool gReverseDirection = false; +// COOLING: How much does the air cool as it rises? +// Less cooling = taller flames. More cooling = shorter flames. +// Default 50, suggested range 20-100 +static uint8_t Fire_Cooling = 50; + +// SPARKING: What chance (out of 255) is there that a new spark will be lit? +// Higher chance = more roaring fire. Lower chance = more flickery fire. +// Default 120, suggested range 50-200. +static uint8_t Fire_Sparking = 100; +#ifdef CROSSGUARDSABER +static byte heat[MN_STRIPE]; +static byte heat_cg[CG_STRIPE]; +#else +static byte heat[NUMPIXELS]; +#endif +#endif // FIREBLADE + // ==================================================================================== // === LED FUNCTIONS === // ==================================================================================== #if defined LEDSTRINGS +#define I_BEGINNEXTSEGMENT 50 +#define R_BEGINNEXTSEGMENT 100 +#define PULSEFLICKERDEPTH 70 +#define PULSEDURATION 1000 + static uint8_t flickerPos = 0; -static long lastFlicker = millis(); +static long lastFlicker = 0; +uint8_t pulseflicker_pwm=0; +bool pulsedir=true; void lightOn(uint8_t ledPins[], int8_t segment) { // Light On @@ -29,33 +66,6 @@ void lightOn(uint8_t ledPins[], int8_t segment) { } else { digitalWrite(ledPins[segment], HIGH); } - /* Should have save some hex size - * Needs further testing - switch (segment) { - case -1: // Light up everything at once ! - PORTD |= B01101000; - PORTB |= B00001110; - break; - case 0: - PORTD |= (1 << PD3);//DPIN 3 - break; - case 1: - PORTD |= (1 << PD5);//DPIN 5 - break; - case 2: - PORTD |= (1 << PD6);//DPIN 6 - break; - case 3: - PORTB |= (1 << PD1);//DPIN 9 - break; - case 4: - PORTB |= (1 << PD2);//DPIN 10 - break; - case 5: - PORTB |= (1 << PD3);//DPIN 11 - break; - } - */ } //lightOn void lightOff() { @@ -69,28 +79,52 @@ void lightOff() { PORTD &= B10010111; } //lightOff + + void lightIgnition(uint8_t ledPins[], uint16_t time, uint8_t type) { + + +uint8_t LS_Status[6]; +bool ongoing=true; + switch (type) { case 0: + for (uint8_t i=0; i<6; i++) { + LS_Status[i]=0; + } + while (ongoing) { // do the loops as long the variable is set to false, when the last segment finsihed the ramp + for (uint8_t i = 0; i < 6; i++) { + analogWrite(ledPins[i], LS_Status[i]); + if (i==0 and LS_Status[i]<255) { + LS_Status[i]++; + } + else if (i>0 and LS_Status[i-1]>=I_BEGINNEXTSEGMENT and LS_Status[i]<255) { + LS_Status[i]++; + } + if (LS_Status[5]==255) { + ongoing=false; + } + } + delayMicroseconds(time * (1000/(5*I_BEGINNEXTSEGMENT+255))); + } + // ramp down to MAX_BRIGHTNESS + for (uint8_t j = 255; j >= MAX_BRIGHTNESS; j--) { + for (uint8_t i = 0; i < 6; i++) { + analogWrite(ledPins[i], j); + } + delay(3); + } + /* // Light up the ledstrings Movie-like for (uint8_t i = 0; i < 6; i++) { - digitalWrite(ledPins[i], HIGH); - delay(time / 5); + for (uint8_t j=0; j<=MAX_BRIGHTNESS;j+=10) { + analogWrite(ledPins[i], j); + delay(time / (6*25)); + } + //delay(time / (5*10)); } - /* - PORTD |= (1 << PD3);//DPIN 3 - delay(time / 5); - PORTD |= (1 << PD5);//DPIN 5 - delay(time / 5); - PORTD |= (1 << PD6);//DPIN 6 - delay(time / 5); - PORTB |= (1 << PD1);//DPIN 9 - delay(time / 5); - PORTB |= (1 << PD2);//DPIN 10 - delay(time / 5); - PORTB |= (1 << PD3);//DPIN 11 - */ + */ break; case 1: for (int8_t i = 5; i >= 0; i--) { @@ -102,171 +136,44 @@ void lightIgnition(uint8_t ledPins[], uint16_t time, uint8_t type) { delay(time / 20); } } - /* - digitalWrite(ledPins[0], HIGH); - delay(time / 20); - digitalWrite(ledPins[0], LOW); - digitalWrite(ledPins[1], HIGH); - delay(time / 20); - digitalWrite(ledPins[1], LOW); - digitalWrite(ledPins[2], HIGH); - delay(time / 20); - digitalWrite(ledPins[2], LOW); - digitalWrite(ledPins[3], HIGH); - delay(time / 20); - digitalWrite(ledPins[3], LOW); - digitalWrite(ledPins[4], HIGH); - delay(time / 20); - digitalWrite(ledPins[4], LOW); - digitalWrite(ledPins[5], HIGH); - delay(time / 20); - - digitalWrite(ledPins[0], HIGH); - delay(time / 20); - digitalWrite(ledPins[0], LOW); - digitalWrite(ledPins[1], HIGH); - delay(time / 20); - digitalWrite(ledPins[1], LOW); - digitalWrite(ledPins[2], HIGH); - delay(time / 20); - digitalWrite(ledPins[2], LOW); - digitalWrite(ledPins[3], HIGH); - delay(time / 20); - digitalWrite(ledPins[3], LOW); - digitalWrite(ledPins[4], HIGH); - delay(time / 20); - - digitalWrite(ledPins[0], HIGH); - delay(time / 20); - digitalWrite(ledPins[0], LOW); - digitalWrite(ledPins[1], HIGH); - delay(time / 20); - digitalWrite(ledPins[1], LOW); - digitalWrite(ledPins[2], HIGH); - delay(time / 20); - digitalWrite(ledPins[2], LOW); - digitalWrite(ledPins[3], HIGH); - delay(time / 20); - - digitalWrite(ledPins[0], HIGH); - delay(time / 20); - digitalWrite(ledPins[0], LOW); - digitalWrite(ledPins[1], HIGH); - delay(time / 20); - digitalWrite(ledPins[1], LOW); - digitalWrite(ledPins[2], HIGH); - delay(time / 20); - - digitalWrite(ledPins[0], HIGH); - delay(time / 20); - digitalWrite(ledPins[0], LOW); - digitalWrite(ledPins[1], HIGH); - delay(time / 20); - - digitalWrite(ledPins[0], HIGH); - // Light up the ledstrings invert - /* - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD6); - delay(time / 20); - PORTD &= ~(1 << PD6); - PORTB |= (1 << PD1); - delay(time / 20); - PORTB &= ~(1 << PD1); - PORTB |= (1 << PD2); - delay(time / 20); - PORTB &= ~(1 << PD1); - PORTB |= (1 << PD3); - delay(time / 20); - - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD6); - delay(time / 20); - PORTD &= ~(1 << PD6); - PORTB |= (1 << PD1); - delay(time / 20); - PORTB &= ~(1 << PD1); - PORTB |= (1 << PD2); - delay(time / 20); - - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD6); - delay(time / 20); - PORTD &= ~(1 << PD6); - PORTB |= (1 << PD1); - delay(time / 20); - - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD6); - delay(time / 20); - - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - PORTD |= (1 << PD5); - delay(time / 20); - - PORTD |= (1 << PD3); - */ break; } } //lightIgnition void lightRetract(uint8_t ledPins[], uint16_t time, uint8_t type) { - /* - //Make sure the light will be on after PWM shut off - PORTB |= B00001110; - PORTD |= B01101000; - //Shut down PWM - TCCR0A &= ~((1 << COM0A1) | (1 << COM0B1)); - TCCR1A &= ~((1 << COM1A1) | (1 << COM1B1)); - TCCR2A &= ~((1 << COM2A1) | (1 << COM2B1)); - */ + +uint8_t LS_Status[6]; +bool ongoing=true; + switch (type) { case 0: // Light off the ledstrings Movie Like - for (int8_t i = 5; i >= 0; i--) { + for (uint8_t i=0; i<6; i++) { + LS_Status[i]=MAX_BRIGHTNESS; + } + while (ongoing) { // do the loops as long the variable is set to false, when the last segment finsihed the ramp + for (uint8_t i = 0; i < 6; i++) { + if (i==5 and LS_Status[i]>0) { + LS_Status[i]--; + } + else if (i<5 and LS_Status[i+1]<=R_BEGINNEXTSEGMENT and LS_Status[i]>0) { + LS_Status[i]--; + } + if (LS_Status[0]==0) { + ongoing=false; + } + analogWrite(ledPins[i], LS_Status[i]); + } + delayMicroseconds(time * (1000/(5*(MAX_BRIGHTNESS-R_BEGINNEXTSEGMENT)+MAX_BRIGHTNESS))); + } + /*for (int8_t i = 5; i >= 0; i--) { //BUG CORRECTION: //Not uint8_t here because Arduino nano clones did go // on an infinite loop for no reason making the board // crash at some point. digitalWrite(ledPins[i], 0); delay(time / 5); - } - /* - //Shut down each Digital PIN - PORTB &= ~(1 << PD3); //DPIN 11 - delay(time / 5); - PORTB &= ~(1 << PD2); //DPIN 10 - delay(time / 5); - PORTB &= ~(1 << PD1); //DPIN 9 - delay(time / 5); - PORTD &= ~(1 << PD6); //DPIN 6 - delay(time / 5); - PORTD &= ~(1 << PD5); //DPIN 5 - delay(time / 5); - PORTD &= ~(1 << PD3); //DPIN 3 - */ + }*/ break; case 1: // Light off the ledstrings invert @@ -279,72 +186,6 @@ void lightRetract(uint8_t ledPins[], uint16_t time, uint8_t type) { delay(time / 20); } } - /* - PORTD &= ~(1 << PD3); - delay(time / 20); - - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - delay(time / 20); - - PORTD &= ~(1 << PD6); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - delay(time / 20); - - PORTB &= ~(1 << PD1); - PORTD |= (1 << PD6); - delay(time / 20); - PORTD &= ~(1 << PD6); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - delay(time / 20); - - PORTB &= ~(1 << PD2); - PORTB |= (1 << PD1); - delay(time / 20); - PORTB &= ~(1 << PD1); - PORTD |= (1 << PD6); - delay(time / 20); - PORTD &= ~(1 << PD6); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - delay(time / 20); - - PORTB &= ~(1 << PD3); - PORTB |= (1 << PD2); - delay(time / 20); - PORTB &= ~(1 << PD2); - PORTB |= (1 << PD1); - delay(time / 20); - PORTB &= ~(1 << PD1); - PORTD |= (1 << PD6); - delay(time / 20); - PORTD &= ~(1 << PD6); - PORTD |= (1 << PD5); - delay(time / 20); - PORTD &= ~(1 << PD5); - PORTD |= (1 << PD3); - delay(time / 20); - PORTD &= ~(1 << PD3); - - - - */ break; } } //lightRetract @@ -361,12 +202,14 @@ void FoCOff(uint8_t pin) { } //FoCOff #endif -void lightFlicker(uint8_t ledPins[], uint8_t type, uint8_t value) { +void lightFlicker(uint8_t ledPins[], uint8_t type, uint8_t value, uint8_t AState) { uint8_t variation = abs(analogRead(SPK1) - analogRead(SPK2)); uint8_t brightness; + if (not value) { // Calculation of the amount of brightness to fade - brightness = MAX_BRIGHTNESS - variation; + brightness = constrain(MAX_BRIGHTNESS + - (abs(analogRead(SPK1) - analogRead(SPK2)))/8,0,255); } else { brightness = value; } @@ -387,7 +230,7 @@ void lightFlicker(uint8_t ledPins[], uint8_t type, uint8_t value) { } break; case 1: - // pulse Flickering + // anarchic Flickering for (uint8_t i = 0; i <= 5; i++) { if (i != flickerPos) analogWrite(ledPins[i], brightness - variation / 2); @@ -405,16 +248,65 @@ void lightFlicker(uint8_t ledPins[], uint8_t type, uint8_t value) { } break; case 2: - // anarchic Flickering - for (uint8_t i = 0; i <= 5; i++) { - randomSeed(analogRead(ledPins[i])); - analogWrite(ledPins[i], - MAX_BRIGHTNESS - random(variation, variation * 2)); - } + // pulse Flickering + if (((millis()-lastFlicker>=PULSEDURATION/PULSEFLICKERDEPTH) and AState != AS_BLADELOCKUP) or ((millis()-lastFlicker>=2) and AState == AS_BLADELOCKUP)) { + lastFlicker=millis(); + for (uint8_t i = 0; i <= 5; i++) { + analogWrite(ledPins[i],MAX_BRIGHTNESS - pulseflicker_pwm); + } + if (pulsedir) { + pulseflicker_pwm++; + } + else { + pulseflicker_pwm--; + } + if (pulseflicker_pwm == PULSEFLICKERDEPTH) { + pulsedir=false; + } + else if (pulseflicker_pwm == 0) { + pulsedir=true; + } + } break; } } //lightFlicker +#ifdef JUKEBOX +void JukeBox_Stroboscope(uint8_t ledPins[]) { + uint16_t variation = 0; + uint16_t temp_variation=0; + for (uint8_t i=0; i<=SAMPLESIZEAVERAGE-1;i++) { + temp_variation=temp_variation + abs(analogRead(SPK1) - analogRead(SPK2)); + } + variation=temp_variation/SAMPLESIZEAVERAGE; + if (variation>=80) { + analogWrite(ledPins[0], MAX_BRIGHTNESS); + } + else analogWrite(ledPins[0], 0); + if (variation>=110) { + analogWrite(ledPins[1], MAX_BRIGHTNESS); + } + else analogWrite(ledPins[1], 0); + if (variation>=140) { + analogWrite(ledPins[2], MAX_BRIGHTNESS); + } + else analogWrite(ledPins[2], 0); + if (variation>=170) { + analogWrite(ledPins[3], MAX_BRIGHTNESS); + } + else analogWrite(ledPins[3], 0); + if (variation>=200) { + analogWrite(ledPins[4], MAX_BRIGHTNESS); + } + else analogWrite(ledPins[4], 0); + if (variation>=230) { + analogWrite(ledPins[5], MAX_BRIGHTNESS); + } + else analogWrite(ledPins[5], 0); + //delay(50); +} +#endif + #endif #if defined LUXEON @@ -480,37 +372,7 @@ void lightFlicker(uint8_t ledPins[], uint8_t color[], uint8_t value) { } } //lightFlicker -#if defined MY_OWN_COLORS -void getColor(uint8_t color[], uint8_t colorID) { - color[3] = colorID; - switch (colorID) { - case 0: -//Red - color[0] = 100; - color[1] = 0; - color[2] = 0; - break; - case 1: -//Green - color[0] = 0; - color[1] = 100; - color[2] = 0; - break; - case 2: -//Blue - color[0] = 0; - color[1] = 0; - color[2] = 100; - break; - default: -// White? - color[0] = 100; - color[1] = 100; - color[2] = 100; - break; - } -} //getColor -#else +/* void getColor(uint8_t color[], uint16_t colorID) { uint8_t tint = (COLORS / 6); uint8_t step = rgbFactor / tint; @@ -548,7 +410,111 @@ void getColor(uint8_t color[], uint16_t colorID) { } } //getColor +*/ +void getColor(uint8_t color[], uint8_t colorID) { + color[3] = colorID; + switch (colorID) { + case 0: +//Red + color[0] = 200; + color[1] = 0; + color[2] = 0; + break; + case 1: +//Yellow + color[0] = 200; + color[1] = 200; + color[2] = 0; + break; + case 2: +//Green + color[0] = 0; + color[1] = 200; + color[2] = 0; + break; + case 3: +//Aqua + color[0] = 0; + color[1] = 200; + color[2] = 200; + break; + case 4: +//Blue + color[0] = 0; + color[1] = 0; + color[2] = 200; + break; + case 5: +//Fuschia + color[0] = 200; + color[1] = 0; + color[2] = 200; + break; + case 6: +//DarkGrey + color[0] = 150; + color[1] = 150; + color[2] = 150; + break; + case 7: +//DarkOrange + color[0] = 200; + color[1] = 102; + color[2] = 0; + break; + case 8: +//DarkViolet + color[0] = 116; + color[1] = 0; + color[2] = 166; + break; + case 9: +//DodgerBlue + color[0] = 24; + color[1] = 112; + color[2] = 200; + break; + case 10: +//Gold + color[0] = 200; + color[1] = 168; + color[2] = 0; + break; + case 11: +//GoldenRod + color[0] = 170; + color[1] = 130; + color[2] = 24; + break; + case 12: +//Indigo + color[0] = 116; + color[1] = 0; + color[2] = 204; + break; + case 13: +//LightGreen + color[0] = 112; + color[1] = 186; + color[2] = 112; + break; + + default: +// White (if enough voltage) + color[0] = 200; + color[1] = 200; + color[2] = 200; + break; + } +} //getColor + +#ifdef JUKEBOX +void JukeBox_Stroboscope() { + +} #endif + + #endif #if defined NEOPIXEL @@ -559,11 +525,32 @@ extern bool isFlickering; extern cRGB currentColor; +void neopixels_stripeKillKey_Enable() { + // cut power to the neopixels stripes by disconnecting their GND signal using the LS pins + digitalWrite(3, LOW); + digitalWrite(5, LOW); + digitalWrite(6, LOW); + digitalWrite(9, LOW); + digitalWrite(10, LOW); + digitalWrite(11, LOW); + digitalWrite(DATA_PIN,HIGH); // in order not to back-connect GND over the Data pin to the stripes when the Low-Sides disconnect it +} + +void neopixels_stripeKillKey_Disable() { + // cut power to the neopixels stripes by disconnecting their GND signal using the LS pins + digitalWrite(3, HIGH); + digitalWrite(5, HIGH); + digitalWrite(6, HIGH); + digitalWrite(9, HIGH); + digitalWrite(10, HIGH); + digitalWrite(11, HIGH); +} + void lightOn(cRGB color, int16_t pixel) { // Light On isFlickering = true; if (pixel == -1) { - for (uint8_t i = 0; i < NUMPIXELS; i++) { + for (uint16_t i = 0; i < NUMPIXELS; i++) { pixels.set_crgb_at(i, color); } } else { @@ -580,7 +567,7 @@ void lightOff() { value.b = 0; value.g = 0; value.r = 0; // RGB Value -> Off - for (uint8_t i = 0; i < NUMPIXELS; i++) { + for (uint16_t i = 0; i < NUMPIXELS; i++) { pixels.set_crgb_at(i, value); } pixels.sync(); @@ -595,13 +582,15 @@ void lightIgnition(cRGB color, uint16_t time, uint8_t type) { switch (type) { case 0: // Light up the ledstrings Movie-like - for (uint8_t i = 0; i < NUMPIXELS; i++) { + RampNeoPixel(time, true); +/* for (uint16_t i = 0; i < NUMPIXELS; i++) { pixels.set_crgb_at(i, value); i++; pixels.set_crgb_at(i, value); pixels.sync(); - delayMicroseconds(time * 1000 / (NUMPIXELS / 2)); - } + //delay(time/NUMPIXELS); + delayMicroseconds((time * 1000) / NUMPIXELS); + }*/ //Serial.println(pixels.getBrightness()); break; /* @@ -628,7 +617,8 @@ void lightRetract(uint16_t time, uint8_t type) { value.b = 0; value.g = 0; value.r = 0; // RGB Value -> Off - for (uint8_t i = NUMPIXELS; i > 0; i--) { + RampNeoPixel(time, false); + /*for (uint16_t i = NUMPIXELS; i > 0; i--) { //BUG CORRECTION: //Not uint8_t here because Arduino nano clones did go // on an infinite loop for no reason making the board @@ -637,10 +627,8 @@ void lightRetract(uint16_t time, uint8_t type) { i--; pixels.set_crgb_at(i, value); pixels.sync(); - delayMicroseconds(time * 1000 / (NUMPIXELS / 2)); - } - pixels.set_crgb_at(0, value); - pixels.sync(); + delayMicroseconds((time * 1000) / NUMPIXELS); + }*/ break; /* case 1: @@ -657,27 +645,94 @@ void lightRetract(uint16_t time, uint8_t type) { break; */ } +#ifdef FIREBLADE +#ifdef CROSSGUARDSABER + for(unsigned int j=0; ji){ + //else if ((jpixel+i)){ + pixels.set_crgb_at(pixel-j, currentColor); + pixels.set_crgb_at(pixel+j, currentColor); + } + else { // j variation max 280 + //uint16_t amplitude = variation; + + for (uint16_t i = 0; i <= variation; i++) { + pixels.set_crgb_at(i, color); + } + for (uint16_t i = variation+1; i <= NUMPIXELS; i++) { + tempcolor.b = 0; + tempcolor.g = 0; + tempcolor.r = 0; // RGB Value -> Off + pixels.set_crgb_at(i, tempcolor); + } + pixels.sync(); +*/ +} +#endif + +void RampNeoPixel(uint16_t RampDuration, bool DirectionUpDown) { + cRGB value; +#ifdef FIREBLADE + for (unsigned int i=0; ii) or (!DirectionUpDown and j>NUMPIXELS-1-i)){ + value.r=0; + value.g=0; + value.b=0; + //heat[j]=0; + pixels.set_crgb_at(j, value); // Set value at LED found at index j + } + } + pixels.sync(); // Sends the data to the LEDs + } +#else + for (uint8_t i=0; ii) or (!DirectionUpDown and j>NUMPIXELS-1-i)){ + value.r=0; + value.g=0; + value.b=0; + } + pixels.set_crgb_at(j, value); + } + pixels.sync(); // Sends the data to the LEDs + delay(5); + //delayMicroseconds(RampDuration * (1000 / NUMPIXELS)); + } +#endif +} + +#ifdef FIREBLADE +void FireBlade() { +// Array of temperature readings at each simulation cell + int pixelnumber; + + // Step 1. Cool down every cell a little +#ifdef CROSSGUARDSABER + for( int i = 0; i < MN_STRIPE; i++) { + heat[i] = constrain(heat[i] - random(((Fire_Cooling * 10) / MN_STRIPE) + 2),0,255); + } + for( int i = 0; i < CG_STRIPE; i++) { + heat_cg[i] = constrain(heat_cg[i] - random(5),0,255); + } #else + for( int i = 0; i < NUMPIXELS; i++) { + heat[i] = constrain(heat[i] - random(((Fire_Cooling * 10) / NUMPIXELS) + 2),0,255); + } +#endif + // Step 2. Heat from each cell drifts 'up' and diffuses a little +#ifdef CROSSGUARDSABER + for( int k= MN_STRIPE - 1; k >= 2; k--) { + heat[k] = (heat[k - 1] + heat[k - 2] + heat[k - 2] ) / 3; + } + for( int k= CG_STRIPE - 1; k >= 2; k--) { + heat_cg[k] = (heat_cg[k - 1] + heat_cg[k - 2] + heat_cg[k - 2] ) / 3; + } +#else + for( int k= NUMPIXELS - 1; k >= 2; k--) { + heat[k] = (heat[k - 1] + heat[k - 2] + heat[k - 2] ) / 3; + } +#endif + + // Step 3. Randomly ignite new 'sparks' of heat near the bottom +#ifdef CROSSGUARDSABER + if( random(255) < Fire_Sparking ) { + int y = random(7); + heat[y] = constrain(heat[y] + random(95)+160,0,255 ); + } + if( random(255) < 10 ) { + int y = random(4); + heat_cg[y] = constrain(heat_cg[0] + random(95)+160,0,255 ); + } +#else + if( random(255) < Fire_Sparking ) { + int y = random(7); + heat[y] = constrain(heat[y] + random(95)+160,0,255 ); + } +#endif + + // Step 4. Map from heat cells to LED colors +#ifdef CROSSGUARDSABER + for( int j = 0; j < CG_STRIPE; j++) { + cRGB color = HeatColor( heat_cg[j]); + int pixelnumber; + if( gReverseDirection ) { + pixelnumber = (CG_STRIPE-1) - j; + } else { + pixelnumber = j; + } + LED.set_crgb_at(pixelnumber, color); // Set value at LED found at index j + } + for( int j = CG_STRIPE; j < CG_STRIPE + MN_STRIPE; j++) { + cRGB color = HeatColor( heat[j]); + int pixelnumber; + if( gReverseDirection ) { + pixelnumber = (CG_STRIPE + MN_STRIPE-1) - j; + } else { + pixelnumber = j; + } + pixels.set_crgb_at(pixelnumber, color); // Set value at LED found at index j + } +#else + for( int j = 0; j < NUMPIXELS; j++) { + cRGB color = HeatColor( heat[j]); + int pixelnumber; + if( gReverseDirection ) { + pixelnumber = (NUMPIXELS-1) - j; + } else { + pixelnumber = j; + } + pixels.set_crgb_at(pixelnumber, color); // Set value at LED found at index j + } +#endif +} + +// CRGB HeatColor( uint8_t temperature) +// +// Approximates a 'black body radiation' spectrum for +// a given 'heat' level. This is useful for animations of 'fire'. +// Heat is specified as an arbitrary scale from 0 (cool) to 255 (hot). +// This is NOT a chromatically correct 'black body radiation' +// spectrum, but it's surprisingly close, and it's fast and small. +// +// On AVR/Arduino, this typically takes around 70 bytes of program memory, +// versus 768 bytes for a full 256-entry RGB lookup table. + +cRGB HeatColor( uint8_t temperature) +{ + cRGB heatcolor; + + // Scale 'heat' down from 0-255 to 0-191, + // which can then be easily divided into three + // equal 'thirds' of 64 units each. + uint8_t t192 = scale8_video( temperature, 192); + //Serial.print(F("scale8_video_result: ")); + //Serial.print(temperature);Serial.print("/t");Serial.println(t192); + + // calculate a value that ramps up from + // zero to 255 in each 'third' of the scale. + uint8_t heatramp = t192 & 0x3F; // 0..63 + heatramp <<= 2; // scale up to 0..252 + + // now figure out which third of the spectrum we're in: + if( t192 & 0x80) { + // we're in the hottest third + heatcolor.r = 255; // full red + heatcolor.g = 255; // full green + heatcolor.b = heatramp; // ramp up blue + + } else if( t192 & 0x40 ) { + // we're in the middle third + heatcolor.r = 255; // full red + heatcolor.g = heatramp; // ramp up green + heatcolor.b = 0; // no blue + + } else { + // we're in the coolest third + heatcolor.r = heatramp; // ramp up red + heatcolor.g = 0; // no green + heatcolor.b = 0; // no blue + } + + return heatcolor; +} + +uint8_t scale8_video( uint8_t i, uint8_t scale) +{ +// uint8_t j = (((int)i * (int)scale) >> 8) + ((i&&scale)?1:0); +// // uint8_t nonzeroscale = (scale != 0) ? 1 : 0; +// // uint8_t j = (i == 0) ? 0 : (((int)i * (int)(scale) ) >> 8) + nonzeroscale; +// return j; + uint8_t j=0; + asm volatile( + " tst %[i]\n\t" + " breq L_%=\n\t" + " mul %[i], %[scale]\n\t" + " mov %[j], r1\n\t" + " clr __zero_reg__\n\t" + " cpse %[scale], r1\n\t" + " subi %[j], 0xFF\n\t" + "L_%=: \n\t" + : [j] "+a" (j) + : [i] "a" (i), [scale] "a" (scale) + : "r0", "r1"); + + return j; +} #endif + +#else // blade type + +#endif // blade type + diff --git a/Light.h b/Light.h index a1b1c62..b15c3e6 100644 --- a/Light.h +++ b/Light.h @@ -1,8 +1,8 @@ /* * Light.h * - * Created on: 6 mars 2016 - * author: Sebastien CAPOU (neskweek@gmail.com) + * Created on: 21 Octber 2016 + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS */ @@ -31,8 +31,10 @@ void lightRetract(uint8_t ledPins[], uint16_t time, uint8_t type); void FoCOn (uint8_t pin); void FoCOff (uint8_t pin); -void lightFlicker(uint8_t ledPins[], uint8_t type, uint8_t value = 0); - +void lightFlicker(uint8_t ledPins[], uint8_t type, uint8_t value = 0,uint8_t AState=0); + #ifdef JUKEBOX + void JukeBox_Stroboscope(uint8_t ledPins[]); + #endif #endif #if defined LUXEON @@ -44,17 +46,17 @@ void lightRetract(uint8_t ledPins[], uint8_t color[], uint16_t time); void lightFlicker(uint8_t ledPins[], uint8_t color[], uint8_t value = 0); - -#if defined MY_OWN_COLORS void getColor(uint8_t color[], uint8_t colorID); //getColor -#endif -#if defined FIXED_RANGE_COLORS -void getColor(uint8_t color[], uint16_t colorID); //getColor + +#ifdef JUKEBOX + void JukeBox_Stroboscope(); #endif #endif #if defined NEOPIXEL +void neopixels_stripeKillKey_Enable(); +void neopixels_stripeKillKey_Disable(); void lightOn(cRGB color,int16_t pixel = -1); void lightOff(); @@ -62,12 +64,24 @@ void lightOff(); void lightIgnition(cRGB color, uint16_t time, uint8_t type); void lightRetract( uint16_t time, uint8_t type); -void lightBlasterEffect( uint8_t pixel, uint8_t range); -void lightFlicker( uint8_t value = 0); +void lightBlasterEffect( uint8_t pixel, uint8_t range, uint8_t SndFnt_MainColor); +void lightFlicker( uint8_t value = 0,uint8_t AState=0); void getColor(uint8_t colorID); //getColor +void RampNeoPixel(uint16_t RampDuration, bool DirectionUpDown); + +#ifdef FIREBLADE +void FireBlade(); +cRGB HeatColor( uint8_t temperature); +uint8_t scale8_video( uint8_t i, uint8_t scale); +#endif + +#ifdef JUKEBOX +void JukeBox_Stroboscope(cRGB color); +#endif #endif #endif /* LIGHT_H_ */ + diff --git a/LightSaberOS.ino b/LightSaberOS.ino index 7dd4240..2e03031 100644 --- a/LightSaberOS.ino +++ b/LightSaberOS.ino @@ -1,23 +1,36 @@ /* - * LightSaberOS V1.0 + * LightSaberOS V1.3 * - * released on: 10 mar 2016 - * author: Sebastien CAPOU (neskweek@gmail.com) + * released on: 21 Octber 2016 + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) * Source : https://github.com/neskweek/LightSaberOS * Description: Operating System for Arduino based LightSaber * * This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. * To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/. */ +/*************************************************************************************************** + * DFPLAYER variables + */ +//#define OLD_DPFPLAYER_LIB +#ifdef OLD_DPFPLAYER_LIB + #include // interestingly the DFPlayer lib refuses + #include "DFPlayer_Mini_Mp3.h" + //SoftwareSerial mp3player(DFPLAYER_TX, DFPLAYER_RX); // TX, RX + SoftwareSerial mp3player(7, 8); // TX, RX +#else + #include + DFPlayer dfplayer; +#endif #include -#include #include #include #include #include #include + #include "Buttons.h" #include "Config.h" #include "ConfigMenu.h" @@ -31,24 +44,29 @@ #include #endif -/* - * DO NOT MODIFY - * Unless you know what you're doing - *************************************/ -#define CONFIG_VERSION "L01" -#define MEMORYBASE 32 -#define SWING_SUPPRESS 420 -/************************************/ -/* - * DEFAULT CONFIG PARAMETERS - * Will be overriden by EEPROM settings - * once the first save will be done - *************************************/ -#define VOL 13 -#define SOUNDFONT 2 -#define SWING 300 -/************************************/ + +SoundFont soundFont; +unsigned long sndSuppress = millis(); +unsigned long sndSuppress2 = millis(); +bool hum_playing=false; // variable to store whether hum is being played +#ifdef JUKEBOX +bool jukebox_play=false; // indicate whether a song is being played in JukeBox mode +uint8_t jb_track; // sound file track number in the directory designated for music playback +#endif + + + + +/*************************************************************************************************** + * Saber Finite State Machine Custom Type and State Variable + */ +enum SaberStateEnum {S_STANDBY, S_SABERON, S_CONFIG, S_SLEEP, S_JUKEBOX}; +SaberStateEnum SaberState; +SaberStateEnum PrevSaberState; + +enum ActionModeSubStatesEnum {AS_HUM, AS_IGNITION, AS_RETRACTION, AS_BLADELOCKUP, AS_BLASTERDEFLECTMOTION, AS_BLASTERDEFLECTPRESS, AS_CLASH, AS_SWING, AS_SPIN, AS_FORCE}; +ActionModeSubStatesEnum ActionModeSubStates; /*************************************************************************************************** * Motion detection Variables @@ -106,29 +124,21 @@ struct softPWM { #endif #endif uint8_t blaster = 0; -bool blasterBlocks = false; +//bool blasterBlocks = false; uint8_t clash = 0; -bool lockup = false; +//bool lockup = false; uint8_t blink = 0; uint8_t randomBlink = 0; - /*************************************************************************************************** * Buttons variables */ OneButton mainButton(MAIN_BUTTON, true); OneButton lockupButton(LOCKUP_BUTTON, true); -bool actionMode = false; // Play with your saber -bool configMode = false; // Configure your saber -static bool ignition = false; -static bool browsing = false; - -/*************************************************************************************************** - * DFPLAYER variables - */ -DFPlayer dfplayer; -SoundFont soundFont; -unsigned long sndSuppress = millis(); -unsigned long sndSuppress2 = millis(); +// replaced by Saber State Machine Variables +//bool actionMode = false; // Play with your saber +//bool configMode = false; // Configure your saber +//static bool ignition = false; +//static bool browsing = false; /*************************************************************************************************** * ConfigMode Variables @@ -146,12 +156,8 @@ struct StoreStruct { // This is for mere detection if they are our settings char version[5]; // The settings - uint8_t volume; // 0 to 30 + uint8_t volume; // 0 to 31 uint8_t soundFont; // as many Sound font you have defined in Soundfont.h Max:253 - uint16_t swingTreshold; // treshold acceleration for Swing - uint8_t sndProfile[SOUNDFONT_QUANTITY + 2][3]; // sndProfile[sndft][0] : PowerOn effect - // sndProfile[sndft][1] : PowerOff effect - // sndProfile[sndft][2] : Flicker effect } storage; #endif #if defined LUXEON @@ -159,13 +165,13 @@ struct StoreStruct { // This is for mere detection if they are our settings char version[5]; // The settings - uint8_t volume;// 0 to 30 + uint8_t volume;// 0 to 31 uint8_t soundFont;// as many as Sound font you have defined in Soundfont.h Max:253 - uint16_t swingTreshold;// treshold acceleration for Swing - uint8_t mainColor;//colorID - uint8_t clashColor;//colorID - uint8_t sndProfile[SOUNDFONT_QUANTITY + 2][2]; // sndProfile[sndft][0] : main colorID - // sndProfile[sndft][1] : clash colorID + struct Profile { + uint8_t mainColor; //colorID + uint8_t clashColor;//colorID + uint8_t blasterboltColor; + }sndProfile[SOUNDFONT_QUANTITY + 2]; }storage; #endif @@ -174,16 +180,13 @@ struct StoreStruct { // This is for mere detection if they are our settings char version[5]; // The settings - uint8_t volume;// 0 to 30 + uint8_t volume;// 0 to 31 uint8_t soundFont;// as many as Sound font you have defined in Soundfont.h Max:253 - uint16_t swingTreshold;// treshold acceleration for Swing - struct Profile { - uint8_t mainColor; //colorID - uint8_t clashColor;//colorID - uint8_t pwrOn; - uint8_t pwrOff; - uint8_t flicker; - }sndProfile[SOUNDFONT_QUANTITY + 2]; + struct Profile { + uint8_t mainColor; //colorID + uint8_t clashColor;//colorID + uint8_t blasterboltColor; + }sndProfile[SOUNDFONT_QUANTITY + 2]; }storage; #endif @@ -207,51 +210,38 @@ void setup() { #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif - -//#if defined LS_INFO // Serial line for debug - Serial.begin(9600); -//#endif + Serial.begin(115200); + -#if defined LS_DEBUG - // Serial line for debug - Serial.begin(9600); -#endif /***** LOAD CONFIG *****/ // Get config from EEPROM if there is one // or initialise value with default ones set in StoreStruct EEPROM.setMemPool(MEMORYBASE, EEPROMSizeATmega328); //Set memorypool base to 32, assume Arduino Uno board configAdress = EEPROM.getAddress(sizeof(StoreStruct)); // Size of config object - + if (!loadConfig()) { - for (uint8_t i = 0; i <= 3; i++) + for (uint8_t i = 0; i <= 2; i++) storage.version[i] = CONFIG_VERSION[i]; storage.soundFont = SOUNDFONT; storage.volume = VOL; - storage.swingTreshold = SWING; #if defined LEDSTRINGS - storage.sndProfile[2][0] = 0; //PowerOn - storage.sndProfile[2][1] = 0; //PowerOff - storage.sndProfile[2][2] = 0; //Flickering - storage.sndProfile[3][0] = 1; - storage.sndProfile[3][1] = 1; - storage.sndProfile[3][2] = 0; #endif #if defined LUXEON - storage.mainColor = 4; - storage.clashColor = 5; - storage.sndProfile[2][0] = 2; - storage.sndProfile[2][1] = 0; - storage.sndProfile[3][0] = 0; - storage.sndProfile[3][1] = 5; + for (uint8_t i=2; i 60 and mpuIntStatus < 70 and not lockup) { - /* - * THIS IS A CLASH ! - */ -#if defined LUXEON - getColor(currentColor, storage.clashColor); - lightOn(ledPins, currentColor); -#endif + if (mpuIntStatus > 60 and mpuIntStatus < 70 and ActionModeSubStates != AS_BLADELOCKUP) { + #if defined LS_CLASH_DEBUG Serial.print(F("CLASH\tmpuIntStatus=")); Serial.println(mpuIntStatus); #endif - if (millis() - sndSuppress >= 100) { - blink = 0; - clash = CLASH_FLASH_TIME; - dfplayer.playPhysicalTrack(soundFont.getClash()); + if (millis() - sndSuppress >= CLASH_SUPRESS) { + //blink = 0; + //clash = CLASH_FLASH_TIME; + SinglePlay_Sound(soundFont.getClash()); sndSuppress = millis(); sndSuppress2 = millis(); - } + /* + * THIS IS A CLASH ! + */ + ActionModeSubStates=AS_CLASH; + #if defined LUXEON + getColor(currentColor, storage.sndProfile[storage.soundFont].clashColor); + lightOn(ledPins, currentColor); + #endif + #if defined LEDSTRINGS + for (uint8_t i = 0; i <= 5; i++) { + analogWrite(ledPins[i], 255); + } + #endif + #if defined NEOPIXEL + #ifdef FIREBLADE // simply flash white + getColor(14); + lightOn(currentColor); + #else + getColor(storage.sndProfile[storage.soundFont].clashColor); + lightOn(currentColor); + #endif + #endif + delay(CLASH_FX_DURATION); // clash duration + + } } + /* + * SIMPLE BLADE MOVEMENT DETECTION FOR MOTION TRIGGERED BLASTER FEDLECT + * We detect swings as hilt's orientation change + * since IMUs sucks at determining relative position in space + */ + // movement of the hilt while blaster move deflect is activated can trigger a blaster deflect + else if ((ActionModeSubStates==AS_BLASTERDEFLECTPRESS or (ActionModeSubStates==AS_BLASTERDEFLECTMOTION and (abs(curDeltAccel.y) > soundFont.getSwingThreshold() // and it has suffisent power on a certain axis + or abs(curDeltAccel.z) > soundFont.getSwingThreshold() + or abs(curDeltAccel.x) > soundFont.getSwingThreshold()))) and (millis() - sndSuppress >= BLASTERBLOCK_SUPRESS)) { + + if (soundFont.getBlaster()) { + SinglePlay_Sound(soundFont.getBlaster()); +#if defined LEDSTRINGS + blasterPin = random(6); //momentary shut off one led segment + blink = 0; + analogWrite(ledPins[blasterPin], LOW); +#endif +#if defined LUXEON + getColor(currentColor, storage.sndProfile[storage.soundFont].blasterboltColor); + lightOn(ledPins, currentColor); +#endif //LUXEON +#if defined NEOPIXEL + #ifdef FIREBLADE + getColor(14); + lightOn(currentColor); + #else + blasterPixel = random(20, NUMPIXELS - 3); //momentary shut off one led segment + blink = 0; + getColor(storage.sndProfile[storage.soundFont].blasterboltColor); + lightBlasterEffect(blasterPixel, 3, storage.sndProfile[storage.soundFont].mainColor); + #endif +#endif + delay(BLASTER_FX_DURATION); // blaster bolt deflect duration + blaster = BLASTER_FLASH_TIME; + // Some Soundfont may not have Blaster sounds + if (millis() - sndSuppress > 50) { + //SinglePlay_Sound(soundFont.getBlaster()); + sndSuppress = millis(); + } + } + } /* * SWING DETECTION * We detect swings as hilt's orientation change * since IMUs sucks at determining relative position in space */ else if ( - not lockup + ActionModeSubStates != AS_BLADELOCKUP and abs(curRotation.w * 1000) < 999 // some rotation movement have been initiated and ( #if defined BLADE_X ( (millis() - sndSuppress > SWING_SUPPRESS) // The movement doesn't follow another to closely - and (abs(curDeltAccel.y) > storage.swingTreshold // and it has suffisent power on a certain axis - or abs(curDeltAccel.z) > storage.swingTreshold - or abs(curDeltAccel.x) > storage.swingTreshold*10) + and (abs(curDeltAccel.y) > soundFont.getSwingThreshold() // and it has suffisent power on a certain axis + or abs(curDeltAccel.z) > soundFont.getSwingThreshold() + or abs(curDeltAccel.x) > soundFont.getSwingThreshold()*10) ) or (// A reverse movement follow a first one (millis() - sndSuppress2 > SWING_SUPPRESS) // The reverse movement doesn't follow another reverse movement to closely // and it must be a reverse movement on Vertical axis and ( abs(curDeltAccel.y) > abs(curDeltAccel.z) - and abs(prevDeltAccel.y) > storage.swingTreshold + and abs(prevDeltAccel.y) > soundFont.getSwingThreshold() and ( (prevDeltAccel.y > 0 - and curDeltAccel.y < -storage.swingTreshold) + and curDeltAccel.y < -soundFont.getSwingThreshold()) or ( prevDeltAccel.y < 0 - and curDeltAccel.y > storage.swingTreshold + and curDeltAccel.y > soundFont.getSwingThreshold() ) ) ) @@ -617,13 +689,13 @@ void loop() { (millis() - sndSuppress2 > SWING_SUPPRESS) // The reverse movement doesn't follow another reverse movement to closely and ( // and it must be a reverse movement on Horizontal axis abs(curDeltAccel.z) > abs(curDeltAccel.y) - and abs(prevDeltAccel.z) > storage.swingTreshold + and abs(prevDeltAccel.z) > soundFont.getSwingThreshold() and ( (prevDeltAccel.z > 0 - and curDeltAccel.z < -storage.swingTreshold) + and curDeltAccel.z < -soundFont.getSwingThreshold()) or ( prevDeltAccel.z < 0 - and curDeltAccel.z > storage.swingTreshold + and curDeltAccel.z > soundFont.getSwingThreshold() ) ) ) @@ -641,22 +713,22 @@ void loop() { #if defined BLADE_Y ( (millis() - sndSuppress > SWING_SUPPRESS) // The movement doesn't follow another to closely - and (abs(curDeltAccel.x) > storage.swingTreshold // and it has suffisent power on a certain axis - or abs(curDeltAccel.z) > storage.swingTreshold - or abs(curDeltAccel.y) > storage.swingTreshold*10) + and (abs(curDeltAccel.x) > soundFont.getSwingThreshold() // and it has suffisent power on a certain axis + or abs(curDeltAccel.z) > soundFont.getSwingThreshold() + or abs(curDeltAccel.y) > soundFont.getSwingThreshold()*10) ) or (// A reverse movement follow a first one (millis() - sndSuppress2 > SWING_SUPPRESS) // The reverse movement doesn't follow another reverse movement to closely // and it must be a reverse movement on Vertical axis and ( abs(curDeltAccel.x) > abs(curDeltAccel.z) - and abs(prevDeltAccel.x) > storage.swingTreshold + and abs(prevDeltAccel.x) > soundFont.getSwingThreshold() and ( (prevDeltAccel.x > 0 - and curDeltAccel.x < -storage.swingTreshold) + and curDeltAccel.x < -soundFont.getSwingThreshold()) or ( prevDeltAccel.x < 0 - and curDeltAccel.x > storage.swingTreshold + and curDeltAccel.x > soundFont.getSwingThreshold() ) ) ) @@ -665,13 +737,13 @@ void loop() { (millis() - sndSuppress2 > SWING_SUPPRESS) // The reverse movement doesn't follow another reverse movement to closely and ( // and it must be a reverse movement on Horizontal axis abs(curDeltAccel.z) > abs(curDeltAccel.x) - and abs(prevDeltAccel.z) > storage.swingTreshold + and abs(prevDeltAccel.z) > soundFont.getSwingThreshold() and ( (prevDeltAccel.z > 0 - and curDeltAccel.z < -storage.swingTreshold) + and curDeltAccel.z < -soundFont.getSwingThreshold()) or ( prevDeltAccel.z < 0 - and curDeltAccel.z > storage.swingTreshold + and curDeltAccel.z > soundFont.getSwingThreshold() ) ) ) @@ -688,22 +760,22 @@ void loop() { #if defined BLADE_Z ( (millis() - sndSuppress > SWING_SUPPRESS) // The movement doesn't follow another to closely - and (abs(curDeltAccel.y) > storage.swingTreshold // and it has suffisent power on a certain axis - or abs(curDeltAccel.x) > storage.swingTreshold - or abs(curDeltAccel.z) > storage.swingTreshold*10) + and (abs(curDeltAccel.y) > soundFont.getSwingThreshold() // and it has suffisent power on a certain axis + or abs(curDeltAccel.x) > soundFont.getSwingThreshold() + or abs(curDeltAccel.z) > soundFont.getSwingThreshold()*10) ) or (// A reverse movement follow a first one (millis() - sndSuppress2 > SWING_SUPPRESS) // The reverse movement doesn't follow another reverse movement to closely // and it must be a reverse movement on Vertical axis and ( abs(curDeltAccel.y) > abs(curDeltAccel.x) - and abs(prevDeltAccel.y) > storage.swingTreshold + and abs(prevDeltAccel.y) > soundFont.getSwingThreshold() and ( (prevDeltAccel.y > 0 - and curDeltAccel.y < -storage.swingTreshold) + and curDeltAccel.y < -soundFont.getSwingThreshold()) or ( prevDeltAccel.y < 0 - and curDeltAccel.y > storage.swingTreshold + and curDeltAccel.y > soundFont.getSwingThreshold() ) ) ) @@ -712,13 +784,13 @@ void loop() { (millis() - sndSuppress2 > SWING_SUPPRESS) // The reverse movement doesn't follow another reverse movement to closely and ( // and it must be a reverse movement on Horizontal axis abs(curDeltAccel.x) > abs(curDeltAccel.y) - and abs(prevDeltAccel.x) > storage.swingTreshold + and abs(prevDeltAccel.x) > soundFont.getSwingThreshold() and ( (prevDeltAccel.x > 0 - and curDeltAccel.x < -storage.swingTreshold) + and curDeltAccel.x < -soundFont.getSwingThreshold()) or ( prevDeltAccel.x < 0 - and curDeltAccel.x > storage.swingTreshold + and curDeltAccel.x > soundFont.getSwingThreshold() ) ) ) @@ -732,13 +804,13 @@ void loop() { abs(prevRotation.z * 1000 - curRotation.z * 1000) > abs(prevRotation.x * 1000 - curRotation.x * 1000) ) #endif -){ +){ // end of the condition definition for swings - if (!blasterBlocks) { + if ( ActionModeSubStates != AS_BLASTERDEFLECTMOTION and ActionModeSubStates != AS_BLASTERDEFLECTPRESS) { /* - * THIS IS A SWING ! + * THIS IS A SWING ! */ prevDeltAccel = curDeltAccel; #if defined LS_SWING_DEBUG @@ -787,28 +859,28 @@ void loop() { ( abs(curDeltAccel.y) > abs(curDeltAccel.z) - and abs(prevDeltAccel.y) > storage.swingTreshold + and abs(prevDeltAccel.y) > soundFont.getSwingThreshold() and ( ( prevDeltAccel.y > 0 - and curDeltAccel.y > storage.swingTreshold) + and curDeltAccel.y > soundFont.getSwingThreshold()) or ( prevDeltAccel.y < 0 - and curDeltAccel.y < -storage.swingTreshold + and curDeltAccel.y < -soundFont.getSwingThreshold() ) ) ) or ( // and it must be a reverse movement on Horizontal axis abs(curDeltAccel.z) > abs(curDeltAccel.y) - and abs(prevDeltAccel.z) > storage.swingTreshold + and abs(prevDeltAccel.z) > soundFont.getSwingThreshold() and ( ( prevDeltAccel.z > 0 - and curDeltAccel.z > storage.swingTreshold) + and curDeltAccel.z > soundFont.getSwingThreshold()) or ( prevDeltAccel.z < 0 - and curDeltAccel.z < -storage.swingTreshold + and curDeltAccel.z < -soundFont.getSwingThreshold() ) ) ) @@ -818,28 +890,28 @@ void loop() { #if defined BLADE_Y ( abs(curDeltAccel.x) > abs(curDeltAccel.z) - and abs(prevDeltAccel.x) > storage.swingTreshold + and abs(prevDeltAccel.x) > soundFont.getSwingThreshold() and ( ( prevDeltAccel.x > 0 - and curDeltAccel.x > storage.swingTreshold) + and curDeltAccel.x > soundFont.getSwingThreshold()) or ( prevDeltAccel.x < 0 - and curDeltAccel.x < -storage.swingTreshold + and curDeltAccel.x < -soundFont.getSwingThreshold() ) ) ) or ( // and it must be a reverse movement on Horizontal axis abs(curDeltAccel.z) > abs(curDeltAccel.x) - and abs(prevDeltAccel.z) > storage.swingTreshold + and abs(prevDeltAccel.z) > soundFont.getSwingThreshold() and ( ( prevDeltAccel.z > 0 - and curDeltAccel.z > storage.swingTreshold) + and curDeltAccel.z > soundFont.getSwingThreshold()) or ( prevDeltAccel.z < 0 - and curDeltAccel.z < -storage.swingTreshold + and curDeltAccel.z < -soundFont.getSwingThreshold() ) ) ) @@ -848,28 +920,28 @@ void loop() { #if defined BLADE_Z ( abs(curDeltAccel.y) > abs(curDeltAccel.x) - and abs(prevDeltAccel.y) > storage.swingTreshold + and abs(prevDeltAccel.y) > soundFont.getSwingThreshold() and ( ( prevDeltAccel.y > 0 - and curDeltAccel.y > storage.swingTreshold) + and curDeltAccel.y > soundFont.getSwingThreshold()) or ( prevDeltAccel.y < 0 - and curDeltAccel.y < -storage.swingTreshold + and curDeltAccel.y < -soundFont.getSwingThreshold() ) ) ) or ( // and it must be a reverse movement on Horizontal axis abs(curDeltAccel.x) > abs(curDeltAccel.y) - and abs(prevDeltAccel.x) > storage.swingTreshold + and abs(prevDeltAccel.x) > soundFont.getSwingThreshold() and ( ( prevDeltAccel.x > 0 - and curDeltAccel.x > storage.swingTreshold) + and curDeltAccel.x > soundFont.getSwingThreshold()) or ( prevDeltAccel.x < 0 - and curDeltAccel.x < -storage.swingTreshold + and curDeltAccel.x < -soundFont.getSwingThreshold() ) ) ) @@ -878,12 +950,14 @@ void loop() { ){ - - dfplayer.playPhysicalTrack(soundFont.getSpin()); + ActionModeSubStates=AS_SPIN; + SinglePlay_Sound(soundFont.getSpin()); }/* SPIN DETECTION */ else{ /* NORMAL SWING */ - dfplayer.playPhysicalTrack(soundFont.getSwing()); + Serial.println("swing"); + ActionModeSubStates=AS_SWING; + SinglePlay_Sound(soundFont.getSwing()); }/* NORMAL SWING */ @@ -896,45 +970,49 @@ void loop() { sndSuppress2 = millis(); } - } else { /* BLASTER MODE */ - if (soundFont.getBlaster()) { -#if defined LEDSTRINGS - blasterPin = random(6); //momentary shut off one led segment - blink = 0; -#endif -#if defined LUXEON - getColor(currentColor, storage.clashColor); - lightOn(ledPins, currentColor); -#endif //LUXEON -#if defined NEOPIXEL - blasterPixel = random(20, NUMPIXELS - 20); //momentary shut off one led segment - blink = 0; -// getColor(storage.sndProfile[storage.soundFont].clashColor); - getColor(255);//Pure white -#endif - blaster = BLASTER_FLASH_TIME; - // Some Soundfont may not have #endifBlaster sounds - if (millis() - sndSuppress > 50) { - dfplayer.playPhysicalTrack(soundFont.getBlaster()); - sndSuppress = millis(); - } - } - } /* BLASTER MODE */ + } } + else { // simply flicker + if (ActionModeSubStates!=AS_BLASTERDEFLECTMOTION and ActionModeSubStates!=AS_BLADELOCKUP) { // do not deactivate blaster move deflect mode in case the saber is idling + ActionModeSubStates=AS_HUM; + } + // relaunch hum if more than HUM_RELAUNCH time elapsed since entering AS_HUM state + if (millis() - sndSuppress > HUM_RELAUNCH and not hum_playing) { + HumRelaunch(); + } +#ifdef LEDSTRINGS + lightFlicker(ledPins, soundFont.getFlickerEffect(),0,ActionModeSubStates); +#endif + +#ifdef LUXEON + getColor(currentColor, storage.sndProfile[storage.soundFont].mainColor); + lightFlicker(ledPins, currentColor,0); +#endif + +#ifdef NEOPIXEL + getColor(storage.sndProfile[storage.soundFont].mainColor); + lightFlicker(0,ActionModeSubStates); +#endif + } // ************************* blade movement detection ends*********************************** } ////END ACTION MODE HANDLER/////////////////////////////////////////////////////////////////////////////////////// /*////////////////////////////////////////////////////////////////////////////////////////////////////////// * CONFIG MODE HANDLER *////////////////////////////////////////////////////////////////////////////////////////////////////////// - else if (configMode) { - if (!browsing) { - dfplayer.playPhysicalTrack(3); + else if (SaberState==S_CONFIG) { + if (PrevSaberState==S_STANDBY) { // entering config mode + PrevSaberState=S_CONFIG; + SinglePlay_Sound(3); delay(600); + +#if defined NEOPIXEL + neopixels_stripeKillKey_Disable(); +#endif + #if defined LS_INFO Serial.println(F("START CONF")); #endif - browsing = true; enterMenu = true; } @@ -943,74 +1021,75 @@ void loop() { #if defined LS_INFO Serial.print(F("-:")); #endif - dfplayer.playPhysicalTrack(2); + SinglePlay_Sound(2); delay(50); } else if (modification == 1) { #if defined LS_INFO Serial.print(F("+:")); #endif - dfplayer.playPhysicalTrack(1); + SinglePlay_Sound(1); delay(50); } switch (menu) { - case 0: //VOLUME - confMenuStart(storage.volume, 4, dfplayer); - - confParseValue(storage.volume, 0, 30, 1, dfplayer); - - if (modification) { - - modification = 0; - storage.volume = value; - dfplayer.setVolume(storage.volume); // Too Slow: we'll change volume on exit - delay(50); -#if defined LS_INFO - Serial.println(storage.volume); -#endif - } - break; - case 1: // SOUNDFONT - confMenuStart(storage.soundFont, 5, dfplayer); + + case 0: // SOUNDFONT + confMenuStart(storage.soundFont, 5, menu); play = false; - confParseValue(storage.soundFont, 2, SOUNDFONT_QUANTITY + 1, 1, - dfplayer); + confParseValue(storage.soundFont, 2, SOUNDFONT_QUANTITY + 1, 1); if (modification) { modification = 0; storage.soundFont = value; soundFont.setID(value); - dfplayer.playPhysicalTrack(soundFont.getBoot()); + SinglePlay_Sound(soundFont.getMenu()); delay(150); -#if defined LUXEON - storage.mainColor = storage.sndProfile[value][0]; - storage.clashColor = storage.sndProfile[value][1]; -#endif #if defined LS_INFO Serial.println(soundFont.getID()); #endif } break; + case 1: //VOLUME + confMenuStart(storage.volume, 4, menu); + + confParseValue(storage.volume, 0, 31, 1); + + if (modification) { + + modification = 0; + storage.volume = value; +#ifdef OLD_DPFPLAYER_LIB + mp3_set_volume (storage.volume); +#else + dfplayer.setVolume(storage.volume); // Too Slow: we'll change volume on exit +#endif + delay(50); +#if defined LS_INFO + Serial.println(storage.volume); +#endif + } + + break; + #if defined LUXEON case 2: // BLADE MAIN COLOR - confMenuStart(storage.mainColor, 9, dfplayer); + confMenuStart(storage.sndProfile[storage.soundFont].mainColor, 6, menu); - confParseValue(storage.mainColor, 0, COLORS - 1, 1, dfplayer); + confParseValue(storage.sndProfile[storage.soundFont].mainColor, 0, COLORS - 1, 1); if (modification) { modification = 0; - storage.mainColor = value; - storage.sndProfile[storage.soundFont][0] =storage.mainColor; - getColor(currentColor, storage.mainColor); + storage.sndProfile[storage.soundFont].mainColor =value; + getColor(currentColor, storage.sndProfile[storage.soundFont].mainColor); lightOn(ledPins, currentColor); #if defined LS_INFO - Serial.print(storage.mainColor); + Serial.print(storage.sndProfile[storage.soundFont].mainColor); Serial.print("\tR:"); Serial.print(currentColor[0]); Serial.print("\tG:"); @@ -1021,19 +1100,18 @@ void loop() { } break; case 3: //BLADE CLASH COLOR - confMenuStart(storage.clashColor, 10, dfplayer); + confMenuStart(storage.sndProfile[storage.soundFont].clashColor, 7, menu); - confParseValue(storage.clashColor, 0, COLORS - 1, 1, dfplayer); + confParseValue(storage.sndProfile[storage.soundFont].clashColor, 0, COLORS - 1, 1); if (modification) { modification = 0; - storage.clashColor = value; - storage.sndProfile[storage.soundFont][1] =storage.clashColor; - getColor(currentColor, storage.clashColor); + storage.sndProfile[storage.soundFont].clashColor =value; + getColor(currentColor, storage.sndProfile[storage.soundFont].clashColor); lightOn(ledPins, currentColor); #if defined LS_INFO - Serial.print(storage.clashColor); + Serial.print(storage.sndProfile[storage.soundFont].clashColor); Serial.print("\tR:"); Serial.print(currentColor[0]); Serial.print("\tG:"); @@ -1043,71 +1121,38 @@ void loop() { #endif } break; -#endif + case 4: //BLADE BLASTER BLOCK COLOR + confMenuStart(storage.sndProfile[storage.soundFont].blasterboltColor, 8, menu); -#if defined LEDSTRINGS - case 4: // POWERON EFFECT - confMenuStart(storage.sndProfile[storage.soundFont][0], 17, - dfplayer); - - confParseValue(storage.sndProfile[storage.soundFont][0], 0, 1, 1, - dfplayer); + confParseValue(storage.sndProfile[storage.soundFont].blasterboltColor, 0, + COLORS - 1, 1); - if (modification) { + if (modification) { - modification = 0; - storage.sndProfile[storage.soundFont][0] = value; + modification = 0; + storage.sndProfile[storage.soundFont].blasterboltColor = value; + getColor(currentColor, storage.sndProfile[storage.soundFont].blasterboltColor); + lightOn(ledPins, currentColor); #if defined LS_INFO - Serial.println(storage.sndProfile[storage.soundFont][0]); + Serial.print(storage.sndProfile[storage.soundFont].blasterboltColor); + Serial.print("\tR:"); + Serial.print(currentColor[0]); + Serial.print("\tG:"); + Serial.print(currentColor[1]); + Serial.print(" \tB:"); + Serial.println(currentColor[2]); #endif - } - break; - case 5: //POWEROFF EFFECT - confMenuStart(storage.sndProfile[storage.soundFont][1], 18, - dfplayer); - - confParseValue(storage.sndProfile[storage.soundFont][1], 0, 1, 1, - dfplayer); - - if (modification) { - - modification = 0; - storage.sndProfile[storage.soundFont][1] = value; -#if defined LS_INFO - Serial.println(storage.sndProfile[storage.soundFont][1]); + } + break; #endif - } - break; - case 6: //FLICKER EFFECT - confMenuStart(storage.sndProfile[storage.soundFont][2], 19, - dfplayer); - - confParseValue(storage.sndProfile[storage.soundFont][2], 0, 2, 1, - dfplayer); - - if (modification) { - - modification = 0; - storage.sndProfile[storage.soundFont][2] = value; -#if defined LS_INFO - Serial.println(storage.sndProfile[storage.soundFont][2]); -#endif - } - break; -#endif //LEDSTRINGS - - - - /*NEOPIXEL*/ #if defined NEOPIXEL case 2: // BLADE MAIN COLOR - confMenuStart(storage.sndProfile[storage.soundFont].mainColor, 9, - dfplayer); + confMenuStart(storage.sndProfile[storage.soundFont].mainColor, 6, menu); confParseValue(storage.sndProfile[storage.soundFont].mainColor, 0, - COLORS - 1, 1, dfplayer); + COLORS - 1, 1); if (modification) { @@ -1127,11 +1172,10 @@ void loop() { } break; case 3: //BLADE CLASH COLOR - confMenuStart(storage.sndProfile[storage.soundFont].clashColor, 10, - dfplayer); + confMenuStart(storage.sndProfile[storage.soundFont].clashColor, 7, menu); confParseValue(storage.sndProfile[storage.soundFont].clashColor, 0, - COLORS - 1, 1, dfplayer); + COLORS - 1, 1); if (modification) { @@ -1140,7 +1184,7 @@ void loop() { getColor(storage.sndProfile[storage.soundFont].clashColor); lightOn(currentColor); #if defined LS_DEBUG - Serial.print(storage.sndProfile[storage.soundFont].mainColor); + Serial.print(storage.sndProfile[storage.soundFont].clashColor); Serial.print("\tR:"); Serial.print(currentColor.r); Serial.print("\tG:"); @@ -1150,73 +1194,30 @@ void loop() { #endif } break; + case 4: //BLADE BLASTER BLOCK COLOR + confMenuStart(storage.sndProfile[storage.soundFont].blasterboltColor, 8, menu); - case 4: // POWERON EFFECT - - confMenuStart(storage.sndProfile[storage.soundFont].pwrOn, 17, - dfplayer); - - confParseValue(storage.sndProfile[storage.soundFont].pwrOn, 0, 1, 1, - dfplayer); - - if (modification) { - - modification = 0; - storage.sndProfile[storage.soundFont].pwrOn = value; -#if defined LS_INFO - Serial.println(storage.sndProfile[storage.soundFont].pwrOn); -#endif - } - break; - case 5: //POWEROFF EFFECT - confMenuStart(storage.sndProfile[storage.soundFont].pwrOff, 18, - dfplayer); - - confParseValue(storage.sndProfile[storage.soundFont].pwrOff, 0, 1, - 1, dfplayer); + confParseValue(storage.sndProfile[storage.soundFont].blasterboltColor, 0, + COLORS - 1, 1); - if (modification) { + if (modification) { - modification = 0; - storage.sndProfile[storage.soundFont].pwrOff = value; -#if defined LS_INFO - Serial.println(storage.sndProfile[storage.soundFont].pwrOff); -#endif - } - break; - case 6: //FLICKER EFFECT - confMenuStart(storage.sndProfile[storage.soundFont].flicker, 19, - dfplayer); - - confParseValue(storage.sndProfile[storage.soundFont].flicker, 0, 2, - 1, dfplayer); - - if (modification) { - - modification = 0; - storage.sndProfile[storage.soundFont].flicker = value; -#if defined LS_INFO - Serial.println(storage.sndProfile[storage.soundFont].flicker); + modification = 0; + storage.sndProfile[storage.soundFont].blasterboltColor = value; + getColor(storage.sndProfile[storage.soundFont].blasterboltColor); + lightOn(currentColor); +#if defined LS_DEBUG + Serial.print(storage.sndProfile[storage.soundFont].blasterboltColor); + Serial.print("\tR:"); + Serial.print(currentColor.r); + Serial.print("\tG:"); + Serial.print(currentColor.g); + Serial.print(" \tB:"); + Serial.println(currentColor.b); #endif - } - break; -#endif -/*NEOPIXEL*/ - - case 7: //SWING SENSIBILITY - confMenuStart(storage.swingTreshold, 6, dfplayer); - - confParseValue(storage.swingTreshold, 200, 2000, -100, dfplayer); - - if (modification) { - - modification = 0; - storage.swingTreshold = value; -#if defined LS_INFO - Serial.println(storage.swingTreshold); + } + break; #endif - } - break; default: menu = 0; break; @@ -1227,19 +1228,17 @@ void loop() { /*////////////////////////////////////////////////////////////////////////////////////////////////////////// * STANDBY MODE *////////////////////////////////////////////////////////////////////////////////////////////////////////// - else if (!actionMode && !configMode) { + else if (SaberState==S_STANDBY) { - if (ignition) { // we just leaved Action Mode - detachInterrupt(0); -#if defined LIGHT_EFFECTS - TIMSK2 &= ~(1 << OCIE2A); -#endif - dfplayer.playPhysicalTrack(soundFont.getPowerOff()); + if (ActionModeSubStates==AS_RETRACTION) { // we just leaved Action Mode + //detachInterrupt(0); + + SinglePlay_Sound(soundFont.getPowerOff()); + ActionModeSubStates=AS_HUM; changeMenu = false; - ignition = false; - blasterBlocks = false; + //ignition = false; + //blasterBlocks = false; modification = 0; - #if defined LS_INFO Serial.println(F("END ACTION")); #endif @@ -1248,21 +1247,18 @@ void loop() { lightRetract(ledPins, currentColor, soundFont.getPowerOffTime()); #endif #if defined LEDSTRINGS - lightRetract(ledPins, soundFont.getPowerOffTime(), - storage.sndProfile[storage.soundFont][1]); + lightRetract(ledPins, soundFont.getPowerOffTime(), + soundFont.getPowerOffEffect()); #endif #if defined NEOPIXEL - - lightRetract(soundFont.getPowerOffTime(), 0); - for (uint8_t i = 0; i < 3; i++) { - digitalWrite(ledPins[i], LOW); - } - + lightRetract(soundFont.getPowerOffTime(), soundFont.getPowerOffEffect()); + neopixels_stripeKillKey_Enable(); #endif } - if (browsing) { // we just leaved Config Mode + if (PrevSaberState==S_CONFIG) { // we just leaved Config Mode saveConfig(); + PrevSaberState=S_STANDBY; /* * RESET CONFIG @@ -1272,20 +1268,15 @@ void loop() { // EEPROM.update(i, 0); // // } // } -#if defined LUXEON - lightOff(ledPins); -#else - lightOff(); -#endif - dfplayer.playPhysicalTrack(3); - browsing = false; + SinglePlay_Sound(3); + //browsing = false; enterMenu = false; modification = 0; //dfplayer.setVolume(storage.volume); menu = 0; #if defined LUXEON - getColor(currentColor, storage.mainColor); + getColor(currentColor, storage.sndProfile[storage.soundFont].mainColor); #endif #if defined NEOPIXEL getColor(storage.sndProfile[storage.soundFont].mainColor); @@ -1296,6 +1287,13 @@ void loop() { #endif } +// switch of light in Stand-by mode +#if defined LUXEON + lightOff(ledPins); +#else + lightOff(); +#endif + #if defined ACCENT_LED #if defined HARD_ACCENT if (millis() - lastAccent <= 400) { @@ -1320,6 +1318,37 @@ void loop() { #endif } // END STANDBY MODE +#ifdef JUKEBOX + /*////////////////////////////////////////////////////////////////////////////////////////////////////////// + * JUKEBOX MODE (a.k.a. MP3 player mode + *////////////////////////////////////////////////////////////////////////////////////////////////////////// + + else if (SaberState==S_JUKEBOX) { + if (PrevSaberState==S_STANDBY) { // just entered JukeBox mode + PrevSaberState=S_JUKEBOX; + SinglePlay_Sound(14); // play intro sound of JukeBox mode + delay(2500); +#if defined LS_INFO + Serial.println(F("START JUKEBOX")); +#endif + // start playing the first song + jb_track=NR_CONFIGFOLDERFILES+1; + SinglePlay_Sound(jb_track); // JukeBox dir/files must be directly adjecent to config sounds on the SD card + } +#ifdef LEDSTRINGS + JukeBox_Stroboscope(ledPins); +#endif + +#ifdef LUXEON + JukeBox_Stroboscope(); +#endif + +#ifdef NEOPIXEL + getColor(storage.sndProfile[storage.soundFont].mainColor); + JukeBox_Stroboscope(currentColor); +#endif + } +#endif } //loop // ==================================================================================== @@ -1415,6 +1444,7 @@ inline bool loadConfig() { for (uint8_t i = 0; i <= 2; i++) { if (storage.version[i] != CONFIG_VERSION[i]) { equals = false; + Serial.println("Wrong config!"); } } Serial.println(storage.version); @@ -1423,6 +1453,12 @@ inline bool loadConfig() { inline void saveConfig() { EEPROM.updateBlock(configAdress, storage); + #ifdef LS_DEBUG + // dump values stored in EEPROM + for (uint8_t i = 0; i < 255; i++) { + Serial.print(i);Serial.print("\t");Serial.println(EEPROM.readByte(i)); + } + #endif } //saveConfig // ==================================================================================== @@ -1466,244 +1502,75 @@ void fadeAccent() { } } #endif -/* - * If no other interrupt has been triggered, and if my calculation are right - * this timer has an almost 44100 khz frequency triggering : - * each 22 \B5s this method is called and modifies the blade brightness - * The parameter is defined in ignition block - */ -#if defined LIGHT_EFFECTS -ISR(TIMER2_COMPA_vect, ISR_NOBLOCK) { - -#ifdef LEDSTRINGS - lightFlicker(ledPins, storage.sndProfile[storage.soundFont][2]); -#endif -#ifdef LUXEON - lightFlicker(ledPins, currentColor,0); -#endif - if (clash) { - - if (blink == 0) { -#if defined LUXEON - getColor(currentColor, storage.clashColor); - lightOn(ledPins, currentColor); -#endif -#if defined NEOPIXEL - if (not isFlickering) { - getColor(storage.sndProfile[storage.soundFont].clashColor); - lightOn(currentColor); - } -#endif -#if defined FoCSTRING - FoCOn(FoCSTRING); -#endif - blink++; - } else if (blink < 14) { - blink++; -#if defined LEDSTRINGS - lightFlicker(ledPins, storage.sndProfile[storage.soundFont][2], - MAX_BRIGHTNESS - (blink / 2)); -#endif -#if defined LUXEON - lightFlicker(ledPins, currentColor, MAX_BRIGHTNESS - (blink / 2)); -#endif - - } else if (blink == 14) { -#if defined LUXEON - getColor(currentColor, storage.mainColor); - lightOn(ledPins, currentColor); -#endif -#if defined NEOPIXEL - if (not isFlickering) { - getColor(storage.sndProfile[storage.soundFont].mainColor); - lightOn(currentColor); - } -#endif -#if defined FoCSTRING - FoCOff(FoCSTRING); -#endif - blink = 0; - clash = 0; - } - } else if (lockup) { - uint8_t brightness = 0; - - if (blink == 0) { - brightness = random(MAX_BRIGHTNESS - 10, MAX_BRIGHTNESS); - randomBlink = random(7, 15); - blink++; -#if defined FoCSTRING - FoCOn(FoCSTRING); -#endif -#if defined LUXEON - getColor(currentColor, storage.clashColor); - lightOn(ledPins, currentColor); -#endif -#if defined NEOPIXEL -// while (isFlickering) { -// } - getColor(storage.sndProfile[storage.soundFont].clashColor); - lightOn(currentColor); -#endif - } else if (blink < randomBlink) { - blink++; - } else if (blink == randomBlink and randomBlink >= 14) { - blink = 0; - } else if (blink == randomBlink and randomBlink < 14) { - randomBlink += random(7, 15); - brightness = 0; -#if defined FoCSTRING - FoCOff(FoCSTRING); -#endif -#if defined LUXEON - getColor(currentColor, storage.mainColor); - lightOn(ledPins, currentColor); -#endif -#if defined NEOPIXEL - getColor(storage.sndProfile[storage.soundFont].mainColor); - if (not isFlickering) { - lightOn(currentColor); - } -#endif - } -#if defined LEDSTRINGS - lightFlicker(ledPins, storage.sndProfile[storage.soundFont][2], - brightness); -#endif -#if defined LUXEON - lightFlicker(ledPins, currentColor, brightness); -#endif - - } else if (not lockup && randomBlink != 0) { // We have released lockup button -#if defined FoCSTRING - FoCOff(FoCSTRING); -#endif -#if defined LUXEON - getColor(currentColor, storage.mainColor); - lightOn(ledPins, currentColor); -#endif -#if defined NEOPIXEL - getColor(storage.sndProfile[storage.soundFont].mainColor); - if (not isFlickering) { - lightOn(currentColor); - } -#endif - randomBlink = 0; +// ==================================================================================== +// === SOUND FUNCTIONS === +// ==================================================================================== - } else if (blaster > 0) { +void HumRelaunch() { + LoopPlay_Sound(soundFont.getHum()); + sndSuppress = millis(); + hum_playing=true; +} -#if defined NEOPIXEL - if (blink == 2) { - getColor(storage.sndProfile[storage.soundFont].clashColor); - } -#endif - if (blink < 14) { -#if defined LEDSTRINGS - analogWrite(ledPins[blasterPin], LOW); -#if defined FoCSTRING - FoCOn(FoCSTRING); -#endif -#endif -#if defined LUXEON - getColor(currentColor, storage.clashColor); - lightOn(ledPins, currentColor); +void SinglePlay_Sound(uint8_t track) { +#ifdef OLD_DPFPLAYER_LIB + mp3_play_physical(track); +#else // DFPlayer_LSOS + dfplayer.playPhysicalTrack(track); #endif +} -#if defined NEOPIXEL - if (not isFlickering) { - lightBlasterEffect(blasterPixel, blink); - } +void LoopPlay_Sound(uint8_t track) { +#ifdef OLD_DPFPLAYER_LIB + mp3_loop_play(track); +#else // DFPlayer_LSOS + dfplayer.playSingleLoop(track); #endif - blink++; - } - -#if defined NEOPIXEL - else if (blink == 14) - { - getColor(storage.sndProfile[storage.soundFont].mainColor); - blink++; - } - else if (blink >= 15 and blink < 29) -#endif - -#if defined LEDSTRINGS - else if (blink >= 14 and blink < 19) { - - lightFlicker(ledPins, storage.sndProfile[storage.soundFont][2]); - - if (blasterPin > 0) - analogWrite(ledPins[blasterPin - 1], LOW); - - if (blasterPin < 5) - analogWrite(ledPins[blasterPin + 1], LOW); +} - blink++; +void Set_Loop_Playback() { +#ifdef OLD_DPFPLAYER_LIB + mp3_single_loop(true); +#else + dfplayer.setSingleLoop(true);; +#endif +} - } - else if (blink >= 19 and blink < 29) - -#endif -#if defined LUXEON - else if (blink >= 14 and blink < 29) - -#endif - { -#if defined NEOPIXEL - if (not isFlickering) { - lightBlasterEffect(blasterPixel, blink - 14); - } -#endif -#if defined LEDSTRINGS - lightFlicker(ledPins, storage.sndProfile[storage.soundFont][2]); -#endif -#if defined LUXEON - getColor(currentColor, storage.mainColor); - lightOn(ledPins, currentColor); -#endif -#if defined FoCSTRING - FoCOff(FoCSTRING); -#endif - blink++; - } - else if (blink == 29) - { -#if defined LUXEON - getColor(currentColor, storage.mainColor); - lightOn(ledPins, currentColor); -#endif +void InitDFPlayer(){ +#ifdef OLD_DPFPLAYER_LIB + mp3_set_serial (mp3player); //set softwareSerial for DFPlayer-mini mp3 module + mp3player.begin(9600); + delay(50); + mp3_set_device(1); //playback from SD card + delay(50); + mp3_set_volume (storage.volume); +#else + dfplayer.setSerial(DFPLAYER_TX, DFPLAYER_RX); + // AK 7.9.2016: if the storage.volume has no or invalid value, it will cause the + // sketch to repeat setup (reset itself) - up till now no idea why? + // this can happen if the EEPROM is erased (i.e. reflash of bootloader) + dfplayer.setVolume(storage.volume); -#if defined FoCSTRING - FoCOff(FoCSTRING); -#endif -#if defined LEDSTRINGS - lightFlicker(ledPins, storage.sndProfile[storage.soundFont][2]); + //setup finished. Boot ready. We notify ! #endif -//#if defined NEOPIXEL -// getColor(storage.sndProfile[storage.soundFont].mainColor); -//#endif - blink = 0; - blaster--; - } +} - } - else - { -#if defined LEDSTRINGS - lightFlicker(ledPins, storage.sndProfile[storage.soundFont][2]); -#endif -#if defined NEOPIXEL - // Neopixels string might be to slow to fill all LEDs register - // So we let the function finish it's duty before launching a new one - if (not isFlickering) { - lightFlicker(0); - } -#endif +void Pause_Sound() { +#ifdef OLD_DPFPLAYER_LIB + mp3_pause(); +#else + dfplayer.pause(); +#endif +} -#if defined LUXEON - lightFlicker(ledPins, currentColor,0); -#endif - } +void Resume_Sound() { +#ifdef OLD_DPFPLAYER_LIB + mp3_play(); +#else + dfplayer.play(); +#endif } -#endif //LIGHT_EFFECTS + + diff --git a/README/ArduinoIDE_Boards_AVRVersion.jpg b/README/ArduinoIDE_Boards_AVRVersion.jpg new file mode 100644 index 0000000..c117660 Binary files /dev/null and b/README/ArduinoIDE_Boards_AVRVersion.jpg differ diff --git a/README/ArduinoIDE_Boards_Manager.jpg b/README/ArduinoIDE_Boards_Manager.jpg new file mode 100644 index 0000000..7fca62b Binary files /dev/null and b/README/ArduinoIDE_Boards_Manager.jpg differ diff --git a/README/LSOS13_ActionModeFSM.jpg b/README/LSOS13_ActionModeFSM.jpg new file mode 100644 index 0000000..8437fb0 Binary files /dev/null and b/README/LSOS13_ActionModeFSM.jpg differ diff --git a/README/LSOS13_TopLFSM.jpg b/README/LSOS13_TopLFSM.jpg new file mode 100644 index 0000000..ee324ba Binary files /dev/null and b/README/LSOS13_TopLFSM.jpg differ diff --git a/SDCard.7z b/SDCard.7z index 790bfc7..72fdcbf 100644 Binary files a/SDCard.7z and b/SDCard.7z differ diff --git a/Soundfont.h b/Soundfont.h index 830f102..0eac504 100644 --- a/Soundfont.h +++ b/Soundfont.h @@ -1,229 +1,277 @@ -/* - * Soundfont.h - * - * Created on: 27 feb 2016 - * author: Sebastien CAPOU (neskweek@gmail.com) - * Source : https://github.com/neskweek/LightSaberOS - * Description: Soundfont Config file for LightSaberOS - * - * This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/. - */ -#include - -#if not defined SOUNDFONT_H_ -#define SOUNDFONT_H_ - -class SoundFont { - -#define SOUNDFONT_QUANTITY 1 - -public: - - SoundFont() { - - boot = LinkedList(); - powerOn = LinkedList(); - powerOff = LinkedList(); - hum = LinkedList(); - swing = LinkedList(); - spin = LinkedList(); - clash = LinkedList(); - lockup = LinkedList(); - blaster = LinkedList(); - wrist = LinkedList(); - force = LinkedList(); - ID=0; - powerOnTime =500; - powerOffTime = 500; - - } - ; - ~SoundFont() { - /* - delete ID; - delete boot; - delete powerOn; - delete powerOff; - delete hum; - delete swing; - delete clash; - delete lockup; - delete blaster; - */ - } - ; - - void setID(uint16_t id) { - uint16_t boot[2]; - uint16_t powerOn[2]; - uint16_t powerOff[2]; - uint16_t hum[2]; - uint16_t swing[2]; - uint16_t spin[2]; - uint16_t clash[2]; - uint16_t lockup[2]; - uint16_t blaster[2]; - uint16_t wrist[2]; - uint16_t force[2]; - - this->ID = id; - - switch (id) { - /* - case EXAMPLE: - // soundFont directory XX : - boot[0] = 1; // first boot sound file - boot[1] = 1; // last boot sound file - powerOn[0] = 2; // first powerOn sound file - powerOn[1] = 2; // last powerOn sound file - powerOff[0] = 3; // first powerOff sound file - powerOff[1] = 3; // last powerOff sound file - hum[0] = 4; // first hum sound file - hum[1] = 4; // last hum sound file - swing[0] = 6; // first swing sound file - swing[1] = 17; // last swing sound file - clash[0] = 18; // first clash sound file - clash[1] = 30; // last clash sound file - lockup[0] = 5; // first lockup sound file - lockup[1] = 5; // last lockup sound file - blaster[0] = 0; // first blaster sound file - blaster[1] = 0; // last blaster sound file - break; - */ - case 1: - // soundFont directory 01 is reserved for config menu sounds - break; - default: - // If you specify a ID number not defined here you will end up - // on the first defined soundfont - case 2: - // soundFont directory 02 : - this->powerOnTime = 400; - this->powerOffTime = 400; - boot[0] = 20; - boot[1] = 20; - powerOn[0] = 21; - powerOn[1] = 21; - powerOff[0] = 22; - powerOff[1] = 22; - hum[0] = 23; - hum[1] = 23; - swing[0] = 24; - swing[1] = 31; - spin[0] = 24; - spin[1] = 24; - clash[0] = 32; - clash[1] = 34; - lockup[0] = 38; - lockup[1] = 38; - blaster[0] = 35; - blaster[1] = 37; - wrist[0] = 39; - wrist[1] = 39; - force[0] = 0; - force[1] = 0; - break; - } - this->boot.clear(); - this->powerOn.clear(); - this->powerOff.clear(); - this->hum.clear(); - this->swing.clear(); - this->clash.clear(); - this->lockup.clear(); - this->blaster.clear(); - this->wrist.clear(); - this->force.clear(); - fill(&this->boot, boot); - fill(&this->powerOn, powerOn); - fill(&this->powerOff, powerOff); - fill(&this->hum, hum); - fill(&this->swing, swing); - fill(&this->spin, spin); - fill(&this->clash, clash); - fill(&this->lockup, lockup); - fill(&this->blaster, blaster); - fill(&this->wrist, wrist); - fill(&this->force, force); - } - - uint16_t getID() const { - return this->ID; - } - - const uint16_t getBlaster() { - return this->blaster.get(random(0, this->blaster.size())); - } - - const uint16_t getBoot() { - return this->boot.get(random(0, this->boot.size())); - } - - const uint16_t getClash() { - return this->clash.get(random(0, this->clash.size())); - } - - const uint16_t getHum() { - return this->hum.get(random(0, this->hum.size())); - } - - const uint16_t getLockup() { - return this->lockup.get(random(0, this->lockup.size())); - } - - const uint16_t getPowerOff() { - return this->powerOff.get(random(0, this->powerOff.size())); - } - - const uint16_t getPowerOn() { - return this->powerOn.get(random(0, this->powerOn.size())); - } - - const uint16_t getSwing() { - return this->swing.get(random(0, this->swing.size())); - } - - const uint16_t getSpin() { - return this->spin.get(random(0, this->spin.size())); - } - - const uint16_t getForce() { - return this->force.get(random(0, this->force.size())); - } - const uint16_t getWrist() { - return this->wrist.get(random(0, this->wrist.size())); - } - - uint16_t getPowerOffTime() const { - return powerOffTime; - } - - uint16_t getPowerOnTime() const { - return powerOnTime; - } - -private: - uint16_t ID; - LinkedList boot; - LinkedList powerOn; - uint16_t powerOnTime; - LinkedList powerOff; - uint16_t powerOffTime; - LinkedList hum; - LinkedList swing; - LinkedList spin; - LinkedList clash; - LinkedList lockup; - LinkedList blaster; - LinkedList wrist; - LinkedList force; - - void fill(LinkedList* list, uint16_t array[]) { - for (uint16_t i = array[0]; i <= array[1]; i++) { - list->add(i); - } - } -}; - -#endif /* SOUNDFONT_H_ */ +/* + * Soundfont.h + * + * Created on: 27 feb 2016 + * author: Sebastien CAPOU (neskweek@gmail.com) and Andras Kun (kun.andras@yahoo.de) + * Source : https://github.com/neskweek/LightSaberOS + * Description: Soundfont Config file for LightSaberOS + * + * This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. + * To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/. + */ +#include + +#ifndef SOUNDFONT_H_ +#define SOUNDFONT_H_ + +class SoundFont { + +#define SOUNDFONT_QUANTITY 1 +#define NR_CONFIGFOLDERFILES 14 +#define NR_JUKEBOXSONGS 1 +#define NR_SF1_FILES 20 + + +public: + + SoundFont() { + + boot = LinkedList(); + powerOn = LinkedList(); + powerOff = LinkedList(); + hum = LinkedList(); + swing = LinkedList(); + spin = LinkedList(); + clash = LinkedList(); + lockup = LinkedList(); + blaster = LinkedList(); + wrist = LinkedList(); + force = LinkedList(); + menu = LinkedList(); + ID=0; + powerOnTime =500; + powerOffTime = 500; + powerOnEffect=0; //0: movie-like; 1: inverted + powerOffEffect=0; //0: movie-like; 1: inverted + flickerEffect=0; //0: standard; 1: pulse; 2: anarchic + swingthreshold=300; + + } + ; + ~SoundFont() { + /* + delete ID; + delete boot; + delete powerOn; + delete powerOff; + delete hum; + delete swing; + delete clash; + delete lockup; + delete blaster; + */ + } + ; + + void setID(uint16_t id) { + uint16_t boot[2]; + uint16_t powerOn[2]; + uint16_t powerOff[2]; + uint16_t hum[2]; + uint16_t swing[2]; + uint16_t spin[2]; + uint16_t clash[2]; + uint16_t lockup[2]; + uint16_t blaster[2]; + uint16_t wrist[2]; + uint16_t force[2]; + uint16_t menu[2]; + + this->ID = id; + + switch (id) { + /* + case EXAMPLE: + // soundFont directory XX : + menu[0] = 1; // first menu sound file + menu[1] = 1; // last menu sound file + boot[0] = 1; // first boot sound file + boot[1] = 1; // last boot sound file + powerOn[0] = 2; // first powerOn sound file + powerOn[1] = 2; // last powerOn sound file + powerOff[0] = 3; // first powerOff sound file + powerOff[1] = 3; // last powerOff sound file + hum[0] = 4; // first hum sound file + hum[1] = 4; // last hum sound file + swing[0] = 6; // first swing sound file + swing[1] = 17; // last swing sound file + clash[0] = 18; // first clash sound file + clash[1] = 30; // last clash sound file + lockup[0] = 5; // first lockup sound file + lockup[1] = 5; // last lockup sound file + blaster[0] = 0; // first blaster sound file + blaster[1] = 0; // last blaster sound file + break; + */ + case 1: + // soundFont directory 01 is reserved for config menu sounds + break; + default: + // If you specify a ID number not defined here you will end up + // on the first defined soundfont + case 2: + // soundFont directory 02 : + this->powerOnTime = 800; + this->powerOffTime = 800; + this->powerOnEffect=0; + this->powerOffEffect=0; + this->flickerEffect=0; + this->swingthreshold=700; + boot[0] = NR_CONFIGFOLDERFILES + NR_JUKEBOXSONGS + 1; // 1 boot sound (1) + boot[1] = boot[0]; + powerOn[0] = boot[1]+1; // 1 power-on sound + powerOn[1] = powerOn[0]; + powerOff[0] = powerOn[1]+1; // 1 power-off sounds + powerOff[1] = powerOff[0]; + swing[0] = powerOff[1]+1; // 8 swing sounds + swing[1] = swing[0]+7; + spin[0] = 0; + spin[1] = 0; + clash[0] = swing[1]+1; // 3 clash sounds + clash[1] = clash[0]+2; + lockup[0] = clash[1]+1; // 1 lockup sound + lockup[1] = lockup[0]; + blaster[0] = lockup[1]+1; // 3 blaster deflect sound + blaster[1] = blaster[0]+2; + wrist[0] = 0; + wrist[1] = 0; + force[0] = 0; + force[1] = 0; + menu[0] = blaster[1]+1; // 1 menu sound file + menu[1] = menu[0]; + // hum must be the last file in the sound font for proper hum relaunch + hum[0] = menu[1]+1; // 1 hum relaunch sound (30) + hum[1] = hum[0]; + break; + } + this->boot.clear(); + this->powerOn.clear(); + this->powerOff.clear(); + this->swing.clear(); + this->spin.clear(); + this->clash.clear(); + this->lockup.clear(); + this->blaster.clear(); + this->wrist.clear(); + this->force.clear(); + this->menu.clear(); + this->hum.clear(); + fill(&this->boot, boot); + fill(&this->powerOn, powerOn); + fill(&this->powerOff, powerOff); + fill(&this->swing, swing); + fill(&this->spin, spin); + fill(&this->clash, clash); + fill(&this->lockup, lockup); + fill(&this->blaster, blaster); + fill(&this->wrist, wrist); + fill(&this->force, force); + fill(&this->menu, menu); + fill(&this->hum, hum); + } + + uint16_t getID() const { + return this->ID; + } + + const uint16_t getBlaster() { + return this->blaster.get(random(0, this->blaster.size())); + } + + const uint16_t getBoot() { + return this->boot.get(random(0, this->boot.size())); + } + + const uint16_t getClash() { + return this->clash.get(random(0, this->clash.size())); + } + + const uint16_t getHum() { + return this->hum.get(random(0, this->hum.size())); + } + + const uint16_t getLockup() { + return this->lockup.get(random(0, this->lockup.size())); + } + + const uint16_t getPowerOff() { + return this->powerOff.get(random(0, this->powerOff.size())); + } + + const uint16_t getPowerOn() { + return this->powerOn.get(random(0, this->powerOn.size())); + } + + const uint16_t getSwing() { + return this->swing.get(random(0, this->swing.size())); + } + + const uint16_t getSpin() { + return this->spin.get(random(0, this->spin.size())); + } + + const uint16_t getForce() { + return this->force.get(random(0, this->force.size())); + } + const uint16_t getWrist() { + return this->wrist.get(random(0, this->wrist.size())); + } + + const uint16_t getMenu() { + return this->menu.get(random(0, this->menu.size())); + } + + uint16_t getPowerOffTime() const { + return powerOffTime; + } + + uint16_t getPowerOnTime() const { + return powerOnTime; + } + + uint16_t getPowerOnEffect() const { + return powerOnEffect; + } + + uint16_t getPowerOffEffect() const { + return powerOffEffect; + } + + uint16_t getFlickerEffect() const { + return flickerEffect; + } + + uint16_t getSwingThreshold() const { + return swingthreshold; + } + + +private: + uint16_t ID; + LinkedList boot; + LinkedList powerOn; + uint16_t powerOnTime; + LinkedList powerOff; + uint16_t powerOffTime; + LinkedList hum; + LinkedList swing; + LinkedList spin; + LinkedList clash; + LinkedList lockup; + LinkedList blaster; + LinkedList wrist; + LinkedList force; + LinkedList menu; + uint16_t powerOnEffect; + uint16_t powerOffEffect; + uint16_t flickerEffect; + uint16_t swingthreshold; + + + void fill(LinkedList* list, uint16_t array[]) { + for (uint16_t i = array[0]; i <= array[1]; i++) { + list->add(i); + } + } +}; +#endif /* SOUNDFONT_H_ */