Added scaffolding for velocity mode
Added records to support detection of the current operation mode (position or velocity), whether one is allowed to change between the two and a record to actually change between the two. Also added a doMoveVelocity method which should be implemented by derived drivers if they support velocity mode.
This commit is contained in:
+43
-12
@@ -333,18 +333,49 @@ asynStatus sinqAxis::doPoll(bool *moving) {
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus sinqAxis::moveVelocity(double minVelocity, double maxVelocity,
|
||||
double acceleration) {
|
||||
|
||||
int motMode = 0;
|
||||
|
||||
// If the motor is not in velocity mode, do nothing
|
||||
getAxisParamChecked(this, motorMode, &motMode);
|
||||
if (motMode != 0) {
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
return doMoveVelocity(minVelocity, maxVelocity, acceleration);
|
||||
}
|
||||
|
||||
asynStatus sinqAxis::doMoveVelocity(double minVelocity, double maxVelocity,
|
||||
double acceleration) {
|
||||
// Suppress unused variable warning - this is just a default fallback
|
||||
// function.
|
||||
(void)minVelocity;
|
||||
(void)maxVelocity;
|
||||
(void)acceleration;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus sinqAxis::move(double position, int relative, double minVelocity,
|
||||
double maxVelocity, double acceleration) {
|
||||
|
||||
// Status of parameter library operations
|
||||
asynStatus status = asynSuccess;
|
||||
double motorRecRes = 0.0;
|
||||
double motRecRes = 0.0;
|
||||
int motMode = 0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// If the motor is not in position mode, do nothing
|
||||
getAxisParamChecked(this, motorMode, &motMode);
|
||||
if (motMode != 0) {
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
// Store the target position internally
|
||||
getAxisParamChecked(this, motorRecResolution, &motorRecRes);
|
||||
pSinqA_->targetPosition = position * motorRecRes;
|
||||
getAxisParamChecked(this, motorRecResolution, &motRecRes);
|
||||
pSinqA_->targetPosition = position * motRecRes;
|
||||
|
||||
status = doMove(position, relative, minVelocity, maxVelocity, acceleration);
|
||||
if (status != asynSuccess) {
|
||||
@@ -455,9 +486,9 @@ asynStatus sinqAxis::enable(bool on) {
|
||||
|
||||
asynStatus sinqAxis::motorPosition(double *motorPos) {
|
||||
asynStatus status = asynSuccess;
|
||||
double motorRecRes = 0.0;
|
||||
double motRecRes = 0.0;
|
||||
|
||||
getAxisParamChecked(this, motorRecResolution, &motorRecRes);
|
||||
getAxisParamChecked(this, motorRecResolution, &motRecRes);
|
||||
|
||||
/*
|
||||
We cannot use getAxisParamChecked checked here, since the name of the index
|
||||
@@ -470,16 +501,16 @@ asynStatus sinqAxis::motorPosition(double *motorPos) {
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
*motorPos = *motorPos * motorRecRes;
|
||||
*motorPos = *motorPos * motRecRes;
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus sinqAxis::setMotorPosition(double motorPos) {
|
||||
asynStatus status = asynSuccess;
|
||||
double motorRecRes = 0.0;
|
||||
double motRecRes = 0.0;
|
||||
|
||||
getAxisParamChecked(this, motorRecResolution, &motorRecRes);
|
||||
setAxisParamChecked(this, motorPosition, motorPos / motorRecRes);
|
||||
getAxisParamChecked(this, motorRecResolution, &motRecRes);
|
||||
setAxisParamChecked(this, motorPosition, motorPos / motRecRes);
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -573,7 +604,7 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
||||
double motorVelocityRec = 0.0;
|
||||
double motorAccel = 0.0;
|
||||
double motorAccelRec = 0.0;
|
||||
double motorRecRes = 0.0;
|
||||
double motRecRes = 0.0;
|
||||
time_t timeContSpeed = 0;
|
||||
time_t timeAccel = 0;
|
||||
|
||||
@@ -602,7 +633,7 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
||||
= VELO / MRES motorAccel = (motorVelocity - motorVelBase) / ACCL
|
||||
Therefore, we need to correct the values from the parameter library.
|
||||
*/
|
||||
getAxisParamChecked(this, motorRecResolution, &motorRecRes);
|
||||
getAxisParamChecked(this, motorRecResolution, &motRecRes);
|
||||
|
||||
// Read the velocity
|
||||
getAxisParamChecked(this, motorVelocity, &motorVelocityRec);
|
||||
@@ -611,7 +642,7 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
||||
// with a sensible value (e.g. > 0)
|
||||
if (pl_status == asynSuccess && motorVelocityRec > 0.0) {
|
||||
// Convert back to the value in the VELO field
|
||||
motorVelocity = motorVelocityRec * motorRecRes;
|
||||
motorVelocity = motorVelocityRec * motRecRes;
|
||||
if (pl_status == asynSuccess) {
|
||||
|
||||
timeContSpeed =
|
||||
|
||||
Reference in New Issue
Block a user