From 58210b8a9b320652f22ebea2204eb7e738e86a62 Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Tue, 29 Mar 2011 18:38:12 +0000 Subject: [PATCH] Major rewrite using asynMotorAxis and asynMotorController classes --- motorApp/MotorSimSrc/motorSimDriver.cpp | 821 ++++++++++++------------ motorApp/MotorSimSrc/motorSimDriver.h | 94 +-- 2 files changed, 459 insertions(+), 456 deletions(-) diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index 39b6a5ef..bf662639 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -1,6 +1,6 @@ /* -FILENAME... motorSimController.cpp -USAGE... Simulated Motor Support. +FILENAME... motorSimController.cpp +USAGE... Simulated Motor Support. Based on drvMotorSim.c @@ -33,258 +33,227 @@ December 13, 2009 static const char *driverName = "motorSimDriver"; +static void motorSimTaskC(void *drvPvt); + typedef struct motorSimControllerNode { - ELLNODE node; - const char *portName; - motorSimController *pController; + ELLNODE node; + const char *portName; + motorSimController *pController; } motorSimControllerNode; static ELLLIST motorSimControllerList; static int motorSimControllerListInitialized = 0; +motorSimAxis::motorSimAxis(motorSimController *pController, int axis, double lowHardLimit, double hiHardLimit, double home, double start ) + : asynMotorAxis(pController, axis), + lowHardLimit_(lowHardLimit), hiHardLimit_(hiHardLimit), home_(home) +{ + route_pars_t pars; + + pars.numRoutedAxes = 1; + pars.routedAxisList[0] = 1; + pars.Tsync = 0.0; + pars.Tcoast = 0.0; + pars.axis[0].Amax = 1.0; + pars.axis[0].Vmax = 1.0; + + endpoint_.T = 0; + endpoint_.axis[0].p = start; + endpoint_.axis[0].v = 0; + nextpoint_.T = 0; + nextpoint_.axis[0].p = start; + route_ = routeNew( &(this->endpoint_), &pars ); + deferred_move_ = 0; +} + + +motorSimController::motorSimController(const char *portName, int numAxes, int priority, int stackSize) + : asynMotorController(portName, numAxes, NUM_SIM_CONTROLLER_PARAMS, + asynInt32Mask | asynFloat64Mask, + asynInt32Mask | asynFloat64Mask, + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + priority, stackSize) +{ + int axis; + motorSimAxis *pAxis; + motorSimControllerNode *pNode; + + if (!motorSimControllerListInitialized) { + motorSimControllerListInitialized = 1; + ellInit(&motorSimControllerList); + } + + // We should make sure this portName is not already in the list */ + pNode = (motorSimControllerNode*) calloc(1, sizeof(motorSimControllerNode)); + pNode->portName = epicsStrDup(portName); + pNode->pController = this; + ellAdd(&motorSimControllerList, (ELLNODE *)pNode); + + if (numAxes < 1 ) numAxes = 1; + numAxes_ = numAxes; + this->movesDeferred_ = 0; + for (axis=0; axismotorPosition_, DEFAULT_START); + } + + this->motorThread_ = epicsThreadCreate("motorSimThread", + epicsThreadPriorityLow, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motorSimTaskC, (void *) this); +} + void motorSimController::report(FILE *fp, int level) { - int axis; - motorSimAxis *pAxis; + int axis; + motorSimAxis *pAxis; - fprintf(fp, "Simulation motor driver %s, numAxes=%d\n", - this->portName, numAxes_); + fprintf(fp, "Simulation motor driver %s, numAxes=%d\n", + this->portName, numAxes_); - for (axis=0; axisaxis_); + for (axis=0; axisaxisNo_); - if (level > 0) - { - double lowSoftLimit=0.0; - double hiSoftLimit=0.0; + if (level > 0) + { + double lowSoftLimit=0.0; + double hiSoftLimit=0.0; - fprintf(fp, " Current position = %f, velocity = %f at current time: %f\n", - pAxis->nextpoint_.axis[0].p, - pAxis->nextpoint_.axis[0].v, - pAxis->nextpoint_.T); - fprintf(fp, " Destination posn = %f, velocity = %f at desination time: %f\n", - pAxis->endpoint_.axis[0].p, - pAxis->endpoint_.axis[0].v, - pAxis->endpoint_.T); + fprintf(fp, " Current position = %f, velocity = %f at current time: %f\n", + pAxis->nextpoint_.axis[0].p, + pAxis->nextpoint_.axis[0].v, + pAxis->nextpoint_.T); + fprintf(fp, " Destination posn = %f, velocity = %f at desination time: %f\n", + pAxis->endpoint_.axis[0].p, + pAxis->endpoint_.axis[0].v, + pAxis->endpoint_.T); - fprintf(fp, " Hard limits: %f, %f\n", pAxis->lowHardLimit_, pAxis->hiHardLimit_); - fprintf(fp, " Home: %f\n", pAxis->home_); - fprintf(fp, " Enc. offset: %f\n", pAxis->enc_offset_); - getDoubleParam(pAxis->axis_, motorHighLimit_, &hiSoftLimit); - getDoubleParam(pAxis->axis_, motorLowLimit_, &lowSoftLimit); - fprintf(fp, " Soft limits: %f, %f\n", lowSoftLimit, hiSoftLimit ); + fprintf(fp, " Hard limits: %f, %f\n", pAxis->lowHardLimit_, pAxis->hiHardLimit_); + fprintf(fp, " Home: %f\n", pAxis->home_); + fprintf(fp, " Enc. offset: %f\n", pAxis->enc_offset_); + getDoubleParam(pAxis->axisNo_, motorHighLimit_, &hiSoftLimit); + getDoubleParam(pAxis->axisNo_, motorLowLimit_, &lowSoftLimit); + fprintf(fp, " Soft limits: %f, %f\n", lowSoftLimit, hiSoftLimit ); - if (pAxis->homing_) fprintf(fp, " Currently homing axis\n" ); - } + if (pAxis->homing_) fprintf(fp, " Currently homing axis\n" ); } + } - // Call the base class method - asynMotorController::report(fp, level); + // Call the base class method + asynMotorController::report(fp, level); } -motorSimAxis * motorSimController::getAxis(asynUser *pasynUser) -{ - int axis; - motorSimAxis *pAxis; - - getAddress(pasynUser, &axis); - pAxis = pAxes_[axis]; - return(pAxis); -} - - asynStatus motorSimController::processDeferredMoves() { - asynStatus status = asynError; - double position = 0.0; - int axis; - motorSimAxis *pAxis; + asynStatus status = asynError; + double position = 0.0; + int axis; + motorSimAxis *pAxis; - for (axis=0; axisdeferred_move_) { - position = pAxis->deferred_position_; - /* Check to see if in hard limits */ - if ((pAxis->nextpoint_.axis[0].p >= pAxis->hiHardLimit_ && position > pAxis->nextpoint_.axis[0].p) || - (pAxis->nextpoint_.axis[0].p <= pAxis->lowHardLimit_ && position < pAxis->nextpoint_.axis[0].p) ) return asynError; - pAxis->endpoint_.axis[0].p = position - pAxis->enc_offset_; - pAxis->endpoint_.axis[0].v = 0.0; - setIntegerParam(axis, motorStatusDone_, 0); - pAxis->deferred_move_ = 0; - } + for (axis=0; axisdeferred_move_) { + position = pAxis->deferred_position_; + /* Check to see if in hard limits */ + if ((pAxis->nextpoint_.axis[0].p >= pAxis->hiHardLimit_ && position > pAxis->nextpoint_.axis[0].p) || + (pAxis->nextpoint_.axis[0].p <= pAxis->lowHardLimit_ && position < pAxis->nextpoint_.axis[0].p)) return asynError; + pAxis->endpoint_.axis[0].p = position - pAxis->enc_offset_; + pAxis->endpoint_.axis[0].v = 0.0; + setIntegerParam(axis, motorStatusDone_, 0); + pAxis->deferred_move_ = 0; } - return status; + } + return status; } - asynStatus motorSimController::writeInt32(asynUser *pasynUser, epicsInt32 value) { - int function = pasynUser->reason; - asynStatus status = asynSuccess; - motorSimAxis *pAxis = this->getAxis(pasynUser); - static const char *functionName = "writeInt32"; - - - /* 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 = setIntegerParam(pAxis->axis_, function, value); - - if (function == motorDeferMoves_) + int function = pasynUser->reason; + asynStatus status = asynSuccess; + motorSimAxis *pAxis = this->getAxis(pasynUser); + static const char *functionName = "writeInt32"; + + + /* 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 = setIntegerParam(pAxis->axisNo_, function, value); + + if (function == motorDeferMoves_) + { + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: %sing Deferred Move flag on driver %s\n", + value != 0.0?"Sett":"Clear", + driverName, functionName, this->portName); + if (value == 0.0 && movesDeferred_ != 0) { - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "%s:%s: %sing Deferred Move flag on driver %s\n", - value != 0.0?"Sett":"Clear", - driverName, functionName, this->portName); - if (value == 0.0 && movesDeferred_ != 0) - { - processDeferredMoves(); - } - movesDeferred_ = value; - } else { - /* Call base class call its method (if we have our parameters check this here) */ - status = asynMotorController::writeInt32(pasynUser, value); + processDeferredMoves(); } - - /* Do callbacks so higher layers see any changes */ - callParamCallbacks(pAxis->axis_); - if (status) - asynPrint(pasynUser, ASYN_TRACE_ERROR, + movesDeferred_ = value; + } else { + /* Call base class call its method (if we have our parameters check this here) */ + status = asynMotorController::writeInt32(pasynUser, value); + } + + /* Do callbacks so higher layers see any changes */ + callParamCallbacks(pAxis->axisNo_); + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s: error, status=%d function=%d, value=%d\n", driverName, functionName, status, function, value); - else - asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, "%s:%s: function=%d, value=%d\n", driverName, functionName, function, value); - return status; + return status; } asynStatus motorSimController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) { - int function = pasynUser->reason; - asynStatus status = asynSuccess; - motorSimAxis *pAxis = this->getAxis(pasynUser); - static const char *functionName = "writeFloat64"; - - - /* 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 = setDoubleParam(pAxis->axis_, function, value); - - if (function == motorPosition_) - { - pAxis->enc_offset_ = (double) value - pAxis->nextpoint_.axis[0].p; - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "%s:%s: Set axis %d to position %d", - driverName, functionName, pAxis->axis_, value); - } else { - /* Call base class call its method (if we have our parameters check this here) */ - status = asynMotorController::writeFloat64(pasynUser, value); - } - - /* Do callbacks so higher layers see any changes */ - callParamCallbacks(pAxis->axis_); - if (status) - asynPrint(pasynUser, ASYN_TRACE_ERROR, + int function = pasynUser->reason; + asynStatus status = asynSuccess; + motorSimAxis *pAxis = this->getAxis(pasynUser); + static const char *functionName = "writeFloat64"; + + + /* 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 = setDoubleParam(pAxis->axisNo_, function, value); + + if (function == motorPosition_) + { + pAxis->enc_offset_ = (double) value - pAxis->nextpoint_.axis[0].p; + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: Set axis %d to position %d\n", + driverName, functionName, pAxis->axisNo_, value); + } else { + /* Call base class call its method (if we have our parameters check this here) */ + status = asynMotorController::writeFloat64(pasynUser, value); + } + + /* Do callbacks so higher layers see any changes */ + callParamCallbacks(pAxis->axisNo_); + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s: error, status=%d function=%d, value=%f\n", driverName, functionName, status, function, value); - else - asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, "%s:%s: function=%d, value=%f\n", driverName, functionName, function, value); - return status; + return status; } -asynStatus motorSimController::moveAxis(asynUser*pasynUser, double position, int relative, double minVelocity, double maxVelocity, double acceleration) +motorSimAxis* motorSimController::getAxis(asynUser *pasynUser) { - motorSimAxis *pAxis = this->getAxis(pasynUser); - route_pars_t pars; - static const char *functionName = "moveAxis"; - - if (relative) position += pAxis->endpoint_.axis[0].p + pAxis->enc_offset_; - - /* Check to see if in hard limits */ - if ((pAxis->nextpoint_.axis[0].p >= pAxis->hiHardLimit_ && position > pAxis->nextpoint_.axis[0].p) || - (pAxis->nextpoint_.axis[0].p <= pAxis->lowHardLimit_ && position < pAxis->nextpoint_.axis[0].p) ) return asynError; - - if (movesDeferred_ == 0) { /*Normal move.*/ - pAxis->endpoint_.axis[0].p = position - pAxis->enc_offset_; - pAxis->endpoint_.axis[0].v = 0.0; - } else { /*Deferred moves.*/ - pAxis->deferred_position_ = position; - pAxis->deferred_move_ = 1; - pAxis->deferred_relative_ = relative; - } - routeGetParams(pAxis->route_, &pars); - if (maxVelocity != 0) pars.axis[0].Vmax = fabs(maxVelocity); - if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); - routeSetParams( pAxis->route_, &pars ); - - setIntegerParam(pAxis->axis_, motorStatusDone_, 0); - callParamCallbacks(pAxis->axis_); - - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "%s:%s: Set driver %s, axis %d move to %f, min vel=%f, maxVel=%f, accel=%f", - driverName, functionName, this->portName, pAxis->axis_, position, minVelocity, maxVelocity, acceleration ); - return asynSuccess; + return static_cast(asynMotorController::getAxis(pasynUser)); } -asynStatus motorSimAxis::velocity(double velocity, double acceleration ) +motorSimAxis* motorSimController::getAxis(int axisNo) { - route_pars_t pars; - double deltaV = velocity - this->nextpoint_.axis[0].v; - double time; - - /* Check to see if in hard limits */ - if ((this->nextpoint_.axis[0].p > hiHardLimit_ && velocity > 0) || - (this->nextpoint_.axis[0].p < lowHardLimit_ && velocity < 0) ) return asynError; - - routeGetParams( this->route_, &pars ); - if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); - routeSetParams( this->route_, &pars ); - - time = fabs( deltaV / pars.axis[0].Amax ); - - this->endpoint_.axis[0].v = velocity; - this->endpoint_.axis[0].p = ( this->nextpoint_.axis[0].p + - time * ( this->nextpoint_.axis[0].v + 0.5 * deltaV )); - this->reroute_ = ROUTE_NEW_ROUTE; - return asynSuccess; -} - - -asynStatus motorSimController::homeAxis(asynUser *pasynUser, double minVelocity, double maxVelocity, double acceleration, int forwards ) -{ - asynStatus status = asynError; - motorSimAxis *pAxis = this->getAxis(pasynUser); - static const char *functionName = "moveAxis"; - - status = pAxis->velocity((forwards? maxVelocity: -maxVelocity), acceleration ); - pAxis->homing_ = 1; - setIntegerParam(pAxis->axis_, motorStatusDone_, 0 ); - callParamCallbacks(pAxis->axis_); - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "%s:%s: Set driver %s, axis %d to home %s, min vel=%f, maxVel=%f, accel=%f", - driverName, functionName, this->portName, pAxis->axis_, (forwards?"FORWARDS":"REVERSE"), minVelocity, maxVelocity, acceleration ); - return status; -} - - -asynStatus motorSimController::moveVelocityAxis(asynUser *pasynUser, double minVelocity, double velocity, double acceleration ) -{ - asynStatus status = asynError; - motorSimAxis *pAxis = this->getAxis(pasynUser); - static const char *functionName = "moveVelocityAxis"; - - status = pAxis->velocity(velocity, acceleration ); - setIntegerParam(pAxis->axis_, motorStatusDone_, 0); - callParamCallbacks(pAxis->axis_); - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "%s:%s: Set port %s, axis %d move with velocity of %f, accel=%f", - driverName, functionName, this->portName, pAxis->axis_, velocity, acceleration ); - return status; + return static_cast(asynMotorController::getAxis(axisNo)); } asynStatus motorSimController::profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger ) @@ -297,237 +266,259 @@ asynStatus motorSimController::triggerProfile(asynUser *pasynUser) return asynError; } -asynStatus motorSimController::stopAxis(asynUser *pasynUser, double acceleration ) +static void motorSimTaskC(void *drvPvt) { - motorSimAxis *pAxis = this->getAxis(pasynUser); - static const char *functionName = "moveVelocityAxis"; + motorSimController *pController = (motorSimController*)drvPvt; + pController->motorSimTask(); +} + - pAxis->velocity(0.0, acceleration ); - pAxis->deferred_move_ = 0; +#define DELTA 0.1 +void motorSimController::motorSimTask() +{ + epicsTimeStamp now; + double delta; + int axis; + motorSimAxis *pAxis; - asynPrint(pasynUser, ASYN_TRACE_FLOW, - "%s:%s: Set axis %d to stop with accel=%f", - driverName, functionName, pAxis->axis_, acceleration ); - return asynSuccess; + while ( 1 ) + { + /* Get a new timestamp */ + epicsTimeGetCurrent( &now ); + delta = epicsTimeDiffInSeconds( &now, &(prevTime_) ); + prevTime_ = now; + + if ( delta > (DELTA/4.0) && delta <= (4.0*DELTA) ) + { + /* A reasonable time has elapsed, it's not a time step in the clock */ + for (axis=0; axislock(); + pAxis = getAxis(axis); + pAxis->process(delta ); + this->unlock(); + } + } + epicsThreadSleep( DELTA ); + } } -asynStatus motorSimController::configAxis(int axis, int hiHardLimit, int lowHardLimit, int home, int start) +asynStatus motorSimAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { - motorSimAxis *pAxis = pAxes_[axis]; - - pAxis->hiHardLimit_ = hiHardLimit; - pAxis->lowHardLimit_ = lowHardLimit; - pAxis->home_ = home; - pAxis->enc_offset_ = start; - return(asynSuccess); + route_pars_t pars; + motorSimController *pC = static_cast(pController_); + static const char *functionName = "move"; + + if (relative) position += endpoint_.axis[0].p + enc_offset_; + + /* Check to see if in hard limits */ + if ((nextpoint_.axis[0].p >= hiHardLimit_ && position > nextpoint_.axis[0].p) || + (nextpoint_.axis[0].p <= lowHardLimit_ && position < nextpoint_.axis[0].p) ) return asynError; + + if (pC->movesDeferred_ == 0) { /*Normal move.*/ + endpoint_.axis[0].p = position - enc_offset_; + endpoint_.axis[0].v = 0.0; + } else { /*Deferred moves.*/ + deferred_position_ = position; + deferred_move_ = 1; + deferred_relative_ = relative; + } + routeGetParams(route_, &pars); + if (maxVelocity != 0) pars.axis[0].Vmax = fabs(maxVelocity); + if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); + routeSetParams( route_, &pars ); + + setIntegerParam(pC->motorStatusDone_, 0); + callParamCallbacks(); + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s:%s: Set driver %s, axis %d move to %f, min vel=%f, maxVel=%f, accel=%f\n", + driverName, functionName, pC->portName, axisNo_, position, minVelocity, maxVelocity, acceleration ); + return asynSuccess; } +asynStatus motorSimAxis::setVelocity(double velocity, double acceleration ) +{ + route_pars_t pars; + double deltaV = velocity - this->nextpoint_.axis[0].v; + double time; + + /* Check to see if in hard limits */ + if ((this->nextpoint_.axis[0].p > hiHardLimit_ && velocity > 0) || + (this->nextpoint_.axis[0].p < lowHardLimit_ && velocity < 0) ) return asynError; + + routeGetParams( this->route_, &pars ); + if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); + routeSetParams( this->route_, &pars ); + + time = fabs( deltaV / pars.axis[0].Amax ); + + this->endpoint_.axis[0].v = velocity; + this->endpoint_.axis[0].p = (this->nextpoint_.axis[0].p + + time * ( this->nextpoint_.axis[0].v + 0.5 * deltaV)); + this->reroute_ = ROUTE_NEW_ROUTE; + return asynSuccess; +} + + +asynStatus motorSimAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards ) +{ + asynStatus status = asynError; + // static const char *functionName = "home"; + + status = setVelocity((forwards? maxVelocity: -maxVelocity), acceleration ); + homing_ = 1; + return status; +} + + +asynStatus motorSimAxis::moveVelocity(double minVelocity, double velocity, double acceleration ) +{ + asynStatus status = asynError; + // static const char *functionName = "moveVelocity"; + + status = setVelocity(velocity, acceleration ); + return status; +} + +asynStatus motorSimAxis::stop(double acceleration ) +{ + // static const char *functionName = "moveVelocityAxis"; + + setVelocity(0.0, acceleration ); + deferred_move_ = 0; + return asynSuccess; +} + +asynStatus motorSimAxis::config(int hiHardLimit, int lowHardLimit, int home, int start) +{ + hiHardLimit_ = hiHardLimit; + lowHardLimit_ = lowHardLimit; + home_ = home; + enc_offset_ = start; + return asynSuccess; +} + +motorSimController* motorSimAxis::getController() +{ + return static_cast(asynMotorAxis::getController()); +} + +asynStatus motorSimAxis::poll(int *moving) +{ + return asynSuccess; +} + + /**\defgroup motorSimTask Routines to implement the motor axis simulation task @{ */ /** Process one iteration of an axis - This routine takes a single axis and propogates its motion forward a given amount - of time. + This routine takes a single axis and propogates its motion forward a given amount + of time. - \param pAxis [in] Pointer to axis information. - \param delta [in] Time in seconds to propogate motion forwards. + \param pAxis [in] Pointer to axis information. + \param delta [in] Time in seconds to propogate motion forwards. - \return Integer indicating 0 (asynSuccess) for success or non-zero for failure. + \return Integer indicating 0 (asynSuccess) for success or non-zero for failure. */ -void motorSimController::process(motorSimAxis *pAxis, double delta ) +void motorSimAxis::process(double delta ) { - double lastpos; - int done = 0; + motorSimController *pC = getController(); + double lastpos; + int done = 0; - lastpos = pAxis->nextpoint_.axis[0].p; - pAxis->nextpoint_.T += delta; - routeFind( pAxis->route_, pAxis->reroute_, &(pAxis->endpoint_), &(pAxis->nextpoint_) ); - /* if (pAxis->reroute == ROUTE_NEW_ROUTE) routePrint( pAxis->route, pAxis->reroute, &(pAxis->endpoint), &(pAxis->nextpoint_), stdout ); */ - pAxis->reroute_ = ROUTE_CALC_ROUTE; + lastpos = nextpoint_.axis[0].p; + nextpoint_.T += delta; + routeFind( route_, reroute_, &endpoint_, &nextpoint_ ); + /* if (reroute_ == ROUTE_NEW_ROUTE) routePrint( route_, reroute_, &endpoint_, &nextpoint_, stdout ); */ + reroute_ = ROUTE_CALC_ROUTE; - /* No, do a limits check */ - if (pAxis->homing_ && - ((lastpos - pAxis->home_) * (pAxis->nextpoint_.axis[0].p - pAxis->home_)) <= 0) + /* No, do a limits check */ + if (homing_ && + ((lastpos - home_) * (nextpoint_.axis[0].p - home_)) <= 0) + { + /* Homing and have crossed the home sensor - return to home */ + homing_ = 0; + reroute_ = ROUTE_NEW_ROUTE; + endpoint_.axis[0].p = home_; + endpoint_.axis[0].v = 0.0; + } + if ( nextpoint_.axis[0].p > hiHardLimit_ && nextpoint_.axis[0].v > 0 ) + { + if (homing_) setVelocity(-endpoint_.axis[0].v, 0.0 ); + else { - /* Homing and have crossed the home sensor - return to home */ - pAxis->homing_ = 0; - pAxis->reroute_ = ROUTE_NEW_ROUTE; - pAxis->endpoint_.axis[0].p = pAxis->home_; - pAxis->endpoint_.axis[0].v = 0.0; + reroute_ = ROUTE_NEW_ROUTE; + endpoint_.axis[0].p = hiHardLimit_; + endpoint_.axis[0].v = 0.0; } - if ( pAxis->nextpoint_.axis[0].p > pAxis->hiHardLimit_ && pAxis->nextpoint_.axis[0].v > 0 ) + } + else if (nextpoint_.axis[0].p < lowHardLimit_ && nextpoint_.axis[0].v < 0) + { + if (homing_) setVelocity(-endpoint_.axis[0].v, 0.0 ); + else { - if (pAxis->homing_) pAxis->velocity(-pAxis->endpoint_.axis[0].v, 0.0 ); - else - { - pAxis->reroute_ = ROUTE_NEW_ROUTE; - pAxis->endpoint_.axis[0].p = pAxis->hiHardLimit_; - pAxis->endpoint_.axis[0].v = 0.0; - } + reroute_ = ROUTE_NEW_ROUTE; + endpoint_.axis[0].p = lowHardLimit_; + endpoint_.axis[0].v = 0.0; } - else if (pAxis->nextpoint_.axis[0].p < pAxis->lowHardLimit_ && pAxis->nextpoint_.axis[0].v < 0) - { - if (pAxis->homing_) pAxis->velocity(-pAxis->endpoint_.axis[0].v, 0.0 ); - else - { - pAxis->reroute_ = ROUTE_NEW_ROUTE; - pAxis->endpoint_.axis[0].p = pAxis->lowHardLimit_; - pAxis->endpoint_.axis[0].v = 0.0; - } + } + + if (nextpoint_.axis[0].v == 0) { + if (!deferred_move_) { + done = 1; } + } else { + done = 0; + } - if (pAxis->nextpoint_.axis[0].v == 0) { - if (!pAxis->deferred_move_) { - done = 1; - } - } else { - done = 0; - } - - setDoubleParam( pAxis->axis_, this->motorPosition_, (pAxis->nextpoint_.axis[0].p+pAxis->enc_offset_) ); - setDoubleParam( pAxis->axis_, this->motorEncoderPosition_, (pAxis->nextpoint_.axis[0].p+pAxis->enc_offset_) ); - setIntegerParam( pAxis->axis_, this->motorStatusDirection_, (pAxis->nextpoint_.axis[0].v > 0) ); - setIntegerParam( pAxis->axis_, this->motorStatusDone_, done ); - setIntegerParam( pAxis->axis_, this->motorStatusHighLimit_, (pAxis->nextpoint_.axis[0].p >= pAxis->hiHardLimit_) ); - setIntegerParam( pAxis->axis_, this->motorStatusHome_, (pAxis->nextpoint_.axis[0].p == pAxis->home_) ); - setIntegerParam( pAxis->axis_, this->motorStatusMoving_, !done ); - setIntegerParam( pAxis->axis_, this->motorStatusLowLimit_, (pAxis->nextpoint_.axis[0].p <= pAxis->lowHardLimit_) ); -} - -static void motorSimTaskC(void *drvPvt) -{ - motorSimController *pController = (motorSimController*)drvPvt; - pController->motorSimTask(); -} - - -#define DELTA 0.1 -void motorSimController::motorSimTask() -{ - epicsTimeStamp now; - double delta; - int axis; - motorSimAxis *pAxis; - - while ( 1 ) - { - /* Get a new timestamp */ - epicsTimeGetCurrent( &now ); - delta = epicsTimeDiffInSeconds( &now, &(now_) ); - now_ = now; - - if ( delta > (DELTA/4.0) && delta <= (4.0*DELTA) ) - { - /* A reasonable time has elapsed, it's not a time step in the clock */ - for (axis=0; axislock(); - pAxis = pAxes_[axis]; - this->process(pAxis, delta ); - callParamCallbacks(axis); - this->unlock(); - } - } - epicsThreadSleep( DELTA ); - } -} - -motorSimAxis::motorSimAxis(motorSimController *pController, int axis, double lowHardLimit, double hiHardLimit, double home, double start ) - : pController_(pController), axis_(axis), lowHardLimit_(lowHardLimit), hiHardLimit_(hiHardLimit), home_(home) -{ - route_pars_t pars; - - pars.numRoutedAxes = 1; - pars.routedAxisList[0] = 1; - pars.Tsync = 0.0; - pars.Tcoast = 0.0; - pars.axis[0].Amax = 1.0; - pars.axis[0].Vmax = 1.0; - - this->endpoint_.T = 0; - this->endpoint_.axis[0].p = start; - this->endpoint_.axis[0].v = 0; - this->nextpoint_.T = 0; - this->nextpoint_.axis[0].p = start; - this->route_ = routeNew( &(this->endpoint_), &pars ); - this->deferred_move_ = 0; -} - - -motorSimController::motorSimController(const char *portName, int numAxes, int priority, int stackSize) - : asynMotorController(portName, numAxes, NUM_SIM_CONTROLLER_PARAMS, - asynInt32Mask | asynFloat64Mask, - asynInt32Mask | asynFloat64Mask, - ASYN_CANBLOCK | ASYN_MULTIDEVICE, - 1, // autoconnect - priority, stackSize) -{ - int axis; - motorSimAxis *pAxis; - motorSimControllerNode *pNode; - - if (!motorSimControllerListInitialized) { - motorSimControllerListInitialized = 1; - ellInit(&motorSimControllerList); - } - - // We should make sure this portName is not already in the list */ - pNode = (motorSimControllerNode*) calloc(1, sizeof(motorSimControllerNode)); - pNode->portName = epicsStrDup(portName); - pNode->pController = this; - ellAdd(&motorSimControllerList, (ELLNODE *)pNode); - - if (numAxes < 1 ) numAxes = 1; - numAxes_ = numAxes; - this->movesDeferred_ = 0; - pAxes_ = (motorSimAxis**) calloc(numAxes, sizeof(motorSimAxis*)); - for (axis=0; axismotorPosition_, DEFAULT_START); - } - - this->motorThread_ = epicsThreadCreate( "motorSimThread", - epicsThreadPriorityLow, - epicsThreadGetStackSize(epicsThreadStackMedium), - (EPICSTHREADFUNC) motorSimTaskC, (void *) this); + setDoubleParam (pC->motorPosition_, (nextpoint_.axis[0].p+enc_offset_)); + setDoubleParam (pC->motorEncoderPosition_, (nextpoint_.axis[0].p+enc_offset_)); + setIntegerParam(pC->motorStatusDirection_, (nextpoint_.axis[0].v > 0)); + setIntegerParam(pC->motorStatusDone_, done); + setIntegerParam(pC->motorStatusHighLimit_, (nextpoint_.axis[0].p >= hiHardLimit_)); + setIntegerParam(pC->motorStatusHome_, (nextpoint_.axis[0].p == home_)); + setIntegerParam(pC->motorStatusMoving_, !done); + setIntegerParam(pC->motorStatusLowLimit_, (nextpoint_.axis[0].p <= lowHardLimit_)); + callParamCallbacks(); } /** Configuration command, called directly or from iocsh */ extern "C" int motorSimCreateController(const char *portName, int numAxes, int priority, int stackSize) { - motorSimController *pSimController - = new motorSimController(portName,numAxes, priority, stackSize); - pSimController = NULL; - return(asynSuccess); + motorSimController *pSimController + = new motorSimController(portName,numAxes, priority, stackSize); + pSimController = NULL; + return(asynSuccess); } extern "C" int motorSimConfigAxis(const char *portName, int axis, int hiHardLimit, int lowHardLimit, int home, int start) { - motorSimControllerNode *pNode; - static const char *functionName = "motorSimConfigAxis"; - - // Find this controller - if (!motorSimControllerListInitialized) { - printf("%s:%s: ERROR, controller list not initialized\n", - driverName, functionName); - return(-1); - } - pNode = (motorSimControllerNode*)ellFirst(&motorSimControllerList); - while(pNode) { - if (strcmp(pNode->portName, portName) == 0) { - printf("%s:%s: configuring controller %s axis %d\n", - driverName, functionName, pNode->portName, axis); - pNode->pController->configAxis(axis, hiHardLimit, lowHardLimit, home, start); - return(0); - } - pNode = (motorSimControllerNode*)ellNext((ELLNODE*)pNode); - } - printf("Controller not found\n"); + motorSimControllerNode *pNode; + static const char *functionName = "motorSimConfigAxis"; + + // Find this controller + if (!motorSimControllerListInitialized) { + printf("%s:%s: ERROR, controller list not initialized\n", + driverName, functionName); return(-1); + } + pNode = (motorSimControllerNode*)ellFirst(&motorSimControllerList); + while(pNode) { + if (strcmp(pNode->portName, portName) == 0) { + printf("%s:%s: configuring controller %s axis %d\n", + driverName, functionName, pNode->portName, axis); + pNode->pController->getAxis(axis)->config(hiHardLimit, lowHardLimit, home, start); + return(0); + } + pNode = (motorSimControllerNode*)ellNext((ELLNODE*)pNode); + } + printf("Controller not found\n"); + return(-1); } /** Code for iocsh registration */ @@ -536,13 +527,13 @@ static const iocshArg motorSimCreateControllerArg1 = {"Number of axes", iocshArg static const iocshArg motorSimCreateControllerArg2 = {"priority", iocshArgInt}; static const iocshArg motorSimCreateControllerArg3 = {"stackSize", iocshArgInt}; static const iocshArg * const motorSimCreateControllerArgs[] = {&motorSimCreateControllerArg0, - &motorSimCreateControllerArg1, - &motorSimCreateControllerArg2, - &motorSimCreateControllerArg3}; + &motorSimCreateControllerArg1, + &motorSimCreateControllerArg2, + &motorSimCreateControllerArg3}; static const iocshFuncDef motorSimCreateControllerDef = {"motorSimCreateController", 4, motorSimCreateControllerArgs}; static void motorSimCreateContollerCallFunc(const iocshArgBuf *args) { - motorSimCreateController(args[0].sval, args[1].ival, args[2].ival, args[3].ival); + motorSimCreateController(args[0].sval, args[1].ival, args[2].ival, args[3].ival); } static const iocshArg motorSimConfigAxisArg0 = { "Post name", iocshArgString}; @@ -553,25 +544,25 @@ static const iocshArg motorSimConfigAxisArg4 = { "Home position", iocshArgInt}; static const iocshArg motorSimConfigAxisArg5 = { "Start posn", iocshArgInt}; static const iocshArg *const motorSimConfigAxisArgs[] = { - &motorSimConfigAxisArg0, - &motorSimConfigAxisArg1, - &motorSimConfigAxisArg2, - &motorSimConfigAxisArg3, - &motorSimConfigAxisArg4, - &motorSimConfigAxisArg5 + &motorSimConfigAxisArg0, + &motorSimConfigAxisArg1, + &motorSimConfigAxisArg2, + &motorSimConfigAxisArg3, + &motorSimConfigAxisArg4, + &motorSimConfigAxisArg5 }; static const iocshFuncDef motorSimConfigAxisDef ={"motorSimConfigAxis",6,motorSimConfigAxisArgs}; static void motorSimConfigAxisCallFunc(const iocshArgBuf *args) { - motorSimConfigAxis(args[0].sval, args[1].ival, args[2].ival, args[3].ival, args[4].ival, args[5].ival); + motorSimConfigAxis(args[0].sval, args[1].ival, args[2].ival, args[3].ival, args[4].ival, args[5].ival); } static void motorSimDriverRegister(void) { - iocshRegister(&motorSimCreateControllerDef, motorSimCreateContollerCallFunc); - iocshRegister(&motorSimConfigAxisDef, motorSimConfigAxisCallFunc); + iocshRegister(&motorSimCreateControllerDef, motorSimCreateContollerCallFunc); + iocshRegister(&motorSimConfigAxisDef, motorSimConfigAxisCallFunc); } extern "C" { diff --git a/motorApp/MotorSimSrc/motorSimDriver.h b/motorApp/MotorSimSrc/motorSimDriver.h index 3f63c006..f65508d4 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.h +++ b/motorApp/MotorSimSrc/motorSimDriver.h @@ -1,6 +1,6 @@ /* -FILENAME... motorSimController.h -USAGE... Simulated Motor Support. +FILENAME... motorSimController.h +USAGE... Simulated Motor Support. Based on drvMotorSim.c @@ -18,51 +18,63 @@ March 28, 2010 #define NUM_SIM_CONTROLLER_PARAMS 0 -class motorSimAxis +class motorSimAxis : public asynMotorAxis { public: - motorSimAxis(class motorSimController *pController, int axis, double lowLimit, double hiLimit, double home, double start); - asynStatus velocity(double velocity, double acceleration); - class motorSimController *pController_; - int axis_; - ROUTE_ID route_; - route_reroute_t reroute_; - route_demand_t endpoint_; - route_demand_t nextpoint_; - double lowHardLimit_; - double hiHardLimit_; - double enc_offset_; - double home_; - int homing_; - epicsTimeStamp tLast_; - double deferred_position_; - int deferred_move_; - int deferred_relative_; - friend class motorSimController; + + /* These are the functions we override from the base class */ + motorSimAxis(class motorSimController *pController, int axis, double lowLimit, double hiLimit, double home, double start); + asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); + asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); + asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards); + asynStatus stop(double acceleration); + asynStatus poll(int *moving); + motorSimController* getController(); + + /* These are the methods that are new to this class */ + asynStatus config(int hiHardLimit, int lowHardLimit, int home, int start); + asynStatus setVelocity(double velocity, double acceleration); + void process(double delta ); + +private: + ROUTE_ID route_; + route_reroute_t reroute_; + route_demand_t endpoint_; + route_demand_t nextpoint_; + double lowHardLimit_; + double hiHardLimit_; + double enc_offset_; + double home_; + int homing_; + epicsTimeStamp tLast_; + double deferred_position_; + int deferred_move_; + int deferred_relative_; + +friend class motorSimController; }; class motorSimController : asynMotorController { public: - motorSimController(const char *portName, int numAxes, int priority, int stackSize); - asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); - asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); - void report(FILE *fp, int level); - asynStatus moveAxis(asynUser *pasynUser, double position, int relative, double min_velocity, double max_velocity, double acceleration); - asynStatus moveVelocityAxis(asynUser *pasynUser, double min_velocity, double max_velocity, double acceleration); - asynStatus homeAxis(asynUser *pasynUser, double min_velocity, double max_velocity, double acceleration, int forwards); - asynStatus stopAxis(asynUser *pasynUser, double acceleration); - asynStatus profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger); - asynStatus triggerProfile(asynUser *pasynUser); - asynStatus configAxis(int axis, int hiHardLimit, int lowHardLimit, int home, int start); - void motorSimTask(); // Should be pivate, but called from non-member function + + /* These are the fucntions we override from the base class */ + motorSimController(const char *portName, int numAxes, int priority, int stackSize); + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); + void report(FILE *fp, int level); + motorSimAxis* getAxis(asynUser *pasynUser); + motorSimAxis* getAxis(int axisNo); + asynStatus profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger); + asynStatus triggerProfile(asynUser *pasynUser); + + /* These are the functions that are new to this class */ + void motorSimTask(); // Should be pivate, but called from non-member function private: - motorSimAxis* getAxis(asynUser *pasynUser); - void process(motorSimAxis *pAxis, double delta ); - asynStatus processDeferredMoves(); - epicsThreadId motorThread_; - epicsTimeStamp now_; - int movesDeferred_; - int numAxes_; - motorSimAxis** pAxes_; + asynStatus processDeferredMoves(); + epicsThreadId motorThread_; + epicsTimeStamp prevTime_; + int movesDeferred_; + +friend class motorSimAxis; };