void I2C_SetConfig(I2cConfigData_t* config) {
	if (config->IsEnabled) {
		SFRPAGE = 0x00;
		TH0 = config->TH0Val;
		I2C0_reset();
		I2C_Enable();
		I2C0_init(I2C0_TIMER0, true);
	} else {
		I2C_Disable();
	}
}
void I2C_SetConfig(I2cConfigData_t* config)
{
	if(config->IsEnabled)
	{
		  I2C0_reset();
		I2C_Enable();
		I2C0_init(I2C0_TIMER1, true);
	} else {
		I2C_Disable();
	}
}