From c0c5b5b351c33549437cffbd4233edb15a9361c2 Mon Sep 17 00:00:00 2001 From: kpetersn Date: Wed, 8 Aug 2018 16:27:33 -0500 Subject: [PATCH] Multiple improvements to the Aerotech A3200 driver from Ron Sluiter: * .04 03-15-18 rls - Restored "Task number" argument to A3200AsynConfig. * .05 05-01-18 rls - Do not check the limit switches of virtual axes. * - Added "single-axis" (MOVE{ABS/INC} or "multi-axis" (LINEAR) user selectable move option. --- motorApp/AerotechSrc/AerotechRegister.cc | 12 ++- motorApp/AerotechSrc/drvA3200Asyn.cc | 117 +++++++++++++++-------- motorApp/AerotechSrc/drvA3200Asyn.h | 5 +- 3 files changed, 88 insertions(+), 46 deletions(-) diff --git a/motorApp/AerotechSrc/AerotechRegister.cc b/motorApp/AerotechSrc/AerotechRegister.cc index ce854133..60cc6bec 100644 --- a/motorApp/AerotechSrc/AerotechRegister.cc +++ b/motorApp/AerotechSrc/AerotechRegister.cc @@ -45,20 +45,24 @@ static const iocshArg ensembleAsynConfigArg2 = {"asyn address (GPIB)", iocshArgI static const iocshArg ensembleAsynConfigArg3 = {"Number of Axes", iocshArgInt}; static const iocshArg ensembleAsynConfigArg4 = {"Moving poll rate", iocshArgInt}; static const iocshArg ensembleAsynConfigArg5 = {"Idle poll rate", iocshArgInt}; +static const iocshArg a3200AsynConfigArg6 = {"Task number", iocshArgInt}; +static const iocshArg a3200AsynConfigArg7 = {"Linear move commands", iocshArgInt}; static const iocshArg * const EnsembleAsynSetupArgs[1] = {&ensembleAsynSetupArg0}; -static const iocshArg * const EnsembleAsynConfigArgs[6] = {&ensembleAsynConfigArg0, +static const iocshArg * const EnsembleAsynConfigArgs[8] = {&ensembleAsynConfigArg0, &ensembleAsynConfigArg1, &ensembleAsynConfigArg2, &ensembleAsynConfigArg3, &ensembleAsynConfigArg4, - &ensembleAsynConfigArg5}; + &ensembleAsynConfigArg5, + &a3200AsynConfigArg6, + &a3200AsynConfigArg7}; static const iocshFuncDef setupEnsembleAsyn = {"EnsembleAsynSetup", 1, EnsembleAsynSetupArgs}; static const iocshFuncDef configEnsembleAsyn = {"EnsembleAsynConfig", 6, EnsembleAsynConfigArgs}; static const iocshFuncDef setupA3200Asyn = {"A3200AsynSetup", 1, EnsembleAsynSetupArgs}; -static const iocshFuncDef configA3200Asyn = {"A3200AsynConfig", 6, EnsembleAsynConfigArgs}; +static const iocshFuncDef configA3200Asyn = {"A3200AsynConfig", 8, EnsembleAsynConfigArgs}; static void setupSoloistCallFunc(const iocshArgBuf *args) { @@ -84,7 +88,7 @@ static void setupA3200AsynCallFunc(const iocshArgBuf *args) } static void configA3200AsynCallFunc(const iocshArgBuf *args) { - A3200AsynConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival); + A3200AsynConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival, args[6].ival, args[7].ival); } static void AerotechRegister(void) diff --git a/motorApp/AerotechSrc/drvA3200Asyn.cc b/motorApp/AerotechSrc/drvA3200Asyn.cc index 91b16d6f..19719ef4 100644 --- a/motorApp/AerotechSrc/drvA3200Asyn.cc +++ b/motorApp/AerotechSrc/drvA3200Asyn.cc @@ -23,8 +23,8 @@ in file LICENSE that is included with this distribution. * Verified with firmware: * - 4.07.000 * -* A3200 Task Usage - This driver uses Task #2 for the ASCII Interface. Task #3 is used to clear Task #2 errors and -* ABORT axis moves issued by Task #2. +* A3200 Task Usage - This driver uses a user selectable Task #n for the ASCII Interface and +* Task #(n+1) to clear Task #n errors and ABORT axis moves issued by Task #n. * * Modification Log: * ----------------- @@ -38,6 +38,9 @@ in file LICENSE that is included with this distribution. * MoveDone false before the 1st status update. * - Added axis name to "RAMP RATE" command. * .03 10-14-15 rls - Use "ReverseDirec" parameter to set "HomeSetup" parameter. +* .04 03-15-18 rls - Restored "Task number" argument to A3200AsynConfig. +* .05 05-01-18 rls - Do not check the limit switches of virtual axes. +* - Added "single-axis" (MOVE{ABS/INC} or "multi-axis" (LINEAR) user selectable move option. */ #include @@ -150,6 +153,8 @@ typedef struct epicsEventId pollEventId; epicsMutexId sendReceiveMutex; AXIS_HDL pAxis; /* array of axes */ + epicsUInt32 taskNumber; /* the task number to use for motion commands */ + epicsUInt32 linear; } A3200Controller; typedef struct motorAxisHandle @@ -393,32 +398,33 @@ static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int value) { - int task2state, task3state, ret_status = MOTOR_AXIS_ERROR; + int tasknum, taskNstate, taskN1state, ret_status = MOTOR_AXIS_ERROR; char inputBuff[BUFFER_SIZE], outputBuff[BUFFER_SIZE]; - const char* TaskStateStr = "~STATUS (2, TaskState) (3, TaskState)"; if (pAxis == NULL || pAxis->pController == NULL) return MOTOR_AXIS_ERROR; + tasknum = pAxis->pController->taskNumber; + epicsMutexLock(pAxis->mutexId); switch (function) { case motorAxisClosedLoop: - ret_status = sendAndReceive(pAxis->pController, (char *) TaskStateStr, inputBuff, sizeof(inputBuff)); + sprintf(outputBuff, "~STATUS (%u, TaskState) (%u, TaskState)", tasknum, tasknum + 1); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); if (ret_status != asynSuccess || inputBuff[0] != ASCII_ACK_CHAR) { motorParam->setInteger(pAxis->params, motorAxisCommError, 1); break; } - sscanf(&inputBuff[1], "%d %d", &task2state, &task3state); - if (task2state == TASKSTATE_Idle) - sendAndReceive(pAxis->pController, "~INITQUEUE 2", inputBuff, sizeof(inputBuff)); -/* - if (task3state == TASKSTATE_Idle) - sendAndReceive(pController, "~INITQUEUE 3", inputBuff, sizeof(inputBuff)); -*/ + sscanf(&inputBuff[1], "%d %d", &taskNstate, &taskN1state); + if (taskNstate == TASKSTATE_Idle) + { + sprintf(outputBuff, "~INITQUEUE %u", tasknum); + sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + } if (value == 0) sprintf(outputBuff, "DISABLE %s", pAxis->axisName); else @@ -427,11 +433,13 @@ static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int va { if (pAxis->lastFault == 52 || pAxis->lastFault == 78) { - ret_status = sendAndReceive(pAxis->pController, (char *) "~TASK 3", inputBuff, sizeof(inputBuff)); + sprintf(outputBuff, "~TASK %u", tasknum + 1); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); /* Prevent Task #3 from blocking during LINEAR commands. */ ret_status = sendAndReceive(pAxis->pController, (char *) "WAIT MODE AUTO", inputBuff, sizeof(inputBuff)); ret_status = sendAndReceive(pAxis->pController, (char *) "ACKNOWLEDGEALL", inputBuff, sizeof(inputBuff)); - ret_status = sendAndReceive(pAxis->pController, (char *) "~TASK 2", inputBuff, sizeof(inputBuff)); + sprintf(outputBuff, "~TASK %u", tasknum); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); sprintf(outputBuff, "~INITQUEUE"); } else @@ -481,24 +489,43 @@ static int motorAxisMove(AXIS_HDL pAxis, double position, int relative, pAxis->card, pAxis->axisName, position, min_velocity, max_velocity, acceleration); if (relative) - { posdir = position >= 0.0; - moveCommand = "MOVEIINC"; + else + posdir = position >= pAxis->currentCmdPos; + + if (pAxis->pController->linear == 1) + { + if (relative) + moveCommand = "INCREMENTAL"; + else + moveCommand = "ABSOLUTE"; + ret_status = sendAndReceive(pAxis->pController, moveCommand, inputBuff, sizeof(inputBuff)); + if (ret_status) + return MOTOR_AXIS_ERROR; } else { - posdir = position >= pAxis->currentCmdPos; - moveCommand = "MOVEABS"; + if (relative) + moveCommand = "MOVEIINC"; + else + moveCommand = "MOVEABS"; } if (acceleration > 0) { /* only use the acceleration if > 0 */ - sprintf(outputBuff, "RAMP RATE %s %.*f", pAxis->axisName, pAxis->maxDigits, acceleration * fabs(pAxis->stepSize)); + if (pAxis->pController->linear == 1) + sprintf(outputBuff, "RAMP RATE %.*f", pAxis->maxDigits, acceleration * fabs(pAxis->stepSize)); + else + sprintf(outputBuff, "RAMP RATE %s %.*f", pAxis->axisName, pAxis->maxDigits, acceleration * fabs(pAxis->stepSize)); ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); } - - sprintf(outputBuff, "%s %s %.*f %.*f", moveCommand, pAxis->axisName, pAxis->maxDigits, position * fabs(pAxis->stepSize), - pAxis->maxDigits, max_velocity * fabs(pAxis->stepSize)); + + if (pAxis->pController->linear == 1) + sprintf(outputBuff, "LINEAR %s %.*f F%.*f", pAxis->axisName, pAxis->maxDigits, position * fabs(pAxis->stepSize), + pAxis->maxDigits, max_velocity * fabs(pAxis->stepSize)); + else + sprintf(outputBuff, "%s %s %.*f %.*f", moveCommand, pAxis->axisName, pAxis->maxDigits, position * fabs(pAxis->stepSize), + pAxis->maxDigits, max_velocity * fabs(pAxis->stepSize)); ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); if (ret_status || inputBuff[0] != ASCII_ACK_CHAR) @@ -508,7 +535,7 @@ static int motorAxisMove(AXIS_HDL pAxis, double position, int relative, int taskerr; motorParam->setInteger(pAxis->params, motorAxisProblem, 1); /* Signal "Controller Error" to user. */ - sprintf(outputBuff, "~STATUS(2, TaskErrorCode)"); + sprintf(outputBuff, "~STATUS(%u, TaskErrorCode)", pAxis->pController->taskNumber); ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); taskerr = atoi(&inputBuff[1]); if (taskerr != 0) @@ -637,21 +664,23 @@ static int motorAxisTriggerProfile(AXIS_HDL pAxis) static int motorAxisStop(AXIS_HDL pAxis, double acceleration) { - int ret_status; + int tasknum, ret_status; char inputBuff[BUFFER_SIZE], outputBuff[BUFFER_SIZE]; if (pAxis == NULL || pAxis->pController == NULL) return MOTOR_AXIS_ERROR; + tasknum = pAxis->pController->taskNumber; + PRINT(pAxis->logParam, FLOW, "Abort on card %d, axis %d\n", pAxis->card, pAxis->axis); /* we can't accurately determine which type of motion is occurring on the controller, * so don't worry about the acceleration rate, just stop the motion on the axis */ - sprintf(outputBuff, "~TASK 3"); + sprintf(outputBuff, "~TASK %u", tasknum + 1); ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); sprintf(outputBuff, "ABORT %s", pAxis->axisName); ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); - sprintf(outputBuff, "~TASK 2"); + sprintf(outputBuff, "~TASK %u", tasknum); ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); return ret_status; } @@ -735,15 +764,18 @@ static void A3200Poller(A3200Controller *pController) motorParam->setInteger(pAxis->params, motorAxisPowerOn, (drive_status & DRIVESTATUS_Enabled) != 0); motorParam->setInteger(pAxis->params, motorAxisHomeSignal, (axis_status & AXISSTATUS_Homed) != 0); - if (pAxis->reverseDirec == false) + if ((axis_status & AXISSTATUS_NotVirtual) != 0) { - motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, !((drive_status & DRIVESTATUS_CwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CWEOTSWstate)); - motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, !((drive_status & DRIVESTATUS_CcwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CCWEOTSWstate)); - } - else - { - motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, !((drive_status & DRIVESTATUS_CcwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CCWEOTSWstate)); - motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, !((drive_status & DRIVESTATUS_CwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CWEOTSWstate)); + if (pAxis->reverseDirec == false) + { + motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, !((drive_status & DRIVESTATUS_CwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CWEOTSWstate)); + motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, !((drive_status & DRIVESTATUS_CcwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CCWEOTSWstate)); + } + else + { + motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, !((drive_status & DRIVESTATUS_CcwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CCWEOTSWstate)); + motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, !((drive_status & DRIVESTATUS_CwEndOfTravelLimitInput) ^ pAxis->swconfig.Bits.CWEOTSWstate)); + } } pAxis->axisStatus = axis_status; @@ -807,10 +839,11 @@ int A3200AsynSetup(int num_controllers) /* number of A3200 controllers in syst int A3200AsynConfig(int card, /* Controller number */ const char *portName, /* asyn port name of serial or GPIB port */ int asynAddress, /* asyn subaddress for GPIB */ - int numAxes, /* The number of axes that the driver controls */ + int numAxes, /* The number of axes that the driver controls */ int movingPollPeriod, /* Time to poll (msec) when an axis is in motion */ - int idlePollPeriod) /* Time to poll (msec) when an axis is idle. 0 for no polling */ - + int idlePollPeriod, /* Time to poll (msec) when an axis is idle. 0 for no polling */ + int taskNumber, /* the task number to use for motion commands */ + int linear) /* Use linear (1) or single-axis (0) move commands. */ { A3200Controller *pController; char threadName[20]; @@ -838,6 +871,8 @@ int A3200AsynConfig(int card, /* Controller number */ pController = &pA3200Controller[card]; pController->numAxes = numAxes; + pController->taskNumber = taskNumber; + pController->linear = linear; pController->movingPollPeriod = movingPollPeriod / 1000.; pController->idlePollPeriod = idlePollPeriod / 1000.; @@ -866,7 +901,7 @@ int A3200AsynConfig(int card, /* Controller number */ do { - sprintf(outputBuff, "~TASK 2"); + sprintf(outputBuff, "~TASK %u", pController->taskNumber); status = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); retry++; } while(status != asynSuccess && retry < 3); @@ -947,9 +982,11 @@ int A3200AsynConfig(int card, /* Controller number */ /* Prevent Task #2 and #3 from blocking during LINEAR commands. */ sendAndReceive(pController, (char *) "WAIT MODE AUTO", inputBuff, sizeof(inputBuff)); - sendAndReceive(pController, (char *) "~TASK 3", inputBuff, sizeof(inputBuff)); + sprintf(outputBuff, "~TASK %u", pController->taskNumber + 1); + sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); sendAndReceive(pController, (char *) "WAIT MODE AUTO", inputBuff, sizeof(inputBuff)); - sendAndReceive(pController, (char *) "~TASK 2", inputBuff, sizeof(inputBuff)); + sprintf(outputBuff, "~TASK %u", pController->taskNumber); + sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); pController->pollEventId = epicsEventMustCreate(epicsEventEmpty); diff --git a/motorApp/AerotechSrc/drvA3200Asyn.h b/motorApp/AerotechSrc/drvA3200Asyn.h index be44f84a..8d57ac16 100644 --- a/motorApp/AerotechSrc/drvA3200Asyn.h +++ b/motorApp/AerotechSrc/drvA3200Asyn.h @@ -50,8 +50,9 @@ extern "C" { int asynAddress, /* asyn subaddress for GPIB */ int numAxes, /* The number of axes that the driver controls */ int movingPollPeriod, /* Time to poll (msec) when an axis is in motion */ - int idlePollPeriod); /* Time to poll (msec) when an axis is idle. 0 for no polling */ - + int idlePollPeriod, /* Time to poll (msec) when an axis is idle. 0 for no polling */ + int taskNumber, /* the task number to use for motion commands */ + int linear); /* linear (1) or single-axis (0) move commands. */ #ifdef __cplusplus } #endif