diff --git a/src/hoverserial.ino b/src/hoverserial.ino
index a39270b..64e15b3 100644
--- a/src/hoverserial.ino
+++ b/src/hoverserial.ino
@@ -34,16 +34,16 @@
 #define BTN_PLUS            4
 #define BTN_MOINS           5
 #define BTN_MARCHE          6
-#define MAX_SPEED           400
+#define MAX_SPEED           420
 
 
 // ########################## DEFINES ##########################
-#define HOVER_SERIAL_BAUD   115200       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
+#define HOVER_SERIAL_BAUD   115200      // [-] 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         0xABCD      // [-] Start frme definition for reliable serial communication
-#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)
+#define TIME_SEND           25          // [ms] Sending time interval
+#define SPEED_MAX_TEST      490         // [-] Maximum speed for testing
+//#define DEBUG_RX                      // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
 
 // ########################## macros ############################
 
@@ -51,12 +51,6 @@
 
 #include <FastLED.h>
 
-
-
-
-// #include <SoftwareSerial.h>
-// SoftwareSerial HoverSerial(2,3);                 // RX, TX
-
 // Global variables
 uint8_t idx = 0;                // Index for new data pointer
 uint16_t bufStartFrame;         // Buffer Start Frame
@@ -86,11 +80,6 @@ typedef struct {
 SerialFeedback Feedback;
 SerialFeedback NewFeedback;
 
-int cmd1;  // normalized input values. -1000 to 1000
-int cmd2;
-
-int motor_test_direction = 1;
-
 int accel_speed =10;
 
 int eeprom_address = 0;
@@ -154,80 +143,70 @@ void Send(int16_t uSteer, int16_t uSpeed)
 
     // Write to Serial
     Serial1.write((uint8_t *) & Command, sizeof(Command));
+    // Serial.println(uSpeed);
 }
 
 // ########################## RECEIVE ##########################
 void Receive()
-
 {
     // Check for new data availability in the Serial buffer
     if (Serial1.available()) {
-        incomingByte = Serial1.read();  // Read the incoming byte
-        bufStartFrame = ((uint16_t) (incomingBytePrev) << 8) + incomingByte;    // Construct the start frame
-    } else {
+        incomingByte 	  = Serial1.read();                                   // Read the incoming byte
+        bufStartFrame	= ((uint16_t)(incomingByte) << 8) | incomingBytePrev; // Construct the start frame
+    }
+    else {
         return;
     }
 
-    // If DEBUG_RX is defined print all incoming bytes
-#ifdef DEBUG_RX
-    Serial.print(incomingByte);
-    return;
-#endif
+  // If DEBUG_RX is defined print all incoming bytes
+  #ifdef DEBUG_RX
+        Serial.print(incomingByte);
+        return;
+    #endif
 
     // Copy received data
-    if (bufStartFrame == START_FRAME) { // Initialize if new data is detected
-        p = (byte *) & NewFeedback;
-        *p++ = incomingBytePrev;
-        *p++ = incomingByte;
-        idx = 2;
-    } else if (idx >= 2 && idx < sizeof(SerialFeedback)) {      // Save the new received data
-        *p++ = incomingByte;
+    if (bufStartFrame == START_FRAME) {	                    // Initialize if new data is detected
+        p       = (byte *)&NewFeedback;
+        *p++    = incomingBytePrev;
+        *p++    = incomingByte;
+        idx     = 2;
+    } else if (idx >= 2 && idx < sizeof(SerialFeedback)) {  // Save the new received data
+        *p++    = incomingByte;
         idx++;
     }
+
     // Check if we reached the end of the package
     if (idx == sizeof(SerialFeedback)) {
         uint16_t checksum;
-        checksum =
-            (uint16_t) (NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas
+        checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas
                             ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed);
 
         // Check validity of the new data
-        if (NewFeedback.start == START_FRAME
-            && checksum == NewFeedback.checksum) {
+        if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
             // Copy the new data
             memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
 
-            // Print data to CDC Serial if available
+            // Print data to built-in Serial
             if (Serial) {
-                Serial.print("1: ");
-                Serial.print(Feedback.cmd1);
-                Serial.print(" 2: ");
-                Serial.print(Feedback.cmd2);
-                Serial.print(" 3: ");
-                Serial.print(Feedback.speedR_meas);
-                Serial.print(" 4: ");
-                Serial.print(Feedback.speedL_meas);
-                Serial.print(" 5: ");
-                Serial.print(Feedback.batVoltage);
-                Serial.print(" 6: ");
-                Serial.print(Feedback.boardTemp);
-                Serial.print(" 7: ");
-                Serial.println(Feedback.cmdLed);
-
-
-            }
+	            Serial.print("1: ");   Serial.print(Feedback.cmd1);
+	            Serial.print(" 2: ");  Serial.print(Feedback.cmd2);
+	            Serial.print(" 3: ");  Serial.print(Feedback.speedR_meas);
+	            Serial.print(" 4: ");  Serial.print(Feedback.speedL_meas);
+	            Serial.print(" 5: ");  Serial.print(Feedback.batVoltage);
+	            Serial.print(" 6: ");  Serial.print(Feedback.boardTemp);
+	            Serial.print(" 7: ");  Serial.println(Feedback.cmdLed);
+			}
         } else {
-            if (Serial)
-                Serial.println("Non-valid data skipped");
+			if (Serial)
+			    Serial.println("Non-valid data skipped");
         }
-
-        idx = 0;                // Reset the index (it prevents to enter in this if condition in the next cycle)
+        idx = 0;    // Reset the index (it prevents to enter in this if condition in the next cycle)
     }
+
     // Update previous states
     incomingBytePrev = incomingByte;
 }
 
-
 // #############################
 //This function will write a 2 byte integer to the eeprom at the specified address and address + 1
 void EEPROMWriteInt(int p_address, int p_value)