diff --git a/main.c b/main.c
index c9518c4..b97d3cd 100644
--- a/main.c
+++ b/main.c
@@ -36,6 +36,11 @@
  * 
  * */
 
+/*
+ * 0: motor test
+ * 1: NFC test
+ * 2: motioncontroller test
+ */
 #define TEST 1
 
 const uchar STEP_PINS = BIT4 | BIT5;
@@ -84,7 +89,7 @@ int main(void)
 
     SPIInit();
     MDInit();
-    //MCInit();
+    MCInit();
     NFCInit();
 
     __delay_cycles(16000000);
@@ -131,6 +136,18 @@ int main(void)
         }
     }
 
+#endif
+
+#if TEST == 2
+
+    while(1){
+        MCSetV(MC_MotorL, 1000);
+        MCSetV(MC_MotorR, 1000);
+        __delay_cycles(32000000);
+        MCSetV(MC_MotorL, -1000);
+        MCSetV(MC_MotorR, -1000);
+    }
+
 #endif
 
     return 0;