From 202bc86b368d7aedd25f9a1475481024ff2c42be Mon Sep 17 00:00:00 2001
From: Mats van Reenen <mail@matsvanreenen.nl>
Date: Mon, 1 Jun 2020 10:57:53 +0200
Subject: [PATCH] add tests

---
 main.c | 85 +++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 81 insertions(+), 4 deletions(-)

diff --git a/main.c b/main.c
index 80b5c33..e049b92 100644
--- a/main.c
+++ b/main.c
@@ -1,5 +1,5 @@
 #include <msp430.h>
-#include <math.h>
+//#include <math.h>
 #include "SPI.h"
 #include "motorDriver.h"
 #include "motionController.h"
@@ -36,24 +36,101 @@
  * 
  * */
 
+#define TEST "nfc"
+
+const uchar STEP_PINS = BIT4 | BIT5;
+const uchar DIRr_PIN = BIT3;
+const uchar DIRl_PIN = BIT2;
+typedef enum Direction {Forward=0, Backward=(DIRl_PIN | DIRr_PIN), RightTurn=DIRr_PIN, LeftTurn=DIRl_PIN};
+
+const uchar v = 36;
+const ulong halfRound = 2800;
+
+void stap(uint n, ulong t){
+  ulong d = t;
+  while(n != 0){
+    n--;
+    P2OUT |= STEP_PINS;
+    P2OUT &= ~STEP_PINS;
+    d=t;
+    while(d != 0){
+        d--;
+        __delay_cycles(160);
+    }
+  }
+}
+
+void dir(enum Direction d){
+  P2OUT &= ~(DIRr_PIN | DIRl_PIN); // set dir bits to 0
+  P2OUT |= d;
+}
+
+
 /**
  * main.c
  */
+
 int main(void)
 {
+    int i, ii;
     WDTCTL = WDTPW | WDTHOLD;   // stop watchdog timer
 
+
     // set cpu op 16MHz
     BCSCTL1 = CALBC1_16MHZ;
     DCOCTL  = CALDCO_16MHZ;
 
+    P2DIR |= STEP_PINS | DIRr_PIN | DIRl_PIN;
+
     SPIInit();
     MDInit();
-    MCInit();
+    //MCInit();
     NFCInit();
-    while (1){
-        uchar NFCAvailable();
+
+    __delay_cycles(16000000);
+
+#if TEST == "nfc"
+
+    while(1){
+        NFCAvailable();
     }
 
+#else
+
+    for(ii=2; ii>0; ii--){
+        dir(Forward);
+        stap(16000, v);
+        __delay_cycles(160000);
+        dir(RightTurn);
+        stap(halfRound, v*2);
+        __delay_cycles(160000);
+        dir(Forward);
+        stap(16000, v);
+        __delay_cycles(160000);
+        dir(LeftTurn);
+        stap(halfRound, v*2);
+        __delay_cycles(160000);
+    }
+    for(ii=2; ii>0; ii--){
+        for(i=4; i>0; i--){
+            dir(Forward);
+            stap(1600, v);
+            __delay_cycles(160000);
+            dir(RightTurn);
+            stap(halfRound/2, v*2);
+            __delay_cycles(160000);
+        }
+        for(i=4; i>0; i--){
+            dir(Forward);
+            stap(1600, v);
+            __delay_cycles(160000);
+            dir(LeftTurn);
+            stap(halfRound/2, v*2);
+            __delay_cycles(160000);
+        }
+    }
+
+#endif
+
     return 0;
 }