diff --git a/motorApp/AMCISrc/ANF2Driver.cpp b/motorApp/AMCISrc/ANF2Driver.cpp index 4d1377fc..a6a30652 100644 --- a/motorApp/AMCISrc/ANF2Driver.cpp +++ b/motorApp/AMCISrc/ANF2Driver.cpp @@ -582,6 +582,30 @@ asynStatus ANF2Axis::sendAccelAndVelocity(double acceleration, double velocity) return asynSuccess; } +/* + * This driver only sets the base speed at initialization when the configuration is sent. + * It is possible that the base speed (VBAS) in the motor record is inconsistent with the + * base speed set at initialization, since there is no way for an asyn motor driver to force + * the base speed to be reset when a user changes it. The resulting acceleration calculated + * by the motor record is likely to be incorrect. The following method calculates the + * acceleration that will give the correct acceleration time (ACCL) for the base speed that + * was specified at initialization. + */ +double ANF2Axis::correctAccel(double minVelocity, double maxVelocity, double acceleration) +{ + double accelTime; + double newAccel; + + accelTime = (maxVelocity - minVelocity) / acceleration; + newAccel = (maxVelocity - (double)baseSpeed_) / accelTime; + + printf("old acceleration = %lf\n", acceleration); + printf("new acceleration = %lf\n", newAccel); + + return newAccel; +} + + // MOVE asynStatus ANF2Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { @@ -598,6 +622,9 @@ asynStatus ANF2Axis::move(double position, int relative, double minVelocity, dou // Clear the motition registers zeroRegisters(motionReg_); + // Correct the acceleration + acceleration = correctAccel(minVelocity, maxVelocity, acceleration); + // This sets indices 2 & 3 of motionReg_ status = sendAccelAndVelocity(acceleration, maxVelocity); @@ -648,6 +675,9 @@ asynStatus ANF2Axis::home(double minVelocity, double maxVelocity, double acceler // Clear the motition registers zeroRegisters(motionReg_); + // Correct the acceleration + acceleration = correctAccel(minVelocity, maxVelocity, acceleration); + // This sets indices 2 & 3 of motionReg_ status = sendAccelAndVelocity(acceleration, maxVelocity); diff --git a/motorApp/AMCISrc/ANF2Driver.h b/motorApp/AMCISrc/ANF2Driver.h index b6e82ff8..6a2fd393 100644 --- a/motorApp/AMCISrc/ANF2Driver.h +++ b/motorApp/AMCISrc/ANF2Driver.h @@ -79,6 +79,7 @@ private: ANF2Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. * Abbreviated because it is used very frequently */ asynStatus sendAccelAndVelocity(double accel, double velocity); + double correctAccel(double minVelocity, double maxVelocity, double acceleration); void getInfo(); void reconfig(epicsInt32 value); void zeroRegisters(epicsInt32 *reg);