diff --git a/User/lcd.c b/User/lcd.c
new file mode 100644
index 0000000..3fa7710
--- /dev/null
+++ b/User/lcd.c
@@ -0,0 +1,145 @@
+#include "lcd.h"
+#include "ch32v20x.h"
+
+
+void lcd_begin(lcd_t *lcd);
+void lcd_expander_write(lcd_t *lcd, uint8_t data);
+void lcd_write4bits(lcd_t *lcd, uint8_t value);
+void lcd_command(lcd_t *lcd, uint8_t cmd);
+
+
+void lcd_init(lcd_t *lcd, u8 address) {
+	lcd->address = address;
+	lcd->backlight = 0;
+
+    // Enable I2C1 and GPIOB
+    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);
+    GPIO_PinRemapConfig(GPIO_Remap_I2C1, ENABLE);
+
+    // Set up GPIO on port B
+    GPIO_InitTypeDef GPIO_InitStructure;
+    GPIO_StructInit(&GPIO_InitStructure);
+
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+
+    // SCL - PB8
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
+    GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+    // SDA - PB9
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
+    GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+    I2C_DeInit(I2C1);
+
+    I2C_InitTypeDef I2C_InitStructure;
+    I2C_StructInit(&I2C_InitStructure);
+    I2C_InitStructure.I2C_ClockSpeed = 100000;
+    I2C_Init(I2C1, &I2C_InitStructure);
+
+    I2C_Cmd(I2C1, ENABLE);
+
+    lcd_begin(lcd);
+}
+
+void lcd_begin(lcd_t *lcd) {
+	lcd->backlight = LCD_BACKLIGHT;
+	Delay_Ms(50);
+
+	lcd_expander_write(lcd, 0);
+	Delay_Ms(1000);
+
+	lcd_write4bits(lcd, 0x03 << 4);
+	Delay_Us(4500);
+	lcd_write4bits(lcd, 0x03 << 4);
+	Delay_Us(4500);
+	lcd_write4bits(lcd, 0x03 << 4);
+	Delay_Us(150);
+	lcd_write4bits(lcd, 0x02 << 4);
+
+	lcd->function = LCD_4BITMODE | LCD_2LINE;
+	lcd_command(lcd, LCD_FUNCTIONSET | lcd->function);
+
+	lcd->dispctrl = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF;
+	lcd_command(lcd, LCD_DISPLAYCONTROL | lcd->dispctrl);
+
+	lcd->entrymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDEC;
+	lcd_command(lcd, LCD_ENTRYMODESET | lcd->entrymode);
+
+	lcd_clear(lcd);
+	lcd_home(lcd);
+}
+
+void lcd_backlight(lcd_t *lcd) {
+	lcd->backlight = LCD_BACKLIGHT;
+	lcd_expander_write(lcd, 0);
+}
+
+void lcd_no_backlight(lcd_t *lcd) {
+	lcd->backlight = LCD_NOBACKLIGHT;
+	lcd_expander_write(lcd, 0);
+}
+
+void lcd_clear(lcd_t *lcd) {
+	lcd_command(lcd, LCD_CLEARDISPLAY);
+	Delay_Us(2000);
+}
+
+void lcd_home(lcd_t *lcd) {
+	lcd_command(lcd, LCD_RETURNHOME);
+	Delay_Us(2000);
+}
+
+void lcd_cursor(lcd_t *lcd, uint8_t col, uint8_t row) {
+	uint8_t row_offsets[] = {0x00, 0x40, 0x14, 0x54};
+	lcd_command(lcd, LCD_SETDDRAMADDR | (col + row_offsets[row]));
+}
+
+void lcd_expander_write(lcd_t *lcd, uint8_t data) {
+	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY) != RESET);
+	I2C_GenerateSTART(I2C1, ENABLE);
+//	Delay_Ms(1);
+
+	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
+//	I2C_Send7bitAddress(I2C1, 0x4E, I2C_Direction_Receiver);
+	I2C_SendData(I2C1, lcd->address << 1);
+//	Delay_Ms(1);
+
+	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
+
+//	I2C_SendData(I2C1, 0x27);
+//	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+	I2C_SendData(I2C1, data | lcd->backlight);
+//	Delay_Ms(1);
+	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+
+	I2C_GenerateSTOP(I2C1, ENABLE);
+//	Delay_Ms(1);
+}
+
+void lcd_write4bits(lcd_t *lcd, uint8_t value) {
+	lcd_expander_write(lcd, value);
+
+	lcd_expander_write(lcd, value | LCD_EN);
+	Delay_Us(1);
+
+	lcd_expander_write(lcd, value & ~LCD_EN);
+	Delay_Us(50);
+}
+
+void lcd_command(lcd_t *lcd, uint8_t cmd) {
+	lcd_write4bits(lcd, cmd & 0xF0);
+	lcd_write4bits(lcd, cmd << 4);
+}
+
+void lcd_write(lcd_t *lcd, char c) {
+	lcd_write4bits(lcd, (c & 0xF0) | LCD_RS);
+	lcd_write4bits(lcd, (c << 4)   | LCD_RS);
+}
+
+void lcd_puts(lcd_t *lcd, char *str) {
+	while(*str)
+		lcd_write(lcd, *str++);
+}
diff --git a/User/lcd.h b/User/lcd.h
new file mode 100644
index 0000000..e48bcfb
--- /dev/null
+++ b/User/lcd.h
@@ -0,0 +1,54 @@
+#ifndef USER_LCD_H_
+#define USER_LCD_H_
+
+#include "ch32v20x.h"
+
+static const uint8_t LCD_RS 		 = 0x01;
+static const uint8_t LCD_RW 		 = 0x02;
+static const uint8_t LCD_EN 		 = 0x04;
+static const uint8_t LCD_BACKLIGHT   = 0x08;
+static const uint8_t LCD_NOBACKLIGHT = 0x00;
+
+static const uint8_t LCD_4BITMODE = 0x00;
+static const uint8_t LCD_2LINE = 0x08;
+
+static const uint8_t LCD_CLEARDISPLAY   = 0x01;
+static const uint8_t LCD_RETURNHOME     = 0x02;
+static const uint8_t LCD_ENTRYMODESET   = 0x04;
+static const uint8_t LCD_DISPLAYCONTROL = 0x08;
+static const uint8_t LCD_CURSORSHIFT    = 0x10;
+static const uint8_t LCD_FUNCTIONSET    = 0x20;
+static const uint8_t LCD_SETCGRAMADDR   = 0x40;
+static const uint8_t LCD_SETDDRAMADDR   = 0x80;
+
+static const uint8_t LCD_BLINKOFF   = 0x00;
+static const uint8_t LCD_BLINKON    = 0x01;
+static const uint8_t LCD_CURSOROFF  = 0x00;
+static const uint8_t LCD_CURSORON   = 0x02;
+static const uint8_t LCD_DISPLAYON  = 0x04;
+static const uint8_t LCD_DISPLAYOFF = 0x00;
+
+static const uint8_t LCD_ENTRYSHIFTDEC = 0x00;
+static const uint8_t LCD_ENTRYSHIFTINC = 0x01;
+static const uint8_t LCD_ENTRYRIGHT    = 0x00;
+static const uint8_t LCD_ENTRYLEFT     = 0x02;
+
+
+typedef struct {
+	uint8_t address;
+	uint8_t backlight;
+	uint8_t function;
+	uint8_t dispctrl;
+	uint8_t entrymode;
+} lcd_t;
+
+void lcd_init(lcd_t *lcd, uint8_t address);
+void lcd_backlight(lcd_t *lcd);
+void lcd_no_backlight(lcd_t *lcd);
+void lcd_clear(lcd_t *lcd);
+void lcd_home(lcd_t *lcd);
+void lcd_write(lcd_t *lcd, char c);
+void lcd_puts(lcd_t *lcd, char *str);
+void lcd_cursor(lcd_t *lcd, uint8_t row, uint8_t col);
+
+#endif /* USER_LCD_H_ */
diff --git a/User/main.c b/User/main.c
index 363f7ee..7861fb3 100644
--- a/User/main.c
+++ b/User/main.c
@@ -4,6 +4,7 @@
 #include "memecoder.h"
 #include "ctre.h"
 #include "mode.h"
+#include "lcd.h"
 
 #include <string.h>
 
@@ -18,17 +19,17 @@ void init_switch_gpio() {
     GPIO_InitTypeDef GPIO_InitStructure;
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
-    GPIO_Init(GPIOB, &GPIO_InitStructure);
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
     GPIO_Init(GPIOB, &GPIO_InitStructure);
+//    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
+//    GPIO_Init(GPIOB, &GPIO_InitStructure);
 }
 
 
 u8 get_switch_state() {
     u8 state = 0;
-    state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)) << 0;
-    state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6)) << 1;
+//    state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)) << 0;
+//    state |= (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_6)) << 1;
     return state;
 }
 
@@ -45,6 +46,12 @@ int main(void)
     memecoder_init();
     can_init();
     ctre_init();
+	lcd_t lcd;
+	lcd_init(&lcd, 0x27);
+	lcd_backlight(&lcd);
+	lcd_puts(&lcd, "LIGMA");
+	lcd_cursor(&lcd, 2, 1);
+	lcd_puts(&lcd, "NUTS");
 //    ws2812_init();
 
     // Clear all existing ports
@@ -77,7 +84,14 @@ int main(void)
 	mode_position2_init(&app, &position2_state);
     while(1)
     {
-        memecoder_update();
+    	printf("on\r\n");
+    	char tmp[20] = {0};
+    	sprintf(tmp, "cnt: %5d", memecoder_get());
+    	lcd_cursor(&lcd, 2, 2);
+    	lcd_puts(&lcd, tmp);
+    }
+    while(1) {
+
         app.switch_state = get_switch_state();
 
         for(int i = 0; i < dev_cnt; i++) {
diff --git a/User/memecoder.c b/User/memecoder.c
index b604248..abc74ab 100644
--- a/User/memecoder.c
+++ b/User/memecoder.c
@@ -7,19 +7,19 @@ int memecoder_count = 0;
 
 
 void memecoder_init() {
-    // Enable GPIOB
+    // Enable GPIOA
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
 
-    // Set up GPIO on port B
+    // Set up GPIO on port A
     GPIO_InitTypeDef GPIO_InitStructure;
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
 
-    // PHASE_A - PB12
+    // PHASE_A - PA0
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
     GPIO_Init(GPIOA, &GPIO_InitStructure);
 
-    // PHASE_A - PB13
+    // PHASE_B - PA1
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
     GPIO_Init(GPIOA, &GPIO_InitStructure);
 
@@ -71,7 +71,7 @@ void memecoder_set(int value) {
 }
 
 int memecoder_get() {
-	return TIM_GetCounter(TIM2);
+	return (int16_t)TIM_GetCounter(TIM2) / 4;
 }
 
 void memecoder_zero() {