Problem read MPU-6050 myself

Hello, I am trying to read and config the MPU-6050 by myself with my own libreries but not work. I always have the same data, it is 15164 for all axis of accelerometer.

I am using and atmega328P

Here is the code, what do I do bud? Help please!!!!!!!!!!!

#include
#include
#include "i2c.h"
#include "usart.h"


#define F_CPU 16000000UL
#define IMU 0x68
#define DLPF_CFG 0x03
#define GYRO_FS 0x03
#define AFS_SEL 0x00
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C

void writeTo(char reg, char data)
{
i2c_start();
i2c_write(IMU);
i2c_write(reg);
i2c_write(data);
i2c_stop();
}

// Lectura de un dato del IMU
char readFrom (char reg)
{
char data;
i2c_start();
i2c_write(IMU);
i2c_write(reg);
i2c_restart();
i2c_write(IMU);
data = i2c_read(1);
i2c_stop();
return data;

}

// Lectura acelerómetro
void get_acc(int *result)
{
char reg = 0x3B;
int dato[6];
for (int i = 0; i <= 5; i++)
{
dato = readFrom(reg + i);
}
result[0] = (int(dato[0]) << 8) | dato[1]; // ACC_X
result[1] = (int(dato[2]) << 8) | dato[3]; // ACC_Y
result[2] = (int(dato[4]) << 8) | dato[4]; // ACC_Z
}

// Inicialización del IMU MPU-6050
void imu_init()
{
writeTo(CONFIG, DLPF_CFG);
writeTo(GYRO_CONFIG, GYRO_FS);
writeTo(ACCEL_CONFIG, AFS_SEL);
}





Libraries i2c.cpp

#include "i2c.h"
#include

#define F_CPU 16000000UL

//Inicializar módulo TWI
//Configurar frecuencia de reloj I2C_BAUD

void i2c_init (void)
{
TWSR &= ~((1 << TWPS1) | (1 << TWPS0));
TWBR = ((F_CPU/I2C_BAUD) - 16)/2;
TWCR = (1 << TWEN);
}

// Envio start o start repetida
// Devuelve 1 si la condición de start es satisfactoria, 0 si falló
char i2c_start(void)
{
TWCR = (1 << TWSTA) | (1 << TWINT) | (1 << TWEN);
while ((TWCR & (1 << TWINT)) == 0); TWCR = (1 << TWEN); if (((TWSR & 0xFC) == 0x08) || ((TWSR & 0xFC) == 0x10)) return 1;
return 0;
}


// Envia condición de STOP
void i2c_stop(void)
{
TWCR = (1 << TWSTO) | (1 << TWINT) | (1 << TWEN); while (TWCR & (1 << TWSTO));
}


// Envia el byte data y devuelve 0 si el esclavo responde con ACK, si no devuelve 1
char i2c_write(char data)
{
TWDR = data;
TWCR = (1 << TWINT) | (1 << TWEN);
while ((TWCR & (1 << TWINT)) == 0);
if (((TWSR & 0xFC) == 0x18) || ((TWSR & 0xFC) == 0x40) || ((TWSR & 0xFC) == 0x28)) return 0; return 1; }

// Lee un byte dato y envia el bit ACK/NACK
char i2c_read(char ack)
{
while ((TWCR & (1 << TWINT)) == 0);
if (ack == 1)
TWCR = (1 << TWINT) | (1 << TWEN);
TWCR = (1 << TWEA) | (1 << TWINT) | (1 << TWEN);
while ((TWCR & (1 << TWINT)) == 0); return TWDR;
}