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; }