diff --git a/.gitignore b/.gitignore
index 9e57fc8..471169c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,7 @@
 .vscode/
 hoverserial.code-workspace
 .piolibdeps/
+hoverserial.ino
+build_opt.h
+.gcc-flags.json
+.clang_complete
diff --git a/platformio.ini b/platformio.ini
index b1d6153..32a2fc2 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -9,7 +9,8 @@
 ; https://docs.platformio.org/page/projectconf.html
 
 [platformio]
-default_envs = leonardo
+; default_envs = leonardo
+default_envs = stm32-411
 
 [common_env_data]
 
@@ -21,14 +22,7 @@ lib_deps =
     Nintendo Extension Ctrl
 monitor_port = /dev/ttyACM*
 upload_port = /dev/ttyACM*
-
-
-; [env:pro_micro]
-; board = sparkfun_promicro16
-; framework = arduino
-; platform = atmelavr
-; upload_port = /dev/ttyACM*
-; monitor_port = /dev/ttyACM*
+monitor_speed = 115200
 
 [env:usbasp]
 platform = atmelavr
@@ -41,6 +35,20 @@ upload_protocol = usbasp
 upload_flags =
     -Pusb
 
+[env:stm32-411]
+platform = ststm32
+framework = arduino
+board = nucleo_f411re
+build_flags =
+  -Wno-deprecated
+lib_deps =
+    Nintendo Extension Ctrl
+
+debug_tool = stlink
+
+monitor_port = /dev/ttyACM*
+monitor_speed = 115200
+
 [env:stm32]
 platform = ststm32
 framework = arduino
diff --git a/src/build_opt.h b/src/build_opt.h
index 78f8159..c60b7a1 100644
--- a/src/build_opt.h
+++ b/src/build_opt.h
@@ -1,3 +1,2 @@
--DENABLE_HWSERIAL2
--DPIN_SERIAL3_RX=PA10
--DPIN_SERIAL3_TX=PA9
\ No newline at end of file
+-DENABLE_HWSERIAL1
+-DPIN_SERIAL1_RX=PA_10 -DPIN_SERIAL1_TX=PA_9
diff --git a/src/hoverserial.ino b/src/hoverserial.ino
index 4136aa6..a233ba5 100644
--- a/src/hoverserial.ino
+++ b/src/hoverserial.ino
@@ -34,7 +34,7 @@
 #define HOVER_SERIAL_BAUD   38400       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
 #define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)
 #define START_FRAME         0xAAAA      // [-] Start frme definition for reliable serial communication
-#define TIME_SEND           100        // [ms] Sending time interval
+#define TIME_SEND           25        // [ms] Sending time interval
 #define SPEED_MAX_TEST      300 // [-] Maximum speed for testing
 //#define DEBUG_RX                        // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
 
@@ -92,6 +92,7 @@ int motor_test_direction = 1;
 Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);
 // Connect to a Nunchuk
 Nunchuk nchuk;
+HardwareSerial Serial1(PA_10, PA_9);
 
 // ########################## SETUP ##########################
 void setup()
@@ -103,14 +104,16 @@ void setup()
     // Clear the buffer
     display.clearDisplay();
     display.display();
+
     display.setTextSize(1);     // Draw 1X-scale text
     display.setTextColor(SSD1306_WHITE);
     display.setCursor(int
                        ((display.width() -
                          (strlen("Hoverboard Serial") * 6)) / 2), 0);
     display.println("Hoverboard Serial");
-    display.setCursor(52, 8);
-    display.println("v0.2");
+    display.setCursor(int
+                       ((display.width() -(strlen("v STM32 0.1") *6))/2), 8);
+    display.println("v STM32 0.1");
 /*
     display.print("Nunchuk ");
     nchuk.connect()? display.print("") : display.print("not ");
@@ -125,7 +128,7 @@ void setup()
 
     Serial.begin(SERIAL_BAUD);
     if (Serial) {
-        Serial.println("Hoverboard Serial v1.0");
+        Serial.println("Hoverboard Serial v STM32 0.1");
     }
 }
 
@@ -236,9 +239,15 @@ int old_cursorX, old_cursorY;
         nchuk.reconnect();
     } else {
 
+
       cmd1 = map(nchuk.joyX(),0,256,150,-150); // x - axis. Nunchuck joystick readings range 30 - 230
       cmd2 = map(nchuk.joyY(),0,256,-150,150); // y - axis
+
+      Serial.print("X : "); Serial.println(nchuk.joyX());
+      Serial.print("Y : "); Serial.println(nchuk.joyY());
+
   }
+  /*
     display.setCursor(0, 40);
     display.setTextSize(1);     // Draw 1X-scale text
     display.setTextColor(SSD1306_WHITE);
@@ -259,6 +268,7 @@ int old_cursorX, old_cursorY;
     display.print(cmd2);
 
     display.display();
+*/
     Send(cmd1, cmd2);
 
 }
@@ -348,13 +358,14 @@ void loop(void)
 {
     unsigned long timeNow = millis();
 
-    // Check for new received data
-    Receive();
-//    Nunchuk_display();
-//    show_dot();
-//    display.display();
+//    Check for new received data
+//    Receive();
 
-Nunchuk_control();
+   if (iTimeSend > timeNow)
+        return;
+    iTimeSend = timeNow + TIME_SEND;
+    // Nunchuk_display();
+    Nunchuk_control();
 /*
     // Send commands
     if (iTimeSend > timeNow)
@@ -362,18 +373,6 @@ Nunchuk_control();
     iTimeSend = timeNow + TIME_SEND;
     Send(0, cmd2);
 
-
-    display.setCursor(0, 30);
-    display.setTextSize(1);     // Draw 1X-scale text
-    display.setTextColor(SSD1306_WHITE);
-    display.print("Speed : ");
-    old_cursorX = display.getCursorX();
-    old_cursorY = display.getCursorY();
-    display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK);      // erase previous display
-    display.setCursor(old_cursorX, old_cursorY);
-    display.print(cmd2);
-    display.display();
-
     // Calculate test command signal
 
     if (motor_test_direction == 1) cmd2 += 1;