ATMEGA328 TWI communication

Hi, I was hoping some one with more experience with the UNO and the Atmega328 TWI communication could help me.
I have been writing a sketch to test communication to a MPU6050 with the wire.h library. I wanted to include a statement to see if there was problem with the connectivity by checking the result of the endTransmission() function from the wire.h library, but with no device connected it hangs.
I then went to the Atmega data sheet

http://www.atmel.com/images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet_Complete.pdf

Starting P217 and looked at the TWI information and have written the below sketch. With this I can check to see if the device is connected and has power by looking to the TWSR reg.

Going further, I thought i’d test out communicating using the atmega register and not the wire.h lib. This is where I’m having problems. I cannot write or read to the MPU6050 and I’m not sure where I’m going wrong… Its taken me 2 days and enough coffee to wire an elephant to get this far …

See the sketch below with comments but the general gist is:

  1. check the contents of the MPU6050 PWR reg with wire.h – result – 0x40
  2. start TWI using ATMEGA registers
  3. send MPU address and R/W bit – result – SLA+W sent and ACK received
  4. send MPU register address – result – DATA sent and ACK received
  5. send data (0x00) clear reg – result – DATA sent and ACK received
  6. send stop condition.
  7. Recheck the contents of the MPU6050 PWR reg with wire.h – result – 0x40…should be 0x00
#include <Wire.h>
byte MPU_REG_0D = 0;
const byte MPU_addr = 0x68; //1101000
byte cnt = 0;
bool MPU_fault = false;

void setup() {
  Serial.begin(56700);
  Wire.begin();

  Serial.println(F("======================================================================"));
  Serial.println(F("=                             Wire.h Test                            ="));
  Serial.println(F("======================================================================"));


  MPU_test(); /// Start by checking register content on MPU6050 using wire.h -- returns 0x40

  Serial.print(F("Connecting to MPU6050"));
  
  /////////START CONDITION/////// 
  DEBUG(); //1
  TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
  while (!(TWCR & (1 << TWINT))) {
  }

  DEBUG(); //2   -- 0x08 in TWSR recieved (GOOD)
  if (TWSR & 0xF8 != 0x08) {
    MPU_fault = true;
  }

  TWDR = 0xD0; // DO is MPU6050 address with R/W set to 0(W)
               
  TWCR = (1 << TWINT) | (1 << TWEN);

  DEBUG(); //3 -- SLA+W sent and ACK recieved (GOOD) mpu is responding
  while (!(TWCR & (1 << TWINT))) {
  }

  if (TWSR & 0xF8 != 0x48) {
    MPU_fault = true;
  }

  TWDR = 0xB6; // Send the register address
  TWCR = (1 << TWINT) | (1 << TWEN);

  DEBUG(); //4 ---DATA + ACK is recieved 

  while (!(TWCR & (1 << TWINT))) {
  }

  TWDR = 0x00; // Send the data -- should clear the register
  TWCR = (1 << TWINT) | (1 << TWEN);

  DEBUG(); //5

  while (!(TWCR & (1 << TWINT))) {
  }

  DEBUG(); //6 ---DATA + ACK is recieved 
  TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN) ;  // stop condition

  DEBUG(); //7 

  if (MPU_fault == false) {
    Serial.println(F("Successful"));
  }


  Serial.println(F("\nSetup End"));





  MPU_test(); // check contents of the register using wire.h -- result reg B6 is still 0x40

  DEBUG(); //8


}

void loop() {


}

void DEBUG() {
  static byte i = 1;
  Serial.print(F("\n"));
  Serial.print("=========DEBUG ");
  Serial.print(i);
  Serial.println("==========");
  Serial.print("TWCR      : ");
  Serial.println(TWCR, BIN);
  Serial.print("TWSR      : ");
  Serial.print(TWSR, HEX);
  if (TWSR == 0x08) {
    Serial.println(" : Start");
  }
  else if (TWSR == 0x18) {
    Serial.println(" : SLA+W ACK recieved");
  }
  else if (TWSR == 0x20) {
    Serial.println(" : SLA+W NACK recieved");
  }
  else if (TWSR == 0x28) {
    Serial.println(" : DATA ACK recieved");
  }
  else if (TWSR == 0x30) {
    Serial.println(" : DATA NACK recieved");
  }
  else if (TWSR == 0x40) {
    Serial.println(" : SLA+R ACK recieved");
  }
  else if (TWSR == 0x48) {
    Serial.println(" : SLA+R NACK recieved");
  }
  else if (TWSR == 0x50) {
    Serial.println(" : DATA NACK recieved");
  }
  else if (TWSR == 0x58) {
    Serial.println(" : DATA ACK recieved");
  }
  else {
    Serial.println(" : Other");
  }
  Serial.print("TWDR      : ");
  Serial.println(TWDR, BIN);
  Serial.print("TWAR      : ");
  Serial.println(TWAR, BIN);
  Serial.print("TWBR      : ");
  Serial.println(TWBR, BIN);
  Serial.print("TWAMR     : ");
  Serial.println(TWAMR, BIN);
  Serial.println("==========================");
  i ++;
}

void MPU_wake() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission ();
}

void MPU_test() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.endTransmission ();
  Wire.requestFrom((int)MPU_addr, 1);
  MPU_REG_0D = Wire.read();
  Serial.println(MPU_REG_0D, HEX);
}

Any help would be good, like I say 2 days and no more coffee.

Hi, Thanks to anyone who took the time to read this, it has now been solved. A fresh pot of coffee and another hour looking at it and I realised I made a mistake with the register address I was writing too, I wrote B6 instead of 6B.
Lesson learned sometimes take a break!!

If anyone did read the code and noticed and errors or things worth improving please let me know.

BR all
D