From a387a94c146028ca8eab7c6ad17147136487ee8d Mon Sep 17 00:00:00 2001
From: Mats van Reenen <mail@matsvanreenen.nl>
Date: Sat, 16 May 2020 11:20:25 +0200
Subject: [PATCH] turn stap/dir mode on

---
 motionController.c | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/motionController.c b/motionController.c
index 84c15bc..836c09e 100644
--- a/motionController.c
+++ b/motionController.c
@@ -70,6 +70,16 @@ const uchar MC_CS = BIT0;
 #define MD_Div3  0x58
 #define MD_PULSE_DIV 12
 #define MD_RAMP_DIV  8
+// IF_CONFIGURATION_429
+#define MD_IF_CONF_429 0x68
+#define MD_InvRef     0x001
+#define MD_SdoInt     0x002
+#define MD_StepHalf   0x004
+#define MD_InvStp     0x008
+#define MD_InvDir     0x010
+#define MD_EnSd       0x020
+#define MD_PosCompSel 6
+#define MD_EnRefr     0x100
 
 
 //TODO: make function to calculate and write P
@@ -128,6 +138,8 @@ ulong MC_read(unsigned char addr) {
 }
 
 void MCInit(){
+  ulong c;
+
   P1DIR |= MC_CS; // set MC_CS (pin 5) as output
   P1OUT |= MC_CS; // set MC_CS high
 
@@ -150,6 +162,10 @@ void MCInit(){
   MC_write(MC_Rmcl1, (0b0111 << MC_REF_CONF) | MC_VELOCITY_MODE);
   MC_write(MC_Rmcl2, (0b0111 << MC_REF_CONF) | MC_VELOCITY_MODE);
 
+  // set to stap/dir mode
+  c = MC_read(MD_IF_CONF_429);
+  MC_write(MD_IF_CONF_429, c | MD_EnSd);
+
   //DEBUG
   MC_read(MC_VMax1);
   MC_read(MC_Rmcl2);