From 2d76ca73efb952db6543477e4fa5adf08d9e7a91 Mon Sep 17 00:00:00 2001
From: Mats van Reenen <mail@matsvanreenen.nl>
Date: Tue, 2 Jun 2020 09:47:05 +0200
Subject: [PATCH] add motion contoller test

---
 main.c | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

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;