| |
/* Enable the amplifier for every motor attached to a motion supervisor */
void motionAmpEnable(MPIMotion motion)
{
MPIControl control;
MPIAxis axis;
MPIMotor motor;
MPIFilter filter;
MPIObjectMap map;
MPIObjectMap motionMotorMap;
long motorIndex;
long filterIndex;
long returnValue;
double position;
long enableState;
/* Get the controller handle */
control = mpiMotionControl(motion);
for (axis = mpiMotionAxisFirst(motion);
axis != MPIHandleVOID;
axis = mpiMotionAxisNext(motion, axis)) {
/* Get the object map for the motors */
returnValue = mpiAxisMotorMapGet(axis, &map);
msgCHECK(returnValue);
/* Add map to motionMotorMap */
motionMotorMap |= map;
}
/* For every motor ... */
for (motorIndex = 0; motorIndex < MEIXmpMAX_Motors; motorIndex++) {
if (mpiObjectMapBitGET(motionMotorMap, motorIndex)) {
/* Create motor handle */
motor = mpiMotorCreate(control, motorIndex);
msgCHECK(mpiMotorValidate(motor));
/* Get the state of the amplifier */
returnValue = mpiMotorAmpEnableGet(motor, &enableState);
msgCHECK(returnValue);
/* If the amplifier is disabled ... */
if (enableState == FALSE) {
/* For every axis */
for (axis = mpiMotionAxisFirst(motion);
axis != MPIHandleVOID;
axis = mpiMotionAxisNext(motion, axis)) {
/* Get the object map for the motors */
returnValue = mpiAxisMotorMapGet(axis, &map);
msgCHECK(returnValue);
/* If axis is attached to motor ... */
if (mpiObjectMapBitGET(map, motorIndex)) {
/* Get the actual position of the axis */
returnValue = mpiAxisActualPositionGet(axis, &position);
msgCHECK(returnValue);
/* Set command position equal to actual position */
returnValue = mpiAxisCommandPositionSet(axis, position);
msgCHECK(returnValue);
}
}
/* Get the object map for the filters */
returnValue = mpiMotorFilterMapGet(motor, &map);
msgCHECK(returnValue);
/* For every filter ... */
for (filterIndex = 0;
filterIndex < MEIXmpMAX_Filters;
filterIndex++) {
if (mpiObjectMapBitGET(map, filterIndex)) {
/* Create filter handle */
filter = mpiFilterCreate(control, filterIndex);
msgCHECK(mpiFilterValidate(filter));
/* Reset integrator */
returnValue = mpiFilterIntegratorReset(filter);
msgCHECK(returnValue);
/* Delete filter handle */
returnValue = mpiFilterDelete(filter);
msgCHECK(returnValue);
}
}
/* Enable the amplifier */
returnValue = mpiMotorAmpEnableSet(motor, TRUE);
msgCHECK(returnValue);
}
/* Delete motor handle */
returnValue = mpiMotorDelete(motor);
msgCHECK(returnValue);
}
}
}
|