From 8e2c1af10e723199a37cb92d3e97d64fa46af063 Mon Sep 17 00:00:00 2001 From: brambilla_m Date: Wed, 6 Apr 2022 11:45:06 +0200 Subject: [PATCH] Some cleanup, force reload encoder before enable --- sinqEPICSApp/src/pmacAxis.cpp | 107 +++++++++++++--------------- sinqEPICSApp/src/pmacController.cpp | 10 ++- 2 files changed, 58 insertions(+), 59 deletions(-) diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index b2ba52e..58500ea 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -940,11 +940,6 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { st = setDoubleParam(pC_->motorEncoderPosition_, position * MULT); cmdStatus = cmdStatus > st ? cmdStatus : st; - if(axisNo_ == 2) { - printf("position(%d): %lf\n", axisNo_, position); - printf("ack(%d): %d\n", axisNo_, acknowledge); - } - /* Use previous position and current position to calculate direction.*/ if ((position - previous_position_) > 0) { direction = 1; @@ -961,65 +956,65 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { previous_direction_ = direction; /* are we done? */ - if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) { + if ((axStat == 0 || axStat < 0) && starting == 0) { done = 1; } else { starting = 0; done = 0; } - if (starting && time(NULL) > startTime + 10) { - /* - did not start in 10 seconds: messed up: cause an alarm - */ - done = 1; - *moving = false; - st = setIntegerParam(pC_->motorStatusMoving_, false); - cmdStatus = cmdStatus > st ? cmdStatus : st; - st = setIntegerParam(pC_->motorStatusDone_, true); - cmdStatus = cmdStatus > st ? cmdStatus : st; - st = setIntegerParam(pC_->motorStatusProblem_, true); - cmdStatus = cmdStatus > st ? cmdStatus : st; - errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_); - updateMsgTxtFromDriver("Axis did not start within 10 seconds"); - starting = 0; - return st; - } + // if (starting && time(NULL) > startTime + 10) { + // /* + // did not start in 10 seconds: messed up: cause an alarm + // */ + // done = 1; + // *moving = false; + // st = setIntegerParam(pC_->motorStatusMoving_, false); + // cmdStatus = cmdStatus > st ? cmdStatus : st; + // st = setIntegerParam(pC_->motorStatusDone_, true); + // cmdStatus = cmdStatus > st ? cmdStatus : st; + // st = setIntegerParam(pC_->motorStatusProblem_, true); + // cmdStatus = cmdStatus > st ? cmdStatus : st; + // errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_); + // updateMsgTxtFromDriver("Axis did not start within 10 seconds"); + // starting = 0; + // return st; + // } - /* - code to test against too long status 5 or 6 - */ - if (axStat == 5 || axStat == 6) { - if (status6Time == 0) { - status6Time = time(NULL); - statusPos = position; - } else { - if (time(NULL) > status6Time + 60) { - /* trigger error only when not moving */ - if (abs(position - statusPos) < .1) { - done = 1; - *moving = false; - st = setIntegerParam(pC_->motorStatusMoving_, false); - cmdStatus = cmdStatus > st ? cmdStatus : st; - st = setIntegerParam(pC_->motorStatusDone_, true); - cmdStatus = cmdStatus > st ? cmdStatus : st; - st = setIntegerParam(pC_->motorStatusProblem_, true); - cmdStatus = cmdStatus > st ? cmdStatus : st; + // /* + // code to test against too long status 5 or 6 + // */ + // if (axStat == 5 || axStat == 6) { + // if (status6Time == 0) { + // status6Time = time(NULL); + // statusPos = position; + // } else { + // if (time(NULL) > status6Time + 60) { + // /* trigger error only when not moving */ + // if (abs(position - statusPos) < .1) { + // done = 1; + // *moving = false; + // st = setIntegerParam(pC_->motorStatusMoving_, false); + // cmdStatus = cmdStatus > st ? cmdStatus : st; + // st = setIntegerParam(pC_->motorStatusDone_, true); + // cmdStatus = cmdStatus > st ? cmdStatus : st; + // st = setIntegerParam(pC_->motorStatusProblem_, true); + // cmdStatus = cmdStatus > st ? cmdStatus : st; - errlogPrintf( - "Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n", - axisNo_); - updateMsgTxtFromDriver( - "Axis stayed in 5,6 for more then 60 seconds: BROKEN"); - status6Time = 0; - return cmdStatus; - } else { - status6Time = time(NULL); - statusPos = position; - } - } - } - } + // errlogPrintf( + // "Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n", + // axisNo_); + // updateMsgTxtFromDriver( + // "Axis stayed in 5,6 for more then 60 seconds: BROKEN"); + // status6Time = 0; + // return cmdStatus; + // } else { + // status6Time = time(NULL); + // statusPos = position; + // } + // } + // } + // } if (!done) { *moving = true; diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 0b8f0c6..7ad60c0 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -816,7 +816,7 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { int function = pasynUser->reason; - asynStatus status = asynError; + asynStatus status = asynSuccess; pmacV3Axis *pAxis = NULL; char command[64] = {0}; char response[64] = {0}; @@ -831,9 +831,13 @@ asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { /* Set the parameter and readback in the parameter library. This may be * overwritten when we read back the status at the end, but that's OK */ - status = pAxis->setIntegerParam(function, value); + pAxis->setIntegerParam(function, value); if (function == enableAxis_) { + if(value == 1) { + sprintf(command, "M%2.2d=15\n", pAxis->axisNo_); + lowLevelWriteRead(pAxis->axisNo_, command, response); + } sprintf(command, "M%2.2d14=%d\n", pAxis->axisNo_, value); asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Enable axis on controller %s, axis %d enable=%d\n", @@ -845,7 +849,7 @@ asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { // Execute the command. if (command[0] != 0 && status == asynSuccess) { - status = lowLevelWriteRead(pAxis->axisNo_, command, response); + lowLevelWriteRead(pAxis->axisNo_, command, response); } return pmacController::writeInt32(pasynUser, value);