diff --git a/src/hoverserial.ino b/src/hoverserial.ino
index 777a04e..97c6aa3 100644
--- a/src/hoverserial.ino
+++ b/src/hoverserial.ino
@@ -38,6 +38,10 @@
 #define SPEED_MAX_TEST      300 // [-] Maximum speed for testing
 //#define DEBUG_RX                        // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
 
+// ########################## macros ############################
+
+#define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
+
 #include <Wire.h>
 #include <Adafruit_GFX.h>
 #include <Adafruit_SSD1306.h>
@@ -214,6 +218,40 @@ void Receive()
     incomingBytePrev = incomingByte;
 }
 
+void Nunchuk_control()
+{
+int old_cursorX, old_cursorY;
+
+    if (!nchuk.update()) {
+        nchuk.reconnect();
+    } else {
+
+      cmd1 = CLAMP((nchuk.joyX() - 128) * 6, -300, 300); // x - axis. Nunchuck joystick readings range 30 - 230
+      cmd2 = CLAMP((nchuk.joyY() - 128) * 6, 300, -300); // y - axis
+  }
+    display.setCursor(0, 40);
+    display.setTextSize(1);     // Draw 1X-scale text
+    display.setTextColor(SSD1306_WHITE);
+    display.print("X : ");
+    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(cmd1);
+    display.setCursor(0, 50);
+    display.setTextSize(1);     // Draw 1X-scale text
+    display.setTextColor(SSD1306_WHITE);
+    display.print("y : ");
+    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();
+    Send(cmd1, cmd2);
+
+}
 // ########################## nunchuk ##########################
 
 void Nunchuk_display()
@@ -306,7 +344,8 @@ void loop(void)
 //    show_dot();
 //    display.display();
 
-
+Nunchuk_control();
+/*
     // Send commands
     if (iTimeSend > timeNow)
         return;
@@ -330,7 +369,7 @@ void loop(void)
     if (motor_test_direction == 1) cmd2 += 1;
       else cmd2 -= 1;
       if (abs(cmd2) > SPEED_MAX_TEST) motor_test_direction = -motor_test_direction;
-
+*/
     // Blink the LED
     digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
 }