diff --git a/Core/Src/Components/GPIO_Pin.hpp b/Core/Src/Components/GPIO_Pin.hpp
index 1210286..9d0dc45 100644
--- a/Core/Src/Components/GPIO_Pin.hpp
+++ b/Core/Src/Components/GPIO_Pin.hpp
@@ -13,11 +13,11 @@
 class GPIO_Pin {
 public:
 	virtual int32_t init() = 0;
-	virtual uint8_t read() = 0;
-	virtual void write(uint8_t state) = 0;
-	virtual void set() = 0;
-	virtual void reset() = 0;
-	virtual void toggle() = 0;
+	virtual int32_t read(uint8_t* state) = 0;
+	virtual int32_t write(uint8_t state) = 0;
+	virtual int32_t set() = 0;
+	virtual int32_t reset() = 0;
+	virtual int32_t toggle() = 0;
 };
 
 #endif /* SRC_COMPONENTS_GPIO_PIN_HPP_ */
diff --git a/Core/Src/Components/GPIO_Pin_STM32.cpp b/Core/Src/Components/GPIO_Pin_STM32.cpp
index 208f89c..9e2731e 100644
--- a/Core/Src/Components/GPIO_Pin_STM32.cpp
+++ b/Core/Src/Components/GPIO_Pin_STM32.cpp
@@ -19,22 +19,27 @@ int32_t GPIO_Pin_STM32::init() {
 	return 0;
 }
 
-uint8_t GPIO_Pin_STM32::read() {
-	return (uint8_t)HAL_GPIO_ReadPin(gpio_port, gpio_pin);
+int32_t GPIO_Pin_STM32::read(uint8_t* state) {
+	*state = (uint8_t)HAL_GPIO_ReadPin(gpio_port, gpio_pin);
+	return 0;
 }
 
-void GPIO_Pin_STM32::write(uint8_t state) {
+int32_t GPIO_Pin_STM32::write(uint8_t state) {
 	HAL_GPIO_WritePin(gpio_port, gpio_pin, (GPIO_PinState)state);
+	return 0;
 }
 
-void GPIO_Pin_STM32::set() {
+int32_t GPIO_Pin_STM32::set() {
 	HAL_GPIO_WritePin(gpio_port, gpio_pin, GPIO_PIN_SET);
+	return 0;
 }
 
-void GPIO_Pin_STM32::reset() {
+int32_t GPIO_Pin_STM32::reset() {
 	HAL_GPIO_WritePin(gpio_port, gpio_pin, GPIO_PIN_RESET);
+	return 0;
 }
 
-void GPIO_Pin_STM32::toggle() {
+int32_t GPIO_Pin_STM32::toggle() {
 	HAL_GPIO_TogglePin(gpio_port, gpio_pin);
+	return 0;
 }
diff --git a/Core/Src/Components/GPIO_Pin_STM32.hpp b/Core/Src/Components/GPIO_Pin_STM32.hpp
index cb36e10..e0286da 100644
--- a/Core/Src/Components/GPIO_Pin_STM32.hpp
+++ b/Core/Src/Components/GPIO_Pin_STM32.hpp
@@ -16,11 +16,11 @@ public:
 	GPIO_Pin_STM32(GPIO_TypeDef* gpio_port, uint16_t gpio_pin);
 	virtual ~GPIO_Pin_STM32();
 	int32_t init();
-	uint8_t read();
-	void write(uint8_t state);
-	void set();
-	void reset();
-	void toggle();
+	int32_t read(uint8_t* state);
+	int32_t write(uint8_t state);
+	int32_t set();
+	int32_t reset();
+	int32_t toggle();
 private:
 	GPIO_TypeDef* gpio_port;
 	uint16_t gpio_pin;
diff --git a/Core/Src/Components/ILI9341.cpp b/Core/Src/Components/ILI9341.cpp
index 3908019..00d05b6 100644
--- a/Core/Src/Components/ILI9341.cpp
+++ b/Core/Src/Components/ILI9341.cpp
@@ -6,6 +6,7 @@
  */
 
 #include "ILI9341.hpp"
+#include <cstring>
 
 ILI9341::ILI9341(SPI_Peripheral* hSpi, GPIO_Pin* hNssPin, GPIO_Pin* hResetPin, GPIO_Pin* hDcrsPin, PWM_Pin* hLedPwm) :
 	hSpi(hSpi), hNssPin(hNssPin), hResetPin(hResetPin), hDcrsPin(hDcrsPin), hLedPwm(hLedPwm) {
@@ -16,36 +17,115 @@ ILI9341::~ILI9341() {
 
 }
 
-int32_t ILI9341::init() {
-	// Init pins
-	hNssPin->init();
-	hResetPin->init();
-	hDcrsPin->init();
-	hLedPwm->init();
-	// Reset LCD
-	hResetPin->reset();
-	vTaskDelay(1);
-	hResetPin->set();
+// Public methods
 
-	hSpi->take(100);
-	hNssPin->reset();
-	hSpi->transmit((uint8_t*)"Hello World", 11);
-	hNssPin->set();
-	hSpi->give();
-	hSpi->take(100);
-	hNssPin->reset();
-	hSpi->transmit((uint8_t*)"Hello World", 11);
-	hNssPin->set();
-	hSpi->give();
-	while(true){
-		hLedPwm->setDuty(0.5);
-		vTaskDelay(1000);
-		hLedPwm->setDuty(1);
-		vTaskDelay(1000);
-		hLedPwm->setDuty(0.5);
-		vTaskDelay(1000);
-		hLedPwm->setDuty(0);
-		vTaskDelay(1000);
+int32_t ILI9341::init() {
+	int32_t errors = 0;
+	// Init pins
+	errors += hNssPin->init();
+	errors += hResetPin->init();
+	errors += hDcrsPin->init();
+	errors += hLedPwm->init();
+	// Reset LCD
+	errors += hResetPin->reset();
+	vTaskDelay(1);
+	errors += hResetPin->set();
+	// Start SPI Command
+	uint32_t id;
+	errors += readReg24(0x04, &id);
+	if(id != 0x858552){
+		// Wrong ID
+		return -1;
 	}
-	return 0;
+	uint8_t idn[3];
+	errors += readReg8(0xDA, idn);
+	errors += readReg8(0xDB, idn+1);
+	errors += readReg8(0xDC, idn+2);
+	writeBuffer(0x11, nullptr, 0);
+	vTaskDelay(5);
+	uint8_t buf[720];
+	uint8_t pasetBuf[4];
+	for(uint16_t i = 0; i<320; i++){
+		pasetBuf[0] = (i>>8);
+		pasetBuf[1] = i;
+		pasetBuf[2] = 319>>8;
+		pasetBuf[3] = 319;
+		writeBuffer(0x2B, pasetBuf, 4);
+		for(uint32_t j = 0; j<720; j++){
+			buf[j] = (j%64)<<2;
+		}
+		writeBuffer(0x2C, buf, 720);
+	}
+	writeBuffer(0x29, nullptr, 0);
+	uint32_t status;
+	errors += readReg32(0x09, &status);
+	errors += hLedPwm->setDuty(1);
+	return errors;
+}
+
+// Private methods
+
+int32_t ILI9341::readReg8(uint8_t command, uint8_t* reg) {
+	int32_t errors = 0;
+	errors += hSpi->take(100);
+	errors += hDcrsPin->reset();
+	errors += hNssPin->reset();
+	txBuffer[0] = command;
+	errors += hSpi->trx(txBuffer, rxBuffer, 2);
+	errors += hNssPin->set();
+	errors += hSpi->give();
+	if(reg != nullptr){
+		*reg = rxBuffer[1];
+	}
+	return errors;
+}
+
+int32_t ILI9341::readReg24(uint8_t command, uint32_t* reg) {
+	int32_t errors = 0;
+	errors += hSpi->take(100);
+	errors += hDcrsPin->reset();
+	errors += hNssPin->reset();
+	txBuffer[0] = command;
+	errors += hSpi->trx(txBuffer, rxBuffer, 5);
+	errors += hNssPin->set();
+	errors += hSpi->give();
+	if(reg != nullptr){
+		*reg = (((uint32_t)rxBuffer[1])<<17) + (((uint32_t)rxBuffer[2])<<9) + (((uint32_t)rxBuffer[3])<<1) + (((uint32_t)rxBuffer[4])>>7);
+	}
+	return errors;
+}
+
+int32_t ILI9341::readReg32(uint8_t command, uint32_t* reg) {
+	int32_t errors = 0;
+	errors += hSpi->take(100);
+	errors += hDcrsPin->reset();
+	errors += hNssPin->reset();
+	txBuffer[0] = command;
+	errors += hSpi->trx(txBuffer, rxBuffer, 6);
+	errors += hNssPin->set();
+	errors += hSpi->give();
+	if(reg != nullptr){
+		*reg = (((uint32_t)rxBuffer[1])<<25) + (((uint32_t)rxBuffer[2])<<17) + (((uint32_t)rxBuffer[3])<<9) + (((uint32_t)rxBuffer[4])<<1) + (((uint32_t)rxBuffer[5])>>7);
+	}
+	return errors;
+}
+
+int32_t ILI9341::writeBuffer(uint8_t command, uint8_t* buffer, uint32_t length) {
+	int32_t errors = 0;
+	txBuffer[0] = command;
+	if(length>0){
+		memcpy(txBuffer+1, buffer, length);
+	}
+	errors += hSpi->take(100);
+	errors += hDcrsPin->reset();
+	errors += hNssPin->reset();
+	errors += hSpi->transmit(txBuffer, 1);
+	if(length>0){
+		errors += hDcrsPin->set();
+		errors += hSpi->transmit(txBuffer+1, length);
+		errors += hDcrsPin->reset();
+	}
+	errors += hNssPin->set();
+	errors += hSpi->give();
+	return errors;
 }
diff --git a/Core/Src/Components/ILI9341.hpp b/Core/Src/Components/ILI9341.hpp
index c855f09..83f9060 100644
--- a/Core/Src/Components/ILI9341.hpp
+++ b/Core/Src/Components/ILI9341.hpp
@@ -21,11 +21,17 @@ public:
 	virtual ~ILI9341();
 	int32_t init();
 private:
+	int32_t readReg8(uint8_t command, uint8_t* reg);
+	int32_t readReg24(uint8_t command, uint32_t* reg);
+	int32_t readReg32(uint8_t command, uint32_t* reg);
+	int32_t writeBuffer(uint8_t command, uint8_t* buffer, uint32_t length);
 	SPI_Peripheral* hSpi;
 	GPIO_Pin* hNssPin;
 	GPIO_Pin* hResetPin;
 	GPIO_Pin* hDcrsPin;
 	PWM_Pin* hLedPwm;
+	uint8_t txBuffer[16384];
+	uint8_t rxBuffer[16384];
 };
 
 #endif /* SRC_COMPONENTS_ILI9341_HPP_ */
diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c
index fd45581..01dfa0b 100644
--- a/Core/Src/freertos.c
+++ b/Core/Src/freertos.c
@@ -51,7 +51,7 @@
 osThreadId_t defaultTaskHandle;
 const osThreadAttr_t defaultTask_attributes = {
   .name = "defaultTask",
-  .stack_size = 128 * 4,
+  .stack_size = 1024 * 4,
   .priority = (osPriority_t) osPriorityNormal,
 };
 
diff --git a/F411VE-ILI9341.ioc b/F411VE-ILI9341.ioc
index a21ad61..176fdfb 100644
--- a/F411VE-ILI9341.ioc
+++ b/F411VE-ILI9341.ioc
@@ -25,8 +25,9 @@ Dma.SPI5_TX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
 Dma.SPI5_TX.1.PeriphInc=DMA_PINC_DISABLE
 Dma.SPI5_TX.1.Priority=DMA_PRIORITY_LOW
 Dma.SPI5_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
-FREERTOS.IPParameters=Tasks01,configMAX_TASK_NAME_LEN,configUSE_NEWLIB_REENTRANT
-FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.FootprintOK=true
+FREERTOS.IPParameters=Tasks01,configMAX_TASK_NAME_LEN,configUSE_NEWLIB_REENTRANT,FootprintOK
+FREERTOS.Tasks01=defaultTask,24,1024,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
 FREERTOS.configMAX_TASK_NAME_LEN=32
 FREERTOS.configUSE_NEWLIB_REENTRANT=1
 File.Version=6