static int motorAxisVelocity( AXIS_HDL pAxis, double velocity, double acceleration )
{
route_pars_t pars;
double deltaV = velocity - pAxis->nextpoint.axis[0].v;
/* Check to see if in hard limits */
if ((pAxis->nextpoint.axis[0].p > pAxis->hiHardLimit && velocity > 0) ||
(pAxis->nextpoint.axis[0].p < pAxis->lowHardLimit && velocity < 0) ) return MOTOR_AXIS_ERROR;
else
{
double time;
routeGetParams( pAxis->route, &pars );
if (acceleration != 0) pars.axis[0].Amax = fabs(acceleration);
routeSetParams( pAxis->route, &pars );
time = fabs( deltaV / pars.axis[0].Amax );
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 ));
pAxis->reroute = ROUTE_NEW_ROUTE;
}
return MOTOR_AXIS_OK;
}
asynStatus motorSimAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{
route_pars_t pars;
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;
}
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
{
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 MOTOR_AXIS_ERROR;
else if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK)
{
route_pars_t pars;
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 );
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 to %f, min vel=%f, max_vel=%f, accel=%f",
pAxis->card, pAxis->axis, position, min_velocity, max_velocity, acceleration );
}
}
return MOTOR_AXIS_OK;
}
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;
}
