diff --git a/CMakeLists.txt b/CMakeLists.txt
index 94c0fca..4e5b8f1 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.13)
 set(PICO_SDK_PATH "${CMAKE_CURRENT_SOURCE_DIR}/pico-sdk")
 include(pico_sdk_import.cmake)
 
-project(SH1106 C CXX ASM)
+project(SSD1306 C CXX ASM)
 pico_sdk_init()
 
 add_subdirectory(src)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index fa70ec5..d78d2dc 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -5,5 +5,6 @@ add_executable(main
 target_link_libraries(main pico_stdlib hardware_i2c hardware_uart)
 
 pico_enable_stdio_uart(main 0)
+pico_enable_stdio_usb(main 1)
 
 pico_add_extra_outputs(main)
diff --git a/src/main.cc b/src/main.cc
index d9b63fb..4c31332 100644
--- a/src/main.cc
+++ b/src/main.cc
@@ -3,64 +3,117 @@
 #include "hardware/gpio.h"
 #include "hardware/i2c.h"
 #include "pico/error.h"
+#include <pico/stdlib.h>
+#include <pico/stdio.h>
+
+#include <cstdio>
+
+#define ADDR 0x3c
 
 static bool failure = false;
 
 int main(void) {
-  sh1106_disp disp;
+  stdio_init_all();
+
+  busy_wait_ms(1000);
 
   gpio_init(25);
   gpio_set_dir(25, GPIO_OUT);
 
+
+  i2c_init(i2c0, 115200);
+  gpio_init(0);
+  gpio_init(1);
+  gpio_set_function(0, GPIO_FUNC_I2C);
+  gpio_set_function(1, GPIO_FUNC_I2C);
+
+  ssd1306 disp;
+
   busy_wait_ms(100);
 
-  disp.poweron();
-  disp.test();
-
   while (true) {
+    /*
     gpio_put(25, false);
-    busy_wait_ms(failure ? 500 : 50);
+    busy_wait_ms(failure ? 50 : 500);
     gpio_put(25, true);
-    busy_wait_ms(failure ? 500 : 50);
+    busy_wait_ms(failure ? 50 : 500);
+    */
+    disp.test();
+    busy_wait_ms(10);
   }
 
   return 0;
 }
 
-sh1106_i2c::sh1106_i2c(uint8_t addr) : m_addr(addr) {
-  m_i2c = i2c0;
-
-  i2c_init(i2c0, 200000);
-
-  gpio_init(0);
-  gpio_init(1);
-
-  gpio_set_function(0, GPIO_FUNC_I2C);
-  gpio_set_function(1, GPIO_FUNC_I2C);
-}
-
-void sh1106_i2c::write_cmd(uint8_t cmd) {
-  failure |= i2c_write_blocking(i2c0, m_addr, &cmd, 1, false) < 0;
-}
-
-void sh1106_i2c::write_data(const uint8_t *data, size_t len) {
-  for (size_t i = 0; i < len; i++) {
-    failure |= i2c_write_blocking(i2c0, m_addr, &data[i], 1, false) < 0;
+static void write_cmd(uint8_t cmd) {
+  uint8_t buf[2] = {0x80, cmd};
+  if (i2c_write_timeout_us(i2c0, ADDR, buf, 2, false, 1000000) < 0) {
+    failure = true;
+    printf("TX fail\n");
+  } else {
+    printf("TX succ\n");
   }
 }
 
-sh1106_disp::sh1106_disp() : m_i2c_conn(0x3c) {}
-
-void sh1106_disp::poweron() { m_i2c_conn.write_cmd(SH1106_SET_DISP | 0x00); }
-
-void sh1106_disp::poweroff() { m_i2c_conn.write_cmd(SH1106_SET_DISP | 0x01); }
-
-void sh1106_disp::test() {
-  uint8_t buf[3] = {0xFF};
-  m_i2c_conn.write_cmd(SH1106_SET_PAGE_ADDRESS | 0x0);
-  m_i2c_conn.write_cmd(SH1106_SET_HIGH_COLUMN_ADDRESS | 0x2);
-  m_i2c_conn.write_cmd(SH1106_SET_LOW_COLUMN_ADDRESS | 0x0);
-  m_i2c_conn.write_cmd(SH1106_SET_CONTRAST);
-  m_i2c_conn.write_cmd(0xFF);
-  m_i2c_conn.write_cmd(SH1106_SET_NORM_INV | 0x1);
+static void write_data(uint8_t *data, size_t len) {
+  if (i2c_write_timeout_us(i2c0, ADDR, data, len, false, 1000000) < 0) {
+    failure = true;
+    printf("data fail\n");
+  } else {
+    printf("data succ\n");
+  }
+}
+
+
+ssd1306::ssd1306() {
+    write_cmd(SET_DISP | 0x00); // Off
+                                //
+    write_cmd(SET_MUX_RATIO); // The height
+    write_cmd(63); // The height
+
+    write_cmd(SET_DISP_OFFSET); // No offset
+    write_cmd(0x00); // No offset
+    
+    write_cmd(SET_DISP_START_LINE | 0x00); // Resolution and layout
+    write_cmd(SET_SEG_REMAP | 0x01);
+    write_cmd(SET_COM_OUT_DIR | 0x08); // Scan from COM[N] to COM0
+
+    write_cmd(SET_COM_PIN_CFG);
+    write_cmd(0x02);
+
+    write_cmd(SET_CONTRAST);
+    write_cmd(0xFF);
+
+    write_cmd(SET_ENTIRE_ON);
+    write_cmd(SET_NORM_INV | 0x00);
+    write_cmd(SET_DISP_CLK_DIV);
+    write_cmd(0x80);
+
+    write_cmd(SET_PRECHARGE);
+    write_cmd(0x22);
+    write_cmd(SET_CHARGE_PUMP);
+    write_cmd(0x14);
+
+    write_cmd(SET_DISP | 0x01);
+
+
+    write_cmd(SET_MEM_ADDR); 
+    write_cmd(0x00); // Horizontal
+
+    /*
+    write_cmd(SET_VCOM_DESEL);
+    write_cmd(0x30);
+    */
+    write_cmd(SET_COL_ADDR);
+    write_cmd(0);
+    write_cmd(127);
+    write_cmd(SET_PAGE_ADDR);
+    write_cmd(0);
+    write_cmd(63);
+}
+
+void ssd1306::test() {
+  static bool flip = true;
+  uint8_t buf[2] = {0x40, 0x00};
+  write_data(buf, 2);
 }
diff --git a/src/main.hh b/src/main.hh
index b0eafa8..c093401 100644
--- a/src/main.hh
+++ b/src/main.hh
@@ -3,35 +3,29 @@
 
 #include <stdint.h>
 
-const uint8_t SH1106_SET_CONTRAST = 0x81u;
-const uint8_t SH1106_SET_NORM_INV = 0xa6u;
-const uint8_t SH1106_SET_DISP = 0xaeu;
-const uint8_t SH1106_SET_SCAN_DIR = 0xc0u;
-const uint8_t SH1106_SET_SEG_REMAP = 0xa0u;
-const uint8_t SH1106_SET_LOW_COLUMN_ADDRESS = 0x00u;
-const uint8_t SH1106_SET_HIGH_COLUMN_ADDRESS = 0x10u;
-const uint8_t SH1106_SET_PAGE_ADDRESS = 0xb0u;
-
-class sh1106_i2c {
-private:
-  i2c_inst_t *m_i2c;
-  uint8_t m_addr;
+#define SET_CONTRAST (0x81)
+#define SET_ENTIRE_ON (0xA4)
+#define SET_NORM_INV (0xA6)
+#define SET_DISP (0xAE)
+#define SET_MEM_ADDR (0x20)
+#define SET_COL_ADDR (0x21)
+#define SET_PAGE_ADDR (0x22)
+#define SET_DISP_START_LINE (0x40)
+#define SET_SEG_REMAP (0xA0)
+#define SET_MUX_RATIO (0xA8)
+#define SET_COM_OUT_DIR (0xC0)
+#define SET_DISP_OFFSET (0xD3)
+#define SET_COM_PIN_CFG (0xDA)
+#define SET_DISP_CLK_DIV (0xD5)
+#define SET_PRECHARGE (0xD9)
+#define SET_VCOM_DESEL (0xDB)
+#define SET_CHARGE_PUMP (0x8D)
 
+class ssd1306 {
 public:
-  sh1106_i2c(uint8_t addr);
-
-  void write_cmd(uint8_t cmd);
-  void write_data(const uint8_t *data, size_t len);
-};
-
-class sh1106_disp {
-private:
-  sh1106_i2c m_i2c_conn;
-
-public:
-  sh1106_disp();
+  ssd1306();
 
   void poweron();
-  void poweroff();
   void test();
 };
+