From bf680d0c6f03a0ba36b4660dc39f3a6a0beb4eed Mon Sep 17 00:00:00 2001
From: Daniel Sissom <daniel.sissom@binarycoalescence.com>
Date: Tue, 6 Aug 2024 12:30:38 -0500
Subject: [PATCH] Updated homing routine to set initial direction.

---
 dsmApp/src/MD90Driver.cpp | 27 +++++++++++++++++++++++++++
 dsmApp/src/MD90Driver.h   |  4 +++-
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/dsmApp/src/MD90Driver.cpp b/dsmApp/src/MD90Driver.cpp
index 4d56f56..7a9f860 100644
--- a/dsmApp/src/MD90Driver.cpp
+++ b/dsmApp/src/MD90Driver.cpp
@@ -12,6 +12,8 @@ March 1, 2012
 #include <string.h>
 #include <stdlib.h>
 #include <math.h>
+#include <chrono>
+#include <thread>
 
 #include <iocsh.h>
 #include <epicsThread.h>
@@ -219,11 +221,36 @@ asynStatus MD90Axis::move(double position, int relative, double minVelocity, dou
 
 asynStatus MD90Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
 {
+  int sleepTime;
   asynStatus status;
   static const char *functionName = "MD90Axis::home";
 
   status = sendAccelAndVelocity(acceleration, maxVelocity);
 
+  // The MD-90 will start the home routine in the direction of the last move
+  // Here we first make a small move to set the desired direction before homing
+
+  sprintf(pC_->outString_, "SNS %d", SMALL_NSTEPS);
+  status = pC_->writeReadController();
+  if (!status) {
+    status = parseReply(functionName, pC_->inString_);
+  }
+
+  if (forwards) {
+    sprintf(pC_->outString_, "ESF");
+  } else {
+    sprintf(pC_->outString_, "ESB");
+  }
+  status = pC_->writeReadController();
+  if (!status) {
+    status = parseReply(functionName, pC_->inString_);
+  }
+
+  // Wait for the move to complete, then home
+
+  sleepTime = SLEEP_MARGIN * SMALL_NSTEPS * COUNTS_PER_STEP / maxVelocity;
+  std::this_thread::sleep_for(std::chrono::seconds(sleepTime));
+
   sprintf(pC_->outString_, "HOM");
   status = pC_->writeReadController();
   if (!status) {
diff --git a/dsmApp/src/MD90Driver.h b/dsmApp/src/MD90Driver.h
index b3b018e..6fb992a 100644
--- a/dsmApp/src/MD90Driver.h
+++ b/dsmApp/src/MD90Driver.h
@@ -12,7 +12,9 @@ USAGE...      Motor driver support for the DSM MD-90 controller.
 // No controller-specific parameters yet
 #define NUM_MD90_PARAMS 0  
 
-#define COUNTS_PER_STEP		1000.0				//Number of encoder counts per motor step (measured by testing)
+#define SLEEP_MARGIN		1.2					// Extra factor to wait after stepping before homing
+#define SMALL_NSTEPS		5					// Number of steps to take to set direction for homing routine
+#define COUNTS_PER_STEP		1000.0				// Number of encoder counts per motor step (measured by testing)
 
 class epicsShareClass MD90Axis : public asynMotorAxis
 {