Hi!
Ein paar Komentare im Code könnten schon helfen, hab nämlich keine Lust, Zeile für Zeile mit dem Datenblatt zu vergleichen.
Wenn ich das aber richtig sehe, dann ist 0x40 die Slaveadresse (???). Wenn ja, dann wäre das natürlich falsch.
Gruß