// Debug defines
#define DEBUG
#ifdef DEBUG
#define DEBUG_START(baud) Serial.begin(baud)
#define DEBUG_AVAILABLE() Serial.available()
#define DEBUG_PRINT(x) Serial.print (x)
#define DEBUG_PRINT(x,y) Serial.print (x,y)
#define DEBUG_PRINTDEC(x) Serial.print (x, DEC)
#define DEBUG_PRINTLN(x) Serial.println (x)
#define DEBUG_PRINTLN(x,y) Serial.println (x,y)
#define DEBUG_READ() Serial.read()
#else
#define DEBUG_START(baud)
#define DEBUG_AVAILABLE()
#define DEBUG_PRINT(x)
#define DEBUG_PRINT(x,y)
#define DEBUG_PRINTDEC(x)
#define DEBUG_PRINTLN(x)
#define DEBUG_PRINTLN(x,y)
#define DEBUG_READ()
#endif
// Variables for the I2C functions
byte i2cbyte; // Byte to send/receive by I2C
byte i2cinchar[20]; // 20 char buffer for i2cRB()
byte i2coutchar[16]; // 16 char buffer for i2cSB()
byte i2cnumber; // Number of actual chars in/out of buffer
byte i2cRA; // Register
byte i2ccount; // General use
byte i2cackbit; // Usually ignored
// Variables for the memory banks
byte mpubank; // Firmware bank (256 bytes) (Reg 6D)
int mpumemadd; // Firmware memory addr in bank (Reg 6E)
int mpufifo; // Fifo count 16 bit unsigned, full at 1024
boolean mpuprsw = true; // Print full loading information if set 1
// Storage for the quaternion values
long quat[4]; // w,x,y,z (32 bit)
long quats[4]; // quat[] scaled (to 16 bits)
// DMP firmware kept in the flash memory
PROGMEM const byte DMPfirm[] = {
/* bank # 0 */
};
void setup() {
DEBUG_START(9600); // Start the serial communication
delay(3000); // Delay to open the serial monitor
// Set PORTB outputs to LOW so they can never drive high
digitalWrite (12, LOW); // Belangrijk!!! i2c mag max 3.3V dus deze pin op high beschadigd de mpu6050
digitalWrite (11, LOW); // op 4 pinnen want deze locaties zijn afhankelijk van je bord
digitalWrite (14, LOW);
digitalWrite (8, LOW);
// Release i2c bus lines and allow GY-521 2.2K to pull up.
pinMode (12, INPUT); //SCL clocks going high
pinMode (11, INPUT); //SDA
pinMode (14, INPUT); //SCL clocks going high
pinMode (8, INPUT); //SDA
pinMode (9, INPUT); //Reset knop
DEBUG_PRINTLN("You can now plug the MPU-6050 safely in.\nSend any data in the serial monitor when you pluged it in.");
while (DEBUG_AVAILABLE() == 0); //Wait for data
while (DEBUG_AVAILABLE() != 0) DEBUG_READ(); //Clear the buffer
// Grab and release bus to ensure clear
i2cstart ();
delay(1);
i2cstop ();
// Is the MPU asleep? Yes ==> Load the firmware
i2cRA = 0x6B;
i2cnumber = 1;
i2cmultiRB ();
if (i2cinchar[0] != B00000001) {
DEBUG_PRINTLN("\nLoad the DMP firmware");
i2cRA = 0x6B;
i2coutchar[0] = B00000001; // Set CLKSEL maar houd in slaap
i2cnumber = 1;
i2cmultiSB ();
delay(100);
// Load the DMP firmware trough the portal (Undocumented)
for (mpubank = 0; mpubank < 12; mpubank++) { // Cycle trough all banks
//Show the current bank
DEBUG_PRINTLN("\n bank # " + String(mpubank) + ":");
for (mpumemadd = 0; mpumemadd < 241; mpumemadd = mpumemadd + 16) { // Cycle trough all memory addresses
loaderror: //Try again from here if there is an error
mpusendbank(); //send bank and mem start
for (int i = 0; i < 16; i++) { //load the I2C send buffer and send
int k = mpubank * 256;
k = k + mpumemadd;
k = k + i;
i2coutchar[i] = pgm_read_byte_near (DMPfirm + k);
} // repeat for loop if condition is true
i2cRA = 0x6F; // DMP firmware portal
i2cnumber = 16; // 16 bytes
i2cmultiSB (); //send 16 bytes via portal
mpusendbank(); //send same bank and mem start
i2cmultiRB (); //read back the 16 bytes into i2cinchar[]
boolean k = false;
for (int i = 0; i < 16; i++) { // Confirm correct firmware load byt comparing in and out bytes
if (i2cinchar[i] != i2coutchar[i]) k = true; // Error
} // repeat for loop if condition is true
if (k) { // was there an errror?
DEBUG_PRINTLN(" error");
delay(200);
goto loaderror;
}
if (mpuprsw) { // Print the values if this is true (set in setup)
DEBUG_PRINT(" block " + String(mpumemadd) + ":\t");
for (int i = 0; i < 16; i++) {
DEBUG_PRINT(" ");
DEBUG_PRINT(i2cinchar[i], HEX);
} // Repeat for loop while the condition is true
DEBUG_PRINTLN(""); // Print \n\r
} // End of if
} // Next block mpumemadd
} // Next mpubank
//Regs 112 and 113 (70 & 71) have DMP program start address $0400
i2cRA = 0x70; //unlisted Regs
i2cnumber = 2;
i2coutchar[0] = 0x04;
i2coutchar[1] = 0x00;
i2cmultiSB (); //send 2 bytes
DEBUG_PRINTLN("\nDMP loaded");
} //end of "if not loaded"
i2cRA = 0x1B; //Reg 27 (1B) gyro config
i2cnumber = 1;
i2coutchar[0] = B00011000; //FSR 2000dps mandated for DMP fusion
i2cmultiSB ();
i2cRA = 0x6A; //Reg 106 (6A) user control
i2coutchar[0] = B11000000; //enable DMP and FIFO (reserved bit)
i2cmultiSB ();
} //end of setup