diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h
index 3ed6332..a951194 100644
--- a/include/motors_movement/drivecontroller.h
+++ b/include/motors_movement/drivecontroller.h
@@ -38,12 +38,12 @@ class DriveController{
         int vxp, vyp, vxn, vyn;
         bool canUnlock;
         unsigned long unlockTime;
-
-    private:
         Motor* m1;
         Motor* m2;
         Motor* m3;
         Motor* m4;
+
+    private:
         PID* pid;
         ComplementaryFilter* speedFilter;
         int pDir, pSpeed, pTilt, oldSpeed;
diff --git a/include/motors_movement/motor.h b/include/motors_movement/motor.h
index 98d0054..acca9aa 100644
--- a/include/motors_movement/motor.h
+++ b/include/motors_movement/motor.h
@@ -8,6 +8,7 @@ class Motor {
         Motor(int a, int b, int pwm, int angle);
         Motor();
         void drive(int speed);
+        void test();
     
     public:
         int pinA, pinB, pinPwm, angle;
diff --git a/include/test_menu.h b/include/test_menu.h
new file mode 100644
index 0000000..09975f7
--- /dev/null
+++ b/include/test_menu.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#define TEST_DELAY 100
+
+class TestMenu{
+    public:
+        void testMenu();
+        char testNum, currentRole;
+        bool flagtest;
+};
\ No newline at end of file
diff --git a/src/motors_movement/motor.cpp b/src/motors_movement/motor.cpp
index c7a3c21..c0e301e 100644
--- a/src/motors_movement/motor.cpp
+++ b/src/motors_movement/motor.cpp
@@ -35,4 +35,15 @@ void Motor::drive(int speed){
     digitalWrite(pinA, VAL_INA);
     digitalWrite(pinB, VAL_INB);
     analogWrite(pinPwm, speed);
+}
+
+void Motor::test(){
+    digitalWrite(pinA, 1);
+    digitalWrite(pinB, 0);
+    analogWrite(pinPwm, 255);
+    delay(150);
+    digitalWrite(pinA, 0);
+    digitalWrite(pinB, 1);
+    analogWrite(pinPwm, 255);
+    delay(150);
 }
\ No newline at end of file
diff --git a/src/test_menu.cpp b/src/test_menu.cpp
new file mode 100644
index 0000000..dddcc3f
--- /dev/null
+++ b/src/test_menu.cpp
@@ -0,0 +1,106 @@
+#include "test_menu.h"
+#include "sensors/data_source_ball.h"
+#include "sensors/data_source_bno055.h"
+#include "sensors/data_source_bt.h"
+#include "sensors/data_source_camera_conicmirror.h"
+#include "sensors/linesys_camera.h"
+#include "sensors/linesys_2019.h"
+#include "sensors/sensors.h"
+#include "motors_movement/motor.h"
+#include "motors_movement/drivecontroller.h"
+#include "position/positionsys_camera.h"
+#include "strategy_roles/game.h"
+#include "strategy_roles/games.h"
+#include "behaviour_control/data_source.h"
+
+void TestMenu :: testMenu(){
+    DEBUG.println();
+    DEBUG.println("Welcome to the test menu c:");
+    DEBUG.println("Set the baudrate to 9600 & NO LINE ENDING");
+    DEBUG.println("Hit correspondent key (1-->8) to execute and press ENTER");
+    DEBUG.println("Hit '0' to exit test menu");
+    DEBUG.println();
+
+    DEBUG.println("Test Menu: ");
+    DEBUG.println("0)Exit test menu and start playing");
+    DEBUG.println("1)Ball test");
+    DEBUG.println("2)IMU test");
+    DEBUG.println("3)Motors test");
+    DEBUG.println("4)Recenter");
+    DEBUG.println("5)BT test");
+    DEBUG.println("6)Camera test");
+    DEBUG.println("7)Line Sensors camera test");
+    DEBUG.println("8)Line Sensors test");
+    do{
+        testNum = DEBUG.read();
+        DEBUG.println();
+        DEBUG.println("Test:");
+        DEBUG.println(testNum);
+        DEBUG.println(" ");
+        if (testNum == '0') DEBUG.println("EXIT TEST MENU");
+        else if (testNum == '1') DEBUG.println("Ball test, turn the ball on and turn it around the robot");
+        else if (testNum == '2') DEBUG.println("IMU test, keep the robot still and on a flat surface");
+        else if (testNum == '3') DEBUG.println("Motor test, turn on SW_MOTORI and keep the robot at eye level");
+        else if (testNum == '4') DEBUG.println("Recenter, turn both SWS on and tilt the robot");
+        else if (testNum == '5') DEBUG.println("BT test, pair to BT");
+        else if (testNum == '6') DEBUG.println("Camera Test");
+        else if (testNum == '7') DEBUG.println("Line Sensors camera test, turn on SW_ELETT");
+        else if (testNum == '8') DEBUG.println("Line Sensors test, turn on SW_ELETT");
+    else {
+      DEBUG.println("UNKNOWN COMMAND");
+      flagtest = false;
+    }
+  }  while (DEBUG.available() > 0);
+    do{
+        switch(testNum){
+            case '0':
+                DEBUG.println("Exiting test menu, may the odds be in your favor c:");
+                flagtest = false;
+                return;
+            break;
+            case '1':
+                ball->test();
+                delay(TEST_DELAY);
+            break;
+            case '2':
+                compass->test();
+                delay(TEST_DELAY);
+            break;
+            case '3':
+                drive->m1->test();
+                drive->m2->test();
+                drive->m3->test();
+                drive->m4->test();
+            break;
+            case '4':
+                drive->drive(0,0,0);
+            break;
+            case '5':
+                bt->test();
+            break;
+            case '6':
+                camera->test();
+            break;
+            case '7':
+            case '8':
+                DEBUG.println("To do Line Sensors test, decide the role first");
+                DEBUG.println("1)Keeper");
+                DEBUG.println("2)Goalie");
+                currentRole = DEBUG.read();
+                switch(currentRole){
+                    case '1':
+                        (goalie->ls)->test();
+                    break;
+                    case '2':
+                        (keeper->ls)->test();
+                    break;
+                    default:
+                        DEBUG.println("INVALID ROLE");
+                    break;
+                }
+            break;
+            default:
+            break;
+        }
+    } while (DEBUG.available() == 0);
+}
\ No newline at end of file