diff --git a/main controller code/src/i2c_master.c b/main controller code/src/i2c_master.c
index 4ce022f..e254453 100644
--- a/main controller code/src/i2c_master.c	
+++ b/main controller code/src/i2c_master.c	
@@ -1,17 +1,31 @@
 #include "include/i2c_master.h"
 
-inline void i2c_master_send(uint8_t address, const uint8_t *src, size_t len)
+#include <pico/stdlib.h>
+#include <hardware/i2c.h>
+
+void i2c_master_init(void)
 {
-    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, src, len, false, I2C_MASTER_MAX_TIMEOUT);
+    gpio_set_function(I2C_MASTER_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(I2C_MASTER_SCL_PIN, GPIO_FUNC_I2C);
+
+    gpio_pull_up(I2C_MASTER_SDA_PIN);
+    gpio_pull_up(I2C_MASTER_SCL_PIN);
+
+    i2c_init(I2C_MASTER_INSTANCE, I2C_MASTER_BAUD_RATE);
 }
 
-inline void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len)
+inline void i2c_master_write(uint8_t address, const uint8_t *src, size_t len)
 {
-    i2c_read_blocking_until(I2C_MASTER_INSTANCE, address, dst, len, false, I2C_MASTER_MAX_TIMEOUT);
+    i2c_write_blocking(I2C_MASTER_INSTANCE, address, src, len, false);
 }
 
-inline void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len)
+inline void i2c_master_read(uint8_t address, uint8_t *dst, size_t len)
 {
-    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, data, len, true, I2C_MASTER_MAX_TIMEOUT);
-    i2c_master_receive(address, data, len);
+    i2c_read_blocking(I2C_MASTER_INSTANCE, address, dst, len, false);
+}
+
+inline void i2c_master_read_reg(uint8_t address, uint8_t reg, uint8_t *dst, size_t len)
+{
+    i2c_master_write(address, &reg, 1);
+    i2c_master_read(address, dst, len);
 }
\ No newline at end of file
diff --git a/main controller code/src/include/i2c_master.h b/main controller code/src/include/i2c_master.h
index d30c5c9..23e825e 100644
--- a/main controller code/src/include/i2c_master.h	
+++ b/main controller code/src/include/i2c_master.h	
@@ -6,13 +6,15 @@
 #define I2C_MASTER_SDA_PIN 4
 #define I2C_MASTER_SCL_PIN 5
 #define I2C_MASTER_INSTANCE i2c0
-#define I2C_MASTER_MAX_TIMEOUT 10000
+#define I2C_MASTER_BAUD_RATE 100 * 1000
 
+// Init master i2c
+void i2c_master_init(void);
 // Send [src] of [len] to [address] and close communication
-void i2c_master_send(uint8_t address, const uint8_t *src, size_t len);
+void i2c_master_write(uint8_t address, const uint8_t *src, size_t len);
 // Receive [dst] of [len] from [address] and close communication
-void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len);
-// Send [data] of [len] to [address], receive [data] of [len] from [address] and close communication
-void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len);
+void i2c_master_read(uint8_t address, uint8_t *dst, size_t len);
+// Send [reg] and receive data in [dst] and close communition
+void i2c_master_read_reg(uint8_t address, uint8_t reg, uint8_t *dst, size_t len);
 
 #endif // I2C_MASTER_H
\ No newline at end of file