diff --git a/dsmApp/src/MD90Driver.cpp b/dsmApp/src/MD90Driver.cpp index 4d56f56..e257d93 100644 --- a/dsmApp/src/MD90Driver.cpp +++ b/dsmApp/src/MD90Driver.cpp @@ -12,6 +12,8 @@ March 1, 2012 #include #include #include +#include +#include #include #include @@ -219,13 +221,43 @@ 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); - sprintf(pC_->outString_, "HOM"); - status = pC_->writeReadController(); + // 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 + + if (!status) { + 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"); + } + if (!status) { + status = pC_->writeReadController(); + } + if (!status) { + status = parseReply(functionName, pC_->inString_); + } + + if (!status) { + // 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) { status = parseReply(functionName, pC_->inString_); } 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 { diff --git a/iocs/dsmIOC/iocBoot/iocDsm/st.cmd.md90.multi b/iocs/dsmIOC/iocBoot/iocDsm/st.cmd.md90.multi index c2550f6..b14985f 100644 --- a/iocs/dsmIOC/iocBoot/iocDsm/st.cmd.md90.multi +++ b/iocs/dsmIOC/iocBoot/iocDsm/st.cmd.md90.multi @@ -147,32 +147,40 @@ iocInit # This IOC does not use save/restore, so set values of some PVs dbpf("DSM:m0.RTRY", "0") dbpf("DSM:m0.TWV", "0.1") -dbpf("DSM:m0.VMAX", "0.625") +dbpf("DSM:m0.VMAX", "1.0") +dbpf("DSM:m0.HVEL", "1.0") dbpf("DSM:m1.RTRY", "0") dbpf("DSM:m1.TWV", "0.1") -dbpf("DSM:m1.VMAX", "0.625") +dbpf("DSM:m1.VMAX", "1.0") +dbpf("DSM:m1.HVEL", "1.0") dbpf("DSM:m2.RTRY", "0") dbpf("DSM:m2.TWV", "0.1") -dbpf("DSM:m2.VMAX", "0.625") +dbpf("DSM:m2.VMAX", "1.0") +dbpf("DSM:m2.HVEL", "1.0") dbpf("DSM:m3.RTRY", "0") dbpf("DSM:m3.TWV", "0.1") -dbpf("DSM:m3.VMAX", "0.625") +dbpf("DSM:m3.VMAX", "1.0") +dbpf("DSM:m3.HVEL", "1.0") dbpf("DSM:m4.RTRY", "0") dbpf("DSM:m4.TWV", "0.1") -dbpf("DSM:m4.VMAX", "0.625") +dbpf("DSM:m4.VMAX", "1.0") +dbpf("DSM:m4.HVEL", "1.0") dbpf("DSM:m5.RTRY", "0") dbpf("DSM:m5.TWV", "0.1") -dbpf("DSM:m5.VMAX", "0.625") +dbpf("DSM:m5.VMAX", "1.0") +dbpf("DSM:m5.HVEL", "1.0") dbpf("DSM:m6.RTRY", "0") dbpf("DSM:m6.TWV", "0.1") -dbpf("DSM:m6.VMAX", "0.625") +dbpf("DSM:m6.VMAX", "1.0") +dbpf("DSM:m6.HVEL", "1.0") dbpf("DSM:m7.RTRY", "0") dbpf("DSM:m7.TWV", "0.1") -dbpf("DSM:m7.VMAX", "0.625") +dbpf("DSM:m7.VMAX", "1.0") +dbpf("DSM:m7.HVEL", "1.0")