diff --git a/src/hoverserial.ino b/src/hoverserial.ino
index fdfbc83..95c81b0 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           1000        // [ms] Sending time interval
+#define TIME_SEND           100        // [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)
 
@@ -77,6 +77,12 @@ typedef struct {
 SerialFeedback Feedback;
 SerialFeedback NewFeedback;
 
+int cmd1;  // normalized input values. -1000 to 1000
+int cmd2;
+
+int motor_test_direction = 1;
+
+
 #define SCREEN_WIDTH 128        // OLED display width, in pixels
 #define SCREEN_HEIGHT 64        // OLED display height, in pixels
 
@@ -97,10 +103,9 @@ void setup()
     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.setCursor(int
+                       ((display.width() -
+                         (strlen("Hoverboard Serial") * 6)) / 2), 0);
     display.println("Hoverboard Serial");
     display.setCursor(52, 8);
     display.println("v0.2");
@@ -108,8 +113,9 @@ void setup()
     display.print("Nunchuk ");
     nchuk.connect()? display.print("") : display.print("not ");
     display.println("connected");
-*/
 
+    display.drawCircle(80, 41, 22, SSD1306_WHITE);
+*/
     display.display();
 
     Serial1.begin(HOVER_SERIAL_BAUD);   // RX, TX from arduino to TX RX on hoverboard board. ! be carreful 3v3
@@ -166,11 +172,11 @@ void Receive()
     if (idx == sizeof(SerialFeedback)) {
         uint16_t checksum;
         checksum =
-            (uint16_t) (NewFeedback.start ^ NewFeedback.
-                        cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.
-                        speedR ^ NewFeedback.speedL ^ NewFeedback.
-                        speedR_meas ^ NewFeedback.speedL_meas ^
-                        NewFeedback.batVoltage ^ NewFeedback.boardTemp);
+            (uint16_t) (NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.
+                        cmd2 ^ NewFeedback.speedR ^ NewFeedback.
+                        speedL ^ NewFeedback.speedR_meas ^ NewFeedback.
+                        speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.
+                        boardTemp);
 
         // Check validity of the new data
         if (NewFeedback.start == START_FRAME
@@ -305,7 +311,8 @@ void loop(void)
     if (iTimeSend > timeNow)
         return;
     iTimeSend = timeNow + TIME_SEND;
-    Send(0, abs(iTest));
+    Send(0, abs(cmd2));
+
 
     display.setCursor(0, 30);
     display.setTextSize(1);     // Draw 1X-scale text
@@ -313,14 +320,16 @@ void loop(void)
     display.print("Speed : ");
     old_cursorX = display.getCursorX();
     old_cursorY = display.getCursorY();
-    display.fillRect(old_cursorX, old_cursorY, (6 * 4), 8, SSD1306_BLACK);      // erase previous display
+    display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK);      // erase previous display
     display.setCursor(old_cursorX, old_cursorY);
-    display.print(iTest);
+    display.print(cmd2);
     display.display();
+
     // Calculate test command signal
-    iTest += 10;
-    if (iTest > iTestMax)
-        iTest = -iTestMax;
+
+	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);