diff --git a/motorApp/MotorSimSrc/drvMotorSim.c b/motorApp/MotorSimSrc/drvMotorSim.c index a726a010..6c03d200 100644 --- a/motorApp/MotorSimSrc/drvMotorSim.c +++ b/motorApp/MotorSimSrc/drvMotorSim.c @@ -2,9 +2,9 @@ FILENAME... drvMotorSim.c USAGE... Simulated Motor Support. -Version: $Revision: 1.9 $ +Version: $Revision: 1.10 $ Modified By: $Author: sluiter $ -Last Modified: $Date: 2009-02-18 21:39:40 $ +Last Modified: $Date: 2009-06-18 19:38:20 $ */ /* @@ -14,6 +14,13 @@ Last Modified: $Date: 2009-02-18 21:39:40 $ * ----------------- * 2006-05-06 npr Added prolog * 2009-02-11 rls lock/unlock motorAxisSetDouble(). + * 2009-06-18 rls - Matthew Pearson's fix for record seeing motorAxisDone True + * on 1st status update after a move; set motorAxisDone False + * in motorAxisDrvSET_t functions that start motion + * (motorAxisHome, motorAxisMove, motorAxisVelocityMove) and + * force a status update with a call to callCallback(). + * - Matthew Pearson added deferred move support. + * */ #include @@ -64,6 +71,18 @@ typedef enum { none, positionMove, velocityMove, homeReverseMove, homeForwardsMo /* typedef struct motorAxis * AXIS_ID; */ +typedef struct drvSim * DRVSIM_ID; +typedef struct drvSim +{ + AXIS_HDL pFirst; + epicsThreadId motorThread; + motorAxisLogFunc print; + void * logParam; + epicsTimeStamp now; + int movesDeferred; + int nAxes; +} motorSim_t; + typedef struct motorAxisHandle { AXIS_HDL pNext; @@ -84,16 +103,13 @@ typedef struct motorAxisHandle void * logParam; epicsTimeStamp tLast; epicsMutexId axisMutex; + double deferred_position; + int deferred_move; + int deferred_relative; + DRVSIM_ID pDrv; } motorAxis; -typedef struct -{ - AXIS_HDL pFirst; - epicsThreadId motorThread; - motorAxisLogFunc print; - void * logParam; - epicsTimeStamp now; -} motorSim_t; + static int motorSimLogMsg( void * param, const motorAxisLogMask_t logMask, const char *pFormat, ...); #define TRACE_FLOW motorAxisTraceFlow @@ -104,6 +120,9 @@ static motorSim_t drv={ NULL, NULL, motorSimLogMsg, NULL, { 0, 0 } }; #define MAX(a,b) ((a)>(b)? (a): (b)) #define MIN(a,b) ((a)<(b)? (a): (b)) +/*Deferred moves functions.*/ +static int processDeferredMoves(const motorSim_t * pDrv); + static void motorAxisReportAxis( AXIS_HDL pAxis, int level ) { printf( "Found driver for motorSim card %d, axis %d, mutex %p\n", pAxis->card, pAxis->axis, pAxis->axisMutex ); @@ -198,7 +217,13 @@ static int motorAxisGetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int * if (pAxis == NULL) return MOTOR_AXIS_ERROR; else { - return motorParam->getInteger( pAxis->params, (paramIndex) function, value ); + switch (function) { + case motorAxisDeferMoves: + *value = pAxis->pDrv->movesDeferred; + return MOTOR_AXIS_OK; + default: + return motorParam->getInteger( pAxis->params, (paramIndex) function, value ); + } } } @@ -207,7 +232,13 @@ static int motorAxisGetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double if (pAxis == NULL) return MOTOR_AXIS_ERROR; else { - return motorParam->getDouble( pAxis->params, (paramIndex) function, value ); + switch (function) { + case motorAxisDeferMoves: + *value = (double)pAxis->pDrv->movesDeferred; + return MOTOR_AXIS_OK; + default: + return motorParam->getDouble( pAxis->params, (paramIndex) function, value ); + } } } @@ -220,6 +251,39 @@ static int motorAxisSetCallback( AXIS_HDL pAxis, motorAxisCallbackFunc callback, } } +static int processDeferredMoves(const motorSim_t * pDrv) +{ + int status = MOTOR_AXIS_ERROR; + double position = 0.0; + AXIS_HDL pAxis = NULL; + + for ( pAxis = pDrv->pFirst; pAxis != NULL; pAxis = pAxis->pNext ) + { + if (pAxis->deferred_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 MOTOR_AXIS_ERROR; + else if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) + { + pAxis->endpoint.axis[0].p = position - pAxis->enc_offset; + pAxis->endpoint.axis[0].v = 0.0; + + motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); + + pAxis->deferred_move = 0; + epicsMutexUnlock( pAxis->axisMutex ); + } + + } + } + + return status; +} + + static int motorAxisSetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double value ) { int status = MOTOR_AXIS_OK; @@ -276,6 +340,17 @@ static int motorAxisSetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d closed loop to %s", pAxis->card, pAxis->axis, (value!=0?"ON":"OFF") ); break; } + case motorAxisDeferMoves: + { + pAxis->print( pAxis->logParam, TRACE_FLOW, + "%sing Deferred Move flag on PMAC card %d\n", + value != 0.0?"Sett":"Clear",pAxis->card); + if (value == 0.0 && pAxis->pDrv->movesDeferred != 0) { + processDeferredMoves(pAxis->pDrv); + } + pAxis->pDrv->movesDeferred = (int)value; + break; + } default: status = MOTOR_AXIS_ERROR; break; @@ -294,45 +369,62 @@ static int motorAxisSetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int v { int status = MOTOR_AXIS_OK; - if (pAxis == NULL) return MOTOR_AXIS_ERROR; + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); else { - switch (function) + if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) { - case motorAxisPosition: - { - pAxis->enc_offset = (double) value - pAxis->nextpoint.axis[0].p; - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to position %d", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisLowLimit: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d low limit to %d", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisHighLimit: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d high limit to %d", pAxis->card, pAxis->axis, value ); - break; - } - case motorAxisClosedLoop: - { - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d closed loop to %s", pAxis->card, pAxis->axis, (value?"ON":"OFF") ); - break; - } - default: - status = MOTOR_AXIS_ERROR; - break; - } + switch (function) + { + case motorAxisPosition: + { + pAxis->enc_offset = (double) value - pAxis->nextpoint.axis[0].p; + pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to position %d", pAxis->card, pAxis->axis, value ); + break; + } + case motorAxisLowLimit: + { + pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d low limit to %d", pAxis->card, pAxis->axis, value ); + break; + } + case motorAxisHighLimit: + { + pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d high limit to %d", pAxis->card, pAxis->axis, value ); + break; + } + case motorAxisClosedLoop: + { + pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d closed loop to %s", pAxis->card, pAxis->axis, (value?"ON":"OFF") ); + break; + } + case motorAxisDeferMoves: + { + pAxis->print( pAxis->logParam, TRACE_FLOW, + "%sing Deferred Move flag on PMAC card %d\n", + value != 0.0?"Sett":"Clear",pAxis->card); + if (value == 0.0 && pAxis->pDrv->movesDeferred != 0) + { + processDeferredMoves(pAxis->pDrv); + } + pAxis->pDrv->movesDeferred = value; + break; + } + default: + status = MOTOR_AXIS_ERROR; + break; + } - if (status != MOTOR_AXIS_ERROR ) status = motorParam->setInteger( pAxis->params, function, value ); + if (status != MOTOR_AXIS_ERROR ) + status = motorParam->setInteger( pAxis->params, function, value ); + } + return (status); } - return status; } - static int motorAxisMove( AXIS_HDL pAxis, double position, int relative, double min_velocity, double max_velocity, double acceleration ) { + if (pAxis == NULL) return MOTOR_AXIS_ERROR; else { @@ -345,19 +437,27 @@ static int motorAxisMove( AXIS_HDL pAxis, double position, int relative, double { route_pars_t pars; - pAxis->endpoint.axis[0].p = position - pAxis->enc_offset; - pAxis->endpoint.axis[0].v = 0.0; + if (pAxis->pDrv->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 (max_velocity != 0) pars.axis[0].Vmax = fabs(max_velocity); if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration); - routeSetParams( pAxis->route, &pars ); + routeSetParams( pAxis->route, &pars ); + motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); - motorParam->setInteger( pAxis->params, motorAxisMoving, 1 ); + motorParam->callCallback( pAxis->params ); epicsMutexUnlock( pAxis->axisMutex ); pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f", pAxis->card, pAxis->axis, position, min_velocity, max_velocity, acceleration ); } + } return MOTOR_AXIS_OK; } @@ -383,8 +483,6 @@ static int motorAxisVelocity( AXIS_HDL pAxis, double velocity, double accelerati pAxis->endpoint.axis[0].v = velocity; pAxis->endpoint.axis[0].p = ( pAxis->nextpoint.axis[0].p + time * ( pAxis->nextpoint.axis[0].v + 0.5 * deltaV )); - motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); - motorParam->setInteger( pAxis->params, motorAxisMoving, 1 ); pAxis->reroute = ROUTE_NEW_ROUTE; } return MOTOR_AXIS_OK; @@ -397,11 +495,15 @@ static int motorAxisHome( AXIS_HDL pAxis, double min_velocity, double max_veloci if (pAxis == NULL) status = MOTOR_AXIS_ERROR; else { - status = motorAxisVelocity( pAxis, (forwards? max_velocity: -max_velocity), acceleration ); - pAxis->homing = 1; - - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f", - pAxis->card, pAxis->axis, (forwards?"FORWARDS":"REVERSE"), min_velocity, max_velocity, acceleration ); + if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) { + status = motorAxisVelocity( pAxis, (forwards? max_velocity: -max_velocity), acceleration ); + pAxis->homing = 1; + motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); + motorParam->callCallback( pAxis->params ); + epicsMutexUnlock( pAxis->axisMutex ); + pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f", + pAxis->card, pAxis->axis, (forwards?"FORWARDS":"REVERSE"), min_velocity, max_velocity, acceleration ); + } } return status; } @@ -417,6 +519,8 @@ static int motorAxisVelocityMove( AXIS_HDL pAxis, double min_velocity, double v if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) { status = motorAxisVelocity( pAxis, velocity, acceleration ); + motorParam->setInteger( pAxis->params, motorAxisDone, 0 ); + motorParam->callCallback( pAxis->params ); epicsMutexUnlock( pAxis->axisMutex ); pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d move with velocity of %f, accel=%f", pAxis->card, pAxis->axis, velocity, acceleration ); @@ -440,10 +544,15 @@ static int motorAxisStop( AXIS_HDL pAxis, double acceleration ) if (pAxis == NULL) return MOTOR_AXIS_ERROR; else { - motorAxisVelocity( pAxis, 0.0, acceleration ); + if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) { + motorAxisVelocity( pAxis, 0.0, acceleration ); + pAxis->deferred_move = 0; + epicsMutexUnlock( pAxis->axisMutex ); + + pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to stop with accel=%f", + pAxis->card, pAxis->axis, acceleration ); + } - pAxis->print( pAxis->logParam, TRACE_FLOW, "Set card %d, axis %d to stop with accel=%f", - pAxis->card, pAxis->axis, acceleration ); } return MOTOR_AXIS_OK; } @@ -466,6 +575,7 @@ static int motorAxisStop( AXIS_HDL pAxis, double acceleration ) static void motorSimProcess( AXIS_HDL pAxis, double delta ) { double lastpos = pAxis->nextpoint.axis[0].p; + int done = 0; pAxis->nextpoint.T += delta; routeFind( pAxis->route, pAxis->reroute, &(pAxis->endpoint), &(pAxis->nextpoint) ); @@ -480,7 +590,7 @@ static void motorSimProcess( AXIS_HDL pAxis, double delta ) pAxis->homing = 0; pAxis->reroute = ROUTE_NEW_ROUTE; pAxis->endpoint.axis[0].p = pAxis->home; - pAxis->endpoint.axis[0].v = 0.0; + pAxis->endpoint.axis[0].v = 0.0; } if ( pAxis->nextpoint.axis[0].p > pAxis->hiHardLimit && pAxis->nextpoint.axis[0].v > 0 ) { @@ -503,13 +613,21 @@ static void motorSimProcess( AXIS_HDL pAxis, double delta ) } } + if (pAxis->nextpoint.axis[0].v == 0) { + if (!pAxis->deferred_move) { + done = 1; + } + } else { + done = 0; + } + motorParam->setDouble( pAxis->params, motorAxisPosition, (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) ); motorParam->setDouble( pAxis->params, motorAxisEncoderPosn, (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) ); motorParam->setInteger( pAxis->params, motorAxisDirection, (pAxis->nextpoint.axis[0].v > 0) ); - motorParam->setInteger( pAxis->params, motorAxisDone, (pAxis->nextpoint.axis[0].v == 0) ); + motorParam->setInteger( pAxis->params, motorAxisDone, done ); motorParam->setInteger( pAxis->params, motorAxisHighHardLimit, (pAxis->nextpoint.axis[0].p >= pAxis->hiHardLimit) ); motorParam->setInteger( pAxis->params, motorAxisHomeSignal, (pAxis->nextpoint.axis[0].p == pAxis->home) ); - motorParam->setInteger( pAxis->params, motorAxisMoving, (pAxis->nextpoint.axis[0].v != 0) ); + motorParam->setInteger( pAxis->params, motorAxisMoving, !done ); motorParam->setInteger( pAxis->params, motorAxisLowHardLimit, (pAxis->nextpoint.axis[0].p <= pAxis->lowHardLimit) ); } @@ -566,6 +684,8 @@ static int motorSimCreateAxis( motorSim_t * pDrv, int card, int axis, double low { route_pars_t pars; + pAxis->pDrv = pDrv; + pars.numRoutedAxes = 1; pars.routedAxisList[0] = 1; pars.Tsync = 0.0; @@ -632,6 +752,8 @@ void motorSimCreate( int card, int axis, int lowLimit, int hiLimit, int home, in if (nCards < 1) nCards = 1; if (nAxes < 1 ) nAxes = 1; + drv.nAxes = nAxes; + drv.print( drv.logParam, TRACE_FLOW, "Creating motor simulator: card: %d, axis: %d, hi: %d, low %d, home: %d, ncards: %d, naxis: %d", card, axis, hiLimit, lowLimit, home, nCards, nAxes );