diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index 8aa291e..6970ecf 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -1,4 +1,4 @@
 idf_component_register(
-    SRCS "main.cpp"
+    SRCS "ssd1366.cpp" "main.cpp"
     INCLUDE_DIRS "."
     REQUIRES ttn-esp32)
diff --git a/main/font8x8_basic.h b/main/font8x8_basic.h
new file mode 100644
index 0000000..1d39023
--- /dev/null
+++ b/main/font8x8_basic.h
@@ -0,0 +1,174 @@
+/*
+ * font8x8_basic.h
+ *
+ *  Created on: 2017/05/03
+ *      Author: yanbe
+ */
+
+#ifndef MAIN_FONT8X8_BASIC_H_
+#define MAIN_FONT8X8_BASIC_H_
+
+#include "freertos/FreeRTOS.h"
+
+/*
+   Constant: font8x8_basic_tr
+   Contains an 90 digree transposed 8x8 font map for unicode points 
+   U+0000 - U+007F (basic latin)
+   
+   To make it easy to use with SSD1306's GDDRAM mapping and API,
+   this constant is an 90 degree transposed.
+   The original version written by Marcel Sondaar is availble at:
+   https://github.com/dhepper/font8x8/blob/master/font8x8_basic.h 
+   Conversion is done via following procedure:
+   
+	for (int code = 0; code < 128; code++) {
+		uint8_t trans[8];
+		for (int w = 0; w < 8; w++) {
+			trans[w] = 0x00;
+			for (int b = 0; b < 8; b++) {
+				trans[w] |= ((font8x8_basic[code][b] & (1 << w)) >> w) << b;
+			}
+		}
+	
+		for (int w = 0; w < 8; w++) {
+			if (w == 0) { printf("    { "); }
+			printf("0x%.2X", trans[w]);
+			if (w < 7) { printf(", "); }
+			if (w == 7) { printf(" },   // U+00%.2X (%c)\n", code, code); }
+		}
+	}
+*/
+
+uint8_t font8x8_basic_tr[128][8] = {
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0000 (nul)
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0001
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0002
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0003
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0004
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0005
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0006
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0007
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0008
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0009
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+000A
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+000B
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+000C
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+000D
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+000E
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+000F
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0010
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0011
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0012
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0013
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0014
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0015
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0016
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0017
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0018
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0019
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+001A
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+001B
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+001C
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+001D
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+001E
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+001F
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0020 (space)
+    { 0x00, 0x00, 0x06, 0x5F, 0x5F, 0x06, 0x00, 0x00 },   // U+0021 (!)
+    { 0x00, 0x03, 0x03, 0x00, 0x03, 0x03, 0x00, 0x00 },   // U+0022 (")
+    { 0x14, 0x7F, 0x7F, 0x14, 0x7F, 0x7F, 0x14, 0x00 },   // U+0023 (#)
+    { 0x24, 0x2E, 0x6B, 0x6B, 0x3A, 0x12, 0x00, 0x00 },   // U+0024 ($)
+    { 0x46, 0x66, 0x30, 0x18, 0x0C, 0x66, 0x62, 0x00 },   // U+0025 (%)
+    { 0x30, 0x7A, 0x4F, 0x5D, 0x37, 0x7A, 0x48, 0x00 },   // U+0026 (&)
+    { 0x04, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00 },   // U+0027 (')
+    { 0x00, 0x1C, 0x3E, 0x63, 0x41, 0x00, 0x00, 0x00 },   // U+0028 (()
+    { 0x00, 0x41, 0x63, 0x3E, 0x1C, 0x00, 0x00, 0x00 },   // U+0029 ())
+    { 0x08, 0x2A, 0x3E, 0x1C, 0x1C, 0x3E, 0x2A, 0x08 },   // U+002A (*)
+    { 0x08, 0x08, 0x3E, 0x3E, 0x08, 0x08, 0x00, 0x00 },   // U+002B (+)
+    { 0x00, 0x80, 0xE0, 0x60, 0x00, 0x00, 0x00, 0x00 },   // U+002C (,)
+    { 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00 },   // U+002D (-)
+    { 0x00, 0x00, 0x60, 0x60, 0x00, 0x00, 0x00, 0x00 },   // U+002E (.)
+    { 0x60, 0x30, 0x18, 0x0C, 0x06, 0x03, 0x01, 0x00 },   // U+002F (/)
+    { 0x3E, 0x7F, 0x71, 0x59, 0x4D, 0x7F, 0x3E, 0x00 },   // U+0030 (0)
+    { 0x40, 0x42, 0x7F, 0x7F, 0x40, 0x40, 0x00, 0x00 },   // U+0031 (1)
+    { 0x62, 0x73, 0x59, 0x49, 0x6F, 0x66, 0x00, 0x00 },   // U+0032 (2)
+    { 0x22, 0x63, 0x49, 0x49, 0x7F, 0x36, 0x00, 0x00 },   // U+0033 (3)
+    { 0x18, 0x1C, 0x16, 0x53, 0x7F, 0x7F, 0x50, 0x00 },   // U+0034 (4)
+    { 0x27, 0x67, 0x45, 0x45, 0x7D, 0x39, 0x00, 0x00 },   // U+0035 (5)
+    { 0x3C, 0x7E, 0x4B, 0x49, 0x79, 0x30, 0x00, 0x00 },   // U+0036 (6)
+    { 0x03, 0x03, 0x71, 0x79, 0x0F, 0x07, 0x00, 0x00 },   // U+0037 (7)
+    { 0x36, 0x7F, 0x49, 0x49, 0x7F, 0x36, 0x00, 0x00 },   // U+0038 (8)
+    { 0x06, 0x4F, 0x49, 0x69, 0x3F, 0x1E, 0x00, 0x00 },   // U+0039 (9)
+    { 0x00, 0x00, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00 },   // U+003A (:)
+    { 0x00, 0x80, 0xE6, 0x66, 0x00, 0x00, 0x00, 0x00 },   // U+003B (;)
+    { 0x08, 0x1C, 0x36, 0x63, 0x41, 0x00, 0x00, 0x00 },   // U+003C (<)
+    { 0x24, 0x24, 0x24, 0x24, 0x24, 0x24, 0x00, 0x00 },   // U+003D (=)
+    { 0x00, 0x41, 0x63, 0x36, 0x1C, 0x08, 0x00, 0x00 },   // U+003E (>)
+    { 0x02, 0x03, 0x51, 0x59, 0x0F, 0x06, 0x00, 0x00 },   // U+003F (?)
+    { 0x3E, 0x7F, 0x41, 0x5D, 0x5D, 0x1F, 0x1E, 0x00 },   // U+0040 (@)
+    { 0x7C, 0x7E, 0x13, 0x13, 0x7E, 0x7C, 0x00, 0x00 },   // U+0041 (A)
+    { 0x41, 0x7F, 0x7F, 0x49, 0x49, 0x7F, 0x36, 0x00 },   // U+0042 (B)
+    { 0x1C, 0x3E, 0x63, 0x41, 0x41, 0x63, 0x22, 0x00 },   // U+0043 (C)
+    { 0x41, 0x7F, 0x7F, 0x41, 0x63, 0x3E, 0x1C, 0x00 },   // U+0044 (D)
+    { 0x41, 0x7F, 0x7F, 0x49, 0x5D, 0x41, 0x63, 0x00 },   // U+0045 (E)
+    { 0x41, 0x7F, 0x7F, 0x49, 0x1D, 0x01, 0x03, 0x00 },   // U+0046 (F)
+    { 0x1C, 0x3E, 0x63, 0x41, 0x51, 0x73, 0x72, 0x00 },   // U+0047 (G)
+    { 0x7F, 0x7F, 0x08, 0x08, 0x7F, 0x7F, 0x00, 0x00 },   // U+0048 (H)
+    { 0x00, 0x41, 0x7F, 0x7F, 0x41, 0x00, 0x00, 0x00 },   // U+0049 (I)
+    { 0x30, 0x70, 0x40, 0x41, 0x7F, 0x3F, 0x01, 0x00 },   // U+004A (J)
+    { 0x41, 0x7F, 0x7F, 0x08, 0x1C, 0x77, 0x63, 0x00 },   // U+004B (K)
+    { 0x41, 0x7F, 0x7F, 0x41, 0x40, 0x60, 0x70, 0x00 },   // U+004C (L)
+    { 0x7F, 0x7F, 0x0E, 0x1C, 0x0E, 0x7F, 0x7F, 0x00 },   // U+004D (M)
+    { 0x7F, 0x7F, 0x06, 0x0C, 0x18, 0x7F, 0x7F, 0x00 },   // U+004E (N)
+    { 0x1C, 0x3E, 0x63, 0x41, 0x63, 0x3E, 0x1C, 0x00 },   // U+004F (O)
+    { 0x41, 0x7F, 0x7F, 0x49, 0x09, 0x0F, 0x06, 0x00 },   // U+0050 (P)
+    { 0x1E, 0x3F, 0x21, 0x71, 0x7F, 0x5E, 0x00, 0x00 },   // U+0051 (Q)
+    { 0x41, 0x7F, 0x7F, 0x09, 0x19, 0x7F, 0x66, 0x00 },   // U+0052 (R)
+    { 0x26, 0x6F, 0x4D, 0x59, 0x73, 0x32, 0x00, 0x00 },   // U+0053 (S)
+    { 0x03, 0x41, 0x7F, 0x7F, 0x41, 0x03, 0x00, 0x00 },   // U+0054 (T)
+    { 0x7F, 0x7F, 0x40, 0x40, 0x7F, 0x7F, 0x00, 0x00 },   // U+0055 (U)
+    { 0x1F, 0x3F, 0x60, 0x60, 0x3F, 0x1F, 0x00, 0x00 },   // U+0056 (V)
+    { 0x7F, 0x7F, 0x30, 0x18, 0x30, 0x7F, 0x7F, 0x00 },   // U+0057 (W)
+    { 0x43, 0x67, 0x3C, 0x18, 0x3C, 0x67, 0x43, 0x00 },   // U+0058 (X)
+    { 0x07, 0x4F, 0x78, 0x78, 0x4F, 0x07, 0x00, 0x00 },   // U+0059 (Y)
+    { 0x47, 0x63, 0x71, 0x59, 0x4D, 0x67, 0x73, 0x00 },   // U+005A (Z)
+    { 0x00, 0x7F, 0x7F, 0x41, 0x41, 0x00, 0x00, 0x00 },   // U+005B ([)
+    { 0x01, 0x03, 0x06, 0x0C, 0x18, 0x30, 0x60, 0x00 },   // U+005C (\)
+    { 0x00, 0x41, 0x41, 0x7F, 0x7F, 0x00, 0x00, 0x00 },   // U+005D (])
+    { 0x08, 0x0C, 0x06, 0x03, 0x06, 0x0C, 0x08, 0x00 },   // U+005E (^)
+    { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 },   // U+005F (_)
+    { 0x00, 0x00, 0x03, 0x07, 0x04, 0x00, 0x00, 0x00 },   // U+0060 (`)
+    { 0x20, 0x74, 0x54, 0x54, 0x3C, 0x78, 0x40, 0x00 },   // U+0061 (a)
+    { 0x41, 0x7F, 0x3F, 0x48, 0x48, 0x78, 0x30, 0x00 },   // U+0062 (b)
+    { 0x38, 0x7C, 0x44, 0x44, 0x6C, 0x28, 0x00, 0x00 },   // U+0063 (c)
+    { 0x30, 0x78, 0x48, 0x49, 0x3F, 0x7F, 0x40, 0x00 },   // U+0064 (d)
+    { 0x38, 0x7C, 0x54, 0x54, 0x5C, 0x18, 0x00, 0x00 },   // U+0065 (e)
+    { 0x48, 0x7E, 0x7F, 0x49, 0x03, 0x02, 0x00, 0x00 },   // U+0066 (f)
+    { 0x98, 0xBC, 0xA4, 0xA4, 0xF8, 0x7C, 0x04, 0x00 },   // U+0067 (g)
+    { 0x41, 0x7F, 0x7F, 0x08, 0x04, 0x7C, 0x78, 0x00 },   // U+0068 (h)
+    { 0x00, 0x44, 0x7D, 0x7D, 0x40, 0x00, 0x00, 0x00 },   // U+0069 (i)
+    { 0x60, 0xE0, 0x80, 0x80, 0xFD, 0x7D, 0x00, 0x00 },   // U+006A (j)
+    { 0x41, 0x7F, 0x7F, 0x10, 0x38, 0x6C, 0x44, 0x00 },   // U+006B (k)
+    { 0x00, 0x41, 0x7F, 0x7F, 0x40, 0x00, 0x00, 0x00 },   // U+006C (l)
+    { 0x7C, 0x7C, 0x18, 0x38, 0x1C, 0x7C, 0x78, 0x00 },   // U+006D (m)
+    { 0x7C, 0x7C, 0x04, 0x04, 0x7C, 0x78, 0x00, 0x00 },   // U+006E (n)
+    { 0x38, 0x7C, 0x44, 0x44, 0x7C, 0x38, 0x00, 0x00 },   // U+006F (o)
+    { 0x84, 0xFC, 0xF8, 0xA4, 0x24, 0x3C, 0x18, 0x00 },   // U+0070 (p)
+    { 0x18, 0x3C, 0x24, 0xA4, 0xF8, 0xFC, 0x84, 0x00 },   // U+0071 (q)
+    { 0x44, 0x7C, 0x78, 0x4C, 0x04, 0x1C, 0x18, 0x00 },   // U+0072 (r)
+    { 0x48, 0x5C, 0x54, 0x54, 0x74, 0x24, 0x00, 0x00 },   // U+0073 (s)
+    { 0x00, 0x04, 0x3E, 0x7F, 0x44, 0x24, 0x00, 0x00 },   // U+0074 (t)
+    { 0x3C, 0x7C, 0x40, 0x40, 0x3C, 0x7C, 0x40, 0x00 },   // U+0075 (u)
+    { 0x1C, 0x3C, 0x60, 0x60, 0x3C, 0x1C, 0x00, 0x00 },   // U+0076 (v)
+    { 0x3C, 0x7C, 0x70, 0x38, 0x70, 0x7C, 0x3C, 0x00 },   // U+0077 (w)
+    { 0x44, 0x6C, 0x38, 0x10, 0x38, 0x6C, 0x44, 0x00 },   // U+0078 (x)
+    { 0x9C, 0xBC, 0xA0, 0xA0, 0xFC, 0x7C, 0x00, 0x00 },   // U+0079 (y)
+    { 0x4C, 0x64, 0x74, 0x5C, 0x4C, 0x64, 0x00, 0x00 },   // U+007A (z)
+    { 0x08, 0x08, 0x3E, 0x77, 0x41, 0x41, 0x00, 0x00 },   // U+007B ({)
+    { 0x00, 0x00, 0x00, 0x77, 0x77, 0x00, 0x00, 0x00 },   // U+007C (|)
+    { 0x41, 0x41, 0x77, 0x3E, 0x08, 0x08, 0x00, 0x00 },   // U+007D (})
+    { 0x02, 0x03, 0x01, 0x03, 0x02, 0x03, 0x01, 0x00 },   // U+007E (~)
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }    // U+007F
+};
+
+#endif /* MAIN_FONT8X8_BASIC_H_ */
+
diff --git a/main/main.cpp b/main/main.cpp
index f57ba0a..755b48c 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -1,55 +1,24 @@
-/*******************************************************************************
- * 
- * ttn-esp32 - The Things Network device library for ESP-IDF / SX127x
- * 
- * Copyright (c) 2018 Manuel Bleichenbacher
- * 
- * Licensed under MIT License
- * https://opensource.org/licenses/MIT
- *
- * Sample program showing how to send and receive messages.
- *******************************************************************************/
-
-#include "freertos/FreeRTOS.h"
-#include "esp_event.h"
-#include "driver/gpio.h"
-#include "nvs_flash.h"
-#include "driver/i2c.h"
-
-#include "TheThingsNetwork.h"
-
-#define SDA_PIN GPIO_NUM_4
-#define SCL_PIN GPIO_NUM_15
-
-
-// NOTE:
-// The LoRaWAN frequency and the radio chip must be configured by running 'idf.py menuconfig'.
-// Go to Components / The Things Network, select the appropriate values and save.
-
-// Pins and other resources
-#define TTN_SPI_HOST      HSPI_HOST
-#define TTN_SPI_DMA_CHAN  1
-#define TTN_PIN_SPI_SCLK  5
-#define TTN_PIN_SPI_MOSI  27
-#define TTN_PIN_SPI_MISO  19
-#define TTN_PIN_NSS       18
-#define TTN_PIN_RXTX      TTN_NOT_CONNECTED
-#define TTN_PIN_RST       14
-#define TTN_PIN_DIO0      26
-#define TTN_PIN_DIO1      35
-
-static TheThingsNetwork ttn;
-
-const unsigned TX_INTERVAL = 5;
-static uint8_t msgData[] = "0xBADC0DED";
+#include "main.h"
 
+#include "ssd1366.h"
 
 void sendMessages(void* pvParameter)
 {
     while (1) {
         printf("Sending message...\n");
         TTNResponseCode res = ttn.transmitMessage(msgData, sizeof(msgData) - 1);
-        printf(res == kTTNSuccessfulTransmission ? "Message sent.\n" : "Transmission failed.\n");
+        
+        if(res == kTTNSuccessfulTransmission){
+            printf("Message sent.\n");
+            ssd1306_display_clear();
+            const char *pcTask1 = "Message sent.\n";
+	        xTaskCreate(&task_ssd1306_display_text, "task_ssd1306_display_text", 4096, (void *) pcTask1, 5, NULL);
+        }else{
+            printf("Transmission failed.\n");
+            ssd1306_display_clear();
+            const char *pcTask1 = "Transmission failed.\n";
+	        xTaskCreate(&task_ssd1306_display_text, "task_ssd1306_display_text", 4096, (void *) pcTask1, 5, NULL);
+        }
 
         vTaskDelay(TX_INTERVAL * pdMS_TO_TICKS(1000));
     }
@@ -94,14 +63,20 @@ extern "C" void app_main(void)
     // Register callback for received messages
     ttn.onMessage(messageReceived);
 
-//    ttn.setAdrEnabled(false);
-//    ttn.setDataRate(kTTNDataRate_US915_SF7);
-//    ttn.setMaxTxPower(14);
+
+    i2c_master_init();
+	ssd1306_init();
 
     printf("Joining...\n");
+    ssd1306_display_clear();
+    const char *pcTask1 = "Joining...\n";
+	xTaskCreate(&task_ssd1306_display_text, "task_ssd1306_display_text", 4096, (void *) pcTask1, 5, NULL);
     if (ttn.join())
     {
         printf("Joined.\n");
+        ssd1306_display_clear();
+        const char *pcTask1 = "Joined.\n";
+	    xTaskCreate(&task_ssd1306_display_text, "task_ssd1306_display_text", 4096, (void *) pcTask1, 5, NULL);
         xTaskCreate(sendMessages, "send_messages", 1024 * 4, (void* )0, 3, nullptr);
     }
     else
diff --git a/main/main.h b/main/main.h
new file mode 100644
index 0000000..6032db7
--- /dev/null
+++ b/main/main.h
@@ -0,0 +1,33 @@
+#ifndef MAIN_H
+#define MAIN_H
+
+#include "freertos/FreeRTOS.h"
+#include "esp_event.h"
+#include "driver/gpio.h"
+#include "nvs_flash.h"
+#include "driver/i2c.h"
+
+#include "TheThingsNetwork.h"
+
+#define SDA_PIN GPIO_NUM_4
+#define SCL_PIN GPIO_NUM_15
+
+// Pins and other resources
+#define TTN_SPI_HOST      HSPI_HOST
+#define TTN_SPI_DMA_CHAN  1
+#define TTN_PIN_SPI_SCLK  5
+#define TTN_PIN_SPI_MOSI  27
+#define TTN_PIN_SPI_MISO  19
+#define TTN_PIN_NSS       18
+#define TTN_PIN_RXTX      TTN_NOT_CONNECTED
+#define TTN_PIN_RST       14
+#define TTN_PIN_DIO0      26
+#define TTN_PIN_DIO1      35
+
+static TheThingsNetwork ttn;
+
+const unsigned TX_INTERVAL = 5;
+static uint8_t msgData[] = "0xBADC0DED";
+
+
+#endif /* MAIN_H */
\ No newline at end of file
diff --git a/main/ssd1366.cpp b/main/ssd1366.cpp
new file mode 100644
index 0000000..ba86219
--- /dev/null
+++ b/main/ssd1366.cpp
@@ -0,0 +1,204 @@
+
+#include "ssd1366.h"
+#include "font8x8_basic.h"
+#include <string.h>
+#include "driver/gpio.h"
+
+#define SSD1366_SDA_PIN GPIO_NUM_4
+#define SSD1366_SCL_PIN GPIO_NUM_15
+#define SSD1366_RST_PIN GPIO_NUM_16
+#define SSD1366_VEXT_PIN GPIO_NUM_21
+
+
+
+
+#define I2C_NUM I2C_NUM_0
+//#define I2C_NUM I2C_NUM
+
+#define I2C_MASTER_FREQ_HZ 400000U /*!< I2C clock of SSD1306 can run at 400 kHz max. */
+
+
+#define tag "SSD1306"
+
+void i2c_master_init()
+{
+	i2c_config_t i2c_config;
+
+		i2c_config.mode = I2C_MODE_MASTER;
+		i2c_config.sda_io_num = SSD1366_SDA_PIN;
+		i2c_config.scl_io_num = SSD1366_SCL_PIN;
+		i2c_config.sda_pullup_en = GPIO_PULLUP_ENABLE;
+		i2c_config.scl_pullup_en = GPIO_PULLUP_ENABLE;
+		i2c_config.master.clk_speed = I2C_MASTER_FREQ_HZ;
+
+
+	ESP_ERROR_CHECK(i2c_param_config(I2C_NUM, &i2c_config));
+	ESP_ERROR_CHECK(i2c_driver_install(I2C_NUM, I2C_MODE_MASTER, 0, 0, 0));
+}
+
+void ssd1306_init()
+{
+	esp_err_t espRc;
+	gpio_set_direction(SSD1366_RST_PIN, GPIO_MODE_OUTPUT);
+	gpio_set_direction(SSD1366_VEXT_PIN, GPIO_MODE_OUTPUT);
+	gpio_set_level(SSD1366_VEXT_PIN, 0); //enable power to oled
+	gpio_set_level(SSD1366_RST_PIN, 0); 
+	vTaskDelay(50 / portTICK_PERIOD_MS);
+	gpio_set_level(SSD1366_RST_PIN, 1); 
+
+	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
+
+	i2c_master_start(cmd);
+	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
+	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
+
+	i2c_master_write_byte(cmd, OLED_CMD_DISPLAY_OFF, true);				// AE
+	i2c_master_write_byte(cmd, OLED_CMD_SET_MUX_RATIO, true);			// A8
+	i2c_master_write_byte(cmd, 0x3F, true);
+	i2c_master_write_byte(cmd, OLED_CMD_SET_DISPLAY_OFFSET, true);		// D3
+	i2c_master_write_byte(cmd, 0x00, true);
+	i2c_master_write_byte(cmd, OLED_CMD_SET_DISPLAY_START_LINE, true);	// 40
+
+	i2c_master_write_byte(cmd, OLED_CMD_SET_SEGMENT_REMAP, true); // reverse left-right mapping
+	i2c_master_write_byte(cmd, OLED_CMD_SET_COM_SCAN_MODE, true); // reverse up-bottom mapping
+
+	i2c_master_write_byte(cmd, OLED_CMD_SET_DISPLAY_CLK_DIV, true);		// D5
+	i2c_master_write_byte(cmd, 0x80, true);
+	i2c_master_write_byte(cmd, OLED_CMD_SET_COM_PIN_MAP, true);			// DA
+	i2c_master_write_byte(cmd, 0x12, true);
+	i2c_master_write_byte(cmd, OLED_CMD_SET_CONTRAST, true);			// 81
+	i2c_master_write_byte(cmd, 0xFF, true);
+	i2c_master_write_byte(cmd, OLED_CMD_DISPLAY_RAM, true);				// A4
+	i2c_master_write_byte(cmd, OLED_CMD_SET_VCOMH_DESELCT, true);		// DB
+	i2c_master_write_byte(cmd, 0x40, true);
+	i2c_master_write_byte(cmd, OLED_CMD_SET_MEMORY_ADDR_MODE, true);	// 20
+	//i2c_master_write_byte(cmd, OLED_CMD_SET_HORI_ADDR_MODE, true);	// 00
+	i2c_master_write_byte(cmd, OLED_CMD_SET_PAGE_ADDR_MODE, true);		// 02
+	// Set Lower Column Start Address for Page Addressing Mode
+	i2c_master_write_byte(cmd, 0x00, true);
+	// Set Higher Column Start Address for Page Addressing Mode
+	i2c_master_write_byte(cmd, 0x10, true);
+	i2c_master_write_byte(cmd, OLED_CMD_SET_CHARGE_PUMP, true);			// 8D
+	i2c_master_write_byte(cmd, 0x14, true);
+	i2c_master_write_byte(cmd, OLED_CMD_DEACTIVE_SCROLL, true);			// 2E
+	i2c_master_write_byte(cmd, OLED_CMD_DISPLAY_NORMAL, true);			// A6
+	i2c_master_write_byte(cmd, OLED_CMD_DISPLAY_ON, true);
+
+	i2c_master_stop(cmd);
+
+	espRc = i2c_master_cmd_begin(I2C_NUM, cmd, 10/portTICK_PERIOD_MS);
+	if (espRc == ESP_OK)
+	{
+		ESP_LOGI(tag, "OLED configured successfully");
+	}
+	else
+	{
+		ESP_LOGE(tag, "OLED configuration failed. code: 0x%.2X", espRc);
+	}
+	i2c_cmd_link_delete(cmd);
+}
+
+void i2c_contrast( int contrast) {
+	i2c_cmd_handle_t cmd;
+	int _contrast = contrast;
+	if (contrast < 0x0) _contrast = 0;
+	if (contrast > 0xFF) _contrast = 0xFF;
+
+	cmd = i2c_cmd_link_create();
+	i2c_master_start(cmd);
+	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
+	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
+	i2c_master_write_byte(cmd, OLED_CMD_SET_CONTRAST, true);			// 81
+	i2c_master_write_byte(cmd, _contrast, true);
+	i2c_master_stop(cmd);
+	i2c_master_cmd_begin(I2C_NUM, cmd, 10/portTICK_PERIOD_MS);
+	i2c_cmd_link_delete(cmd);
+}
+
+void ssd1306_display_clear(void) {
+  i2c_cmd_handle_t cmd;
+  uint8_t zero[128] = {0};
+  for (uint8_t i = 0; i < 8; i++) {
+    cmd = i2c_cmd_link_create();
+    i2c_master_start(cmd);
+      i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
+      i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_SINGLE, true);
+      i2c_master_write_byte(cmd, 0xB0 | i, true); // Set GDDRAM page start address
+        i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_DATA_STREAM, true);
+        i2c_master_write(cmd, zero, 128, true);
+    i2c_master_stop(cmd);
+    i2c_master_cmd_begin(I2C_NUM_0, cmd, 10/portTICK_PERIOD_MS);
+    i2c_cmd_link_delete(cmd);
+  }
+}
+
+void ssd1306_display_text(char* text, uint16_t text_len)
+{
+
+	i2c_cmd_handle_t cmd;
+
+	uint8_t cur_page = 0;
+
+	cmd = i2c_cmd_link_create();
+	i2c_master_start(cmd);
+	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
+
+	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
+	i2c_master_write_byte(cmd, 0x00, true); // reset column
+	i2c_master_write_byte(cmd, 0x10, true);
+	i2c_master_write_byte(cmd, 0xB0 | cur_page, true); // reset page
+
+	i2c_master_stop(cmd);
+	i2c_master_cmd_begin(I2C_NUM, cmd, 10/portTICK_PERIOD_MS);
+	i2c_cmd_link_delete(cmd);
+
+	for (uint8_t i = 0; i < text_len; i++)
+	{
+		if (text[i] == '\n')
+		{
+			cmd = i2c_cmd_link_create();
+			i2c_master_start(cmd);
+			i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
+
+			i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
+			i2c_master_write_byte(cmd, 0x00, true); // reset column
+			i2c_master_write_byte(cmd, 0x10, true);
+			i2c_master_write_byte(cmd, 0xB0 | ++cur_page, true); // increment page
+
+			i2c_master_stop(cmd);
+			i2c_master_cmd_begin(I2C_NUM, cmd, 10/portTICK_PERIOD_MS);
+			i2c_cmd_link_delete(cmd);
+		}
+		else
+		{
+			cmd = i2c_cmd_link_create();
+			i2c_master_start(cmd);
+			i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
+
+			i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_DATA_STREAM, true);
+			i2c_master_write(cmd, font8x8_basic_tr[(uint8_t)text[i]], 8, true);
+
+			i2c_master_stop(cmd);
+			i2c_master_cmd_begin(I2C_NUM, cmd, 10/portTICK_PERIOD_MS);
+			i2c_cmd_link_delete(cmd);
+		}
+	}
+}
+
+
+void task_ssd1306_display_text(void *pvParameters)
+{
+	char *pcTaskName;
+  	pcTaskName = (char *) pvParameters;
+
+	//char text[13] = "Hello world!";
+	//uint8_t text_len = strlen(text);
+
+	ssd1306_display_text(pcTaskName,  strlen(pcTaskName));
+
+	vTaskDelete(NULL);
+}
+
+
+	
+
diff --git a/main/ssd1366.h b/main/ssd1366.h
new file mode 100644
index 0000000..13fa6d0
--- /dev/null
+++ b/main/ssd1366.h
@@ -0,0 +1,65 @@
+#ifndef MAIN_SSD1366_H_
+#define MAIN_SSD1366_H_
+
+#include "freertos/FreeRTOS.h"
+#include "driver/gpio.h"
+#include "driver/i2c.h"
+#include "esp_log.h"
+
+// SLA (0x3C) + WRITE_MODE (0x00) =  0x78 (0b01111000)
+#define OLED_I2C_ADDRESS   0x3C
+
+// Control byte
+#define OLED_CONTROL_BYTE_CMD_SINGLE    0x80
+#define OLED_CONTROL_BYTE_CMD_STREAM    0x00
+#define OLED_CONTROL_BYTE_DATA_STREAM   0x40
+
+// Fundamental commands (pg.28)
+#define OLED_CMD_SET_CONTRAST           0x81    // follow with 0x7F
+#define OLED_CMD_DISPLAY_RAM            0xA4
+#define OLED_CMD_DISPLAY_ALLON          0xA5
+#define OLED_CMD_DISPLAY_NORMAL         0xA6
+#define OLED_CMD_DISPLAY_INVERTED       0xA7
+#define OLED_CMD_DISPLAY_OFF            0xAE
+#define OLED_CMD_DISPLAY_ON             0xAF
+
+// Addressing Command Table (pg.30)
+#define OLED_CMD_SET_MEMORY_ADDR_MODE   0x20    // follow with 0x00 = HORZ mode = Behave like a KS108 graphic LCD
+#define OLED_CMD_SET_COLUMN_RANGE       0x21    // can be used only in HORZ/VERT mode - follow with 0x00 and 0x7F = COL127
+#define OLED_CMD_SET_PAGE_RANGE         0x22    // can be used only in HORZ/VERT mode - follow with 0x00 and 0x07 = PAGE7
+#define OLED_CMD_SET_PAGE_ADDR_MODE     0x02    // Page Addressing Mode
+
+// Hardware Config (pg.31)
+#define OLED_CMD_SET_DISPLAY_START_LINE 0x40
+#define OLED_CMD_SET_SEGMENT_REMAP      0xA1    
+#define OLED_CMD_SET_MUX_RATIO          0xA8    // follow with 0x3F = 64 MUX
+#define OLED_CMD_SET_COM_SCAN_MODE      0xC8    
+#define OLED_CMD_SET_DISPLAY_OFFSET     0xD3    // follow with 0x00
+#define OLED_CMD_SET_COM_PIN_MAP        0xDA    // follow with 0x12
+#define OLED_CMD_NOP                    0xE3    // NOP
+
+// Timing and Driving Scheme (pg.32)
+#define OLED_CMD_SET_DISPLAY_CLK_DIV    0xD5    // follow with 0x80
+#define OLED_CMD_SET_PRECHARGE          0xD9    // follow with 0xF1
+#define OLED_CMD_SET_VCOMH_DESELCT      0xDB    // follow with 0x30
+
+// Charge Pump (pg.62)
+#define OLED_CMD_SET_CHARGE_PUMP        0x8D    // follow with 0x14
+
+// Scrolling Command
+#define OLED_CMD_HORIZONTAL_RIGHT       0x26
+#define OLED_CMD_HORIZONTAL_LEFT        0x27
+#define OLED_CMD_CONTINUOUS_SCROLL      0x29
+#define OLED_CMD_DEACTIVE_SCROLL        0x2E
+#define OLED_CMD_ACTIVE_SCROLL          0x2F
+#define OLED_CMD_VERTICAL               0xA3
+
+
+void i2c_master_init();
+void ssd1306_init();
+void task_ssd1306_display_text(void *pvParameters);
+void i2c_contrast( int contrast);
+void ssd1306_display_clear(void);
+void ssd1306_display_text(char* text, uint16_t text_len);
+
+#endif /* MAIN_SSD1366_H_ */
\ No newline at end of file