From 92364a1de8b63c8da58ea6d6bd68323c24256237 Mon Sep 17 00:00:00 2001
From: Michele Brambilla <michele.brambilla@psi.ch>
Date: Thu, 10 Mar 2022 12:57:14 +0100
Subject: [PATCH] Can set and read enable, something broken with movements and
 status

---
 sinqEPICSApp/src/pmacAxis.cpp       |  8 ++++++--
 sinqEPICSApp/src/pmacController.cpp | 23 +++++++++++++----------
 sinqEPICSApp/src/pmacController.h   |  4 ++--
 3 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp
index a2da5e9..6d1b89a 100644
--- a/sinqEPICSApp/src/pmacAxis.cpp
+++ b/sinqEPICSApp/src/pmacAxis.cpp
@@ -927,8 +927,10 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
     updateMsgTxtFromDriver("Cannot read Axis position and status");
   }
 
-  // printf("axStat: %d\n", axStat);
-  setIntegerParam(p3C_->axisState_, axStat);
+  
+  int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0);
+  // st = callParamCallbacks();
+  // printf("axStat: callParamCallbacks -> ok: %d\n", st == asynSuccess);
 
   setDoubleParam(pC_->motorPosition_, position * MULT);
   setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
@@ -969,6 +971,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
     errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_);
     updateMsgTxtFromDriver("Axis did not start within 10 seconds");
     starting = 0;
+    callParamCallbacks();
     return asynSuccess;
   }
 
@@ -994,6 +997,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
           updateMsgTxtFromDriver(
               "Axis stayed in 5,6 for more then 60 seconds: BROKEN");
           status6Time = 0;
+          callParamCallbacks();
           return asynSuccess;
         } else {
           status6Time = time(NULL);
diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp
index b35cda4..514db21 100644
--- a/sinqEPICSApp/src/pmacController.cpp
+++ b/sinqEPICSApp/src/pmacController.cpp
@@ -32,7 +32,7 @@
 using std::cout;
 using std::endl;
 
-#include <epicsTime.h>
+#include "epicsTime.h"
 #include <epicsThread.h>
 #include <epicsExport.h>
 #include <epicsString.h>
@@ -148,8 +148,6 @@ pmacController::pmacController(const char *portName, const char *lowLevelPortNam
 {
   static const char *functionName = "pmacController::pmacController";
 
-  printf("  Constructor: %s\n", functionName);
-
   //Initialize non static data members
   lowLevelPortUser_ = NULL;
   debugFlag_ = 0;
@@ -366,8 +364,11 @@ asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
 
   /* Set the parameter and readback in the parameter library. */
   status = pAxis->setDoubleParam(function, value);
+  printf("%s: pAxis->setDoubleParam(%d, %lf)\n", functionName, function, value);
 
-  // if (function == motorPosition_) {
+  if (function == motorPosition_) {
+    printf("%s: pAxis->setDoubleParam(%d, %lf)\n", functionName, function, value);
+  }
   //   /*Set position on motor axis.*/            
   //   epicsInt32 position = (epicsInt32) floor(value*32/pAxis->scale_ + 0.5);
     
@@ -580,7 +581,7 @@ SeleneController::SeleneController(const char *portName, const char *lowLevelPor
     pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod, idlePollPeriod, extraParams) {
       static const char *functionName = "pmacV3Controller::pmacV3Controller";
       createParam(EnableAxisString, asynParamInt32, &enableAxis_);
-      createParam(AxisStateString, asynParamInt32, &axisState_);
+      createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
       callParamCallbacks();
   }
 
@@ -815,6 +816,7 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu
 
 }
 
+
 asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
   int function = pasynUser->reason;
   asynStatus status = asynError;
@@ -845,13 +847,14 @@ asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
   // Execute the command.
   if (command[0] != 0 && status == asynSuccess) {
     status = lowLevelWriteRead(pAxis->axisNo_, command, response);
+    printf("%s: lowLevelWriteRead: success=%d\n (function == enableAxis_? %d)\n",functionName, status==asynSuccess, function == enableAxis_);
   }
 
   // Call base class method
   // This will handle callCallbacks even if the function was handled here.
-  status = asynMotorController::writeInt32(pasynUser, value);
+  // status = asynMotorController::writeInt32(pasynUser, value);
 
-  return status;
+  return pmacController::writeInt32(pasynUser, value);
 }
 
 asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
@@ -869,14 +872,14 @@ asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
   if (!pAxis) {
     return asynError;
   }
-  if (function == axisState_) {
+  if (function == axisEnabled_) {
     snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
     status = this->lowLevelWriteRead(pAxis->axisNo_, command, response);
     *value = strtol(response, NULL, 10);
     
-    setIntegerParam(axisState_, *value);
+    int st = setIntegerParam(axisEnabled_, *value);
+    // printf("%s: setIntegerParam(%d, %d) -> success=%d\n", functionName,axisEnabled_, *value, st==asynSuccess);
     callParamCallbacks();
-    return status;
   }
   return pmacController::readInt32(pasynUser, value);
 }
diff --git a/sinqEPICSApp/src/pmacController.h b/sinqEPICSApp/src/pmacController.h
index 4f0ba64..5703011 100644
--- a/sinqEPICSApp/src/pmacController.h
+++ b/sinqEPICSApp/src/pmacController.h
@@ -165,7 +165,7 @@ class SeleneController : public pmacController {
 };
 
 #define EnableAxisString "ENABLE_AXIS"
-#define AxisStateString "AXIS_STATE"
+#define AxisEnabledString "AXIS_ENABLED"
 
 class pmacV3Controller : public pmacController {
 public:
@@ -186,7 +186,7 @@ protected:
   pmacV3Axis **pAxes_;       /**< Array of pointers to axis objects */
 
   int enableAxis_;
-  int axisState_;
+  int axisEnabled_;
 };
 
 #endif /* pmacController_H */