diff --git a/src/main.cpp b/src/main.cpp
index f31365b..153468a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -12,10 +12,10 @@
 #define SIGNAL_TIMEOUT 5000       // Time in ms to consider ball lost if no detection
 
 // Pin definitions for the motors
-#define MOTOR1_R_PWM 3
-#define MOTOR1_L_PWM 5
-#define MOTOR2_R_PWM 6
-#define MOTOR2_L_PWM 9
+#define MOTOR1_R_PWM 5
+#define MOTOR1_L_PWM 3
+#define MOTOR2_R_PWM 9
+#define MOTOR2_L_PWM 6
 #define MOTOR3_R_PWM 10
 #define MOTOR3_L_PWM 11
 
@@ -72,6 +72,31 @@ void setup()
   motors.stop();
   delay(200);
 
+  // Manual motor test
+  analogWrite(MOTOR1_R_PWM, MOTOR_SPEED);
+  delay(500);
+  analogWrite(MOTOR1_R_PWM, 0);
+  delay(200);
+  analogWrite(MOTOR1_L_PWM, MOTOR_SPEED);
+  delay(500);
+  analogWrite(MOTOR1_L_PWM, 0);
+  delay(200);
+  analogWrite(MOTOR2_L_PWM, MOTOR_SPEED);
+  delay(500);
+  analogWrite(MOTOR2_L_PWM, 0);
+  delay(200);
+  analogWrite(MOTOR2_R_PWM, MOTOR_SPEED);
+  delay(500);
+  analogWrite(MOTOR2_R_PWM, 0);
+  delay(200);
+  analogWrite(MOTOR3_R_PWM, MOTOR_SPEED);
+  delay(500);
+  analogWrite(MOTOR3_R_PWM, 0);
+  delay(200);
+  analogWrite(MOTOR3_L_PWM, MOTOR_SPEED);
+  delay(500);
+  analogWrite(MOTOR3_L_PWM, 0);
+
   Serial.println("Initialization complete. Waiting for commands...");
 
   // wait for 'start' command from serial
@@ -270,6 +295,20 @@ void loop()
       robotActive = false;
       motors.stop();
       Serial.println("Robot stopped! Send 'start' to resume");
+    } else if (command.equalsIgnoreCase("test")) {
+      motors.moveForward(100);
+      delay(1000);
+      motors.moveBackward(100);
+      delay(1000);
+      motors.moveLeft(100);
+      delay(1000);
+      motors.moveRight(100);
+      delay(1000);
+      motors.rotateToAngle(90);
+      delay(1000);
+      motors.rotateToAngle(-90);
+      delay(1000);
+      motors.stop();  
     }
     else if (command.equalsIgnoreCase("start") && !robotActive)
     {