lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

- Matthew Pearson's fix for record seeing motorAxisDone True on 1st status update after a move.

Browse Source 
- Matthew Pearson added deferred move support.
This commit is contained in:
Ron Sluiter
2009-06-18 19:38:20 +00:00
parent 1918b8c682
commit 076f2c342f
1 changed files with 181 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/MotorSimSrc/drvMotorSim.c
+181 -59
View File
@@ -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 <stddef.h>
@@ -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 );