lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

- Removed "Task number" argument from A3200AsynConfig and switched to using Task #2 for the ASCII Interface. Task #3 is used to clear Task #2 errors and ABORT axis moves issued by Task #2.

Browse Source 
- Deleted duplicate A3200 Setup and Config arguments from AerotechRegister.cc.
- Switched from "LINEAR" TO "MOVE[ABS/INC]" commands.
- Added delays to motorAxisMove() and motorAxisVelocityMove() so controller has time to set MoveDone false before the 1st status update.
- Added axis name to "RAMP RATE" command.
This commit is contained in:
Ron Sluiter
2015-04-30 14:31:32 +00:00
parent 73eea6e747
commit a52ec2f62f
3 changed files with 87 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/AerotechSrc/AerotechRegister.cc
+4 -26
View File
@@ -50,17 +50,6 @@ static const iocshArg ensembleAsynConfigArg3 = {"Number of Axes", iocshArgInt};
static const iocshArg ensembleAsynConfigArg4 = {"Moving poll rate", iocshArgInt};
static const iocshArg ensembleAsynConfigArg5 = {"Idle poll rate", iocshArgInt};
// A3200 Asyn Setup arguments
static const iocshArg a3200AsynSetupArg0 = {"Max. controller count", iocshArgInt};
// A3200 Asyn Config arguments
static const iocshArg a3200AsynConfigArg0 = {"Card being configured", iocshArgInt};
static const iocshArg a3200AsynConfigArg1 = {"asyn port name", iocshArgString};
static const iocshArg a3200AsynConfigArg2 = {"asyn address", iocshArgInt};
static const iocshArg a3200AsynConfigArg3 = {"Number of Axes", iocshArgInt};
static const iocshArg a3200AsynConfigArg4 = {"Task number", iocshArgInt};
static const iocshArg a3200AsynConfigArg5 = {"Moving poll rate", iocshArgInt};
static const iocshArg a3200AsynConfigArg6 = {"Idle poll rate", iocshArgInt};
static const iocshArg * const EnsembleAsynSetupArgs[2] = {&ensembleAsynSetupArg0};
static const iocshArg * const EnsembleAsynConfigArgs[6] = {&ensembleAsynConfigArg0,
&ensembleAsynConfigArg1,
@@ -72,17 +61,8 @@ static const iocshArg * const EnsembleAsynConfigArgs[6] = {&ensembleAsynConfigAr
static const iocshFuncDef setupEnsembleAsyn = {"EnsembleAsynSetup", 1, EnsembleAsynSetupArgs};
static const iocshFuncDef configEnsembleAsyn = {"EnsembleAsynConfig", 6, EnsembleAsynConfigArgs};
static const iocshArg * const A3200AsynSetupArgs[2] = {&a3200AsynSetupArg0};
static const iocshArg * const A3200AsynConfigArgs[7] = {&a3200AsynConfigArg0,
&a3200AsynConfigArg1,
&a3200AsynConfigArg2,
&a3200AsynConfigArg3,
&a3200AsynConfigArg4,
&a3200AsynConfigArg5,
&a3200AsynConfigArg6};
static const iocshFuncDef setupA3200Asyn = {"A3200AsynSetup", 1, A3200AsynSetupArgs};
static const iocshFuncDef configA3200Asyn = {"A3200AsynConfig", 7, A3200AsynConfigArgs};
static const iocshFuncDef setupA3200Asyn = {"A3200AsynSetup", 1, EnsembleAsynSetupArgs};
static const iocshFuncDef configA3200Asyn = {"A3200AsynConfig", 6, EnsembleAsynConfigArgs};
static void setupSoloistCallFunc(const iocshArgBuf *args)
{
@@ -99,8 +79,7 @@ static void setupEnsembleAsynCallFunc(const iocshArgBuf *args)
}
static void configEnsembleAsynCallFunc(const iocshArgBuf *args)
{
EnsembleAsynConfig(args[0].ival, args[1].sval, args[2].ival,
args[3].ival, args[4].ival, args[5].ival);
EnsembleAsynConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}
static void setupA3200AsynCallFunc(const iocshArgBuf *args)
@@ -109,8 +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, args[6].ival);
A3200AsynConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}
static void AerotechRegister(void)
motorApp/AerotechSrc/drvA3200Asyn.cc
+83 -31
View File
@@ -16,23 +16,31 @@ HeadURL: $URL$
-----------------------------------------------------------------------------
COPYRIGHT NOTICE
-----------------------------------------------------------------------------
Copyright (c) 2002 The University of Chicago, as Operator of Argonne
National Laboratory.
Copyright (c) 2002 The University of Chicago, as Operator of Argonne National Laboratory.
Copyright (c) 2002 The Regents of the University of California, as
Operator of Los Alamos National Laboratory.
Synapps Versions 4-5
and higher are distributed subject to a Software License Agreement found
Synapps Versions 4-5 and higher are distributed subject to a Software License Agreement found
in file LICENSE that is included with this distribution.
-----------------------------------------------------------------------------
* NOTES
* -----
* 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.
*
* Modification Log:
* -----------------
*
* .01 11-15-13 cjb Initialized from drvEnsembleAsyn.c (Aerotech)
* .02 30-04-14 rls - Removed "Task number" argument from A3200AsynConfig and switched to using Task #2 for the ASCII
* Interface. Task #3 is used to clear Task #2 errors and ABORT axis moves issued by Task #2.
* - Deleted duplicate A3200 Setup and Config arguments from AerotechRegister.cc.
* - Switched from "LINEAR" TO "MOVE[ABS/INC]" commands.
* - Added delays to motorAxisMove() and motorAxisVelocityMove() so controller has time to set
* MoveDone false before the 1st status update.
* - Added axis name to "RAMP RATE" command.
*/
#include <stddef.h>
@@ -145,7 +153,6 @@ typedef struct
epicsEventId pollEventId;
epicsMutexId sendReceiveMutex;
AXIS_HDL pAxis; /* array of axes */
epicsUInt32 taskNumber; /* the task number to use for motion commands */
} A3200Controller;
typedef struct motorAxisHandle
@@ -392,8 +399,9 @@ static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double
static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int value)
{
int ret_status = MOTOR_AXIS_ERROR;
int task2state, task3state, 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;
@@ -403,19 +411,52 @@ static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int va
switch (function)
{
case motorAxisClosedLoop:
ret_status = sendAndReceive(pAxis->pController, (char *) TaskStateStr, inputBuff, sizeof(inputBuff));
if (ret_status != asynSuccess || inputBuff[0] != ASCII_ACK_CHAR)
{
motorParam->setInteger(pAxis->params, motorAxisCommError, 1);
epicsMutexUnlock(pAxis->mutexId);
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));
*/
if (value == 0)
sprintf(outputBuff, "DISABLE %s", pAxis->axisName);
else
{
if (pAxis->lastFault)
{
sprintf(outputBuff, "FAULTACK %s", pAxis->axisName);
if (pAxis->lastFault == 52 || pAxis->lastFault == 78)
{
ret_status = sendAndReceive(pAxis->pController, (char *) "~TASK 3", 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, "~INITQUEUE");
}
else
sprintf(outputBuff, "FAULTACK %s", pAxis->axisName);
ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff));
pAxis->lastFault = 0; /* Clear fault indicator. */
motorParam->setInteger(pAxis->params, motorAxisProblem, 0);
}
sprintf(outputBuff, "ENABLE %s", pAxis->axisName);
}
ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff));
/* Prevent Task #2 from blocking during LINEAR commands. */
ret_status = sendAndReceive(pAxis->pController, (char *) "WAIT MODE AUTO", inputBuff, sizeof(inputBuff));
break;
default:
PRINT(pAxis->logParam, TERROR, "motorAxisSetInteger: unknown function %d\n", function);
break;
@@ -447,30 +488,39 @@ static int motorAxisMove(AXIS_HDL pAxis, double position, int relative,
if (relative)
{
posdir = position >= 0.0;
moveCommand = "INCREMENTAL";
moveCommand = "MOVEIINC";
}
else
{
posdir = position >= pAxis->currentCmdPos;
moveCommand = "ABSOLUTE";
moveCommand = "MOVEABS";
}
ret_status = sendAndReceive(pAxis->pController, moveCommand, inputBuff, sizeof(inputBuff));
if (ret_status)
return MOTOR_AXIS_ERROR;
if (acceleration > 0)
{ /* only use the acceleration if > 0 */
sprintf(outputBuff, "RAMP RATE %.*f", pAxis->maxDigits, acceleration * fabs(pAxis->stepSize));
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, "LINEAR %s %.*f F%.*f", pAxis->axisName, pAxis->maxDigits, position * fabs(pAxis->stepSize),
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)
if (ret_status || inputBuff[0] != ASCII_ACK_CHAR)
{
if (inputBuff[0] == ASCII_FAULT_CHAR)
{
int taskerr;
motorParam->setInteger(pAxis->params, motorAxisProblem, 1); /* Signal "Controller Error" to user. */
sprintf(outputBuff, "~STATUS(2, TaskErrorCode)");
ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff));
taskerr = atoi(&inputBuff[1]);
if (taskerr != 0)
pAxis->lastFault = taskerr; /* Indicate a Fault needs to be ACK'd. */
}
return MOTOR_AXIS_ERROR;
}
if (epicsMutexLock(pAxis->mutexId) == epicsMutexLockOK)
{
@@ -478,6 +528,7 @@ static int motorAxisMove(AXIS_HDL pAxis, double position, int relative,
/* Ensure that the motor record's next status update sees motorAxisDone = False. */
motorParam->setInteger(pAxis->params, motorAxisDone, 0);
motorParam->callCallback(pAxis->params);
epicsThreadSleep(0.010); /* Delay status update so controller has time to set MoveDone false. */
epicsMutexUnlock(pAxis->mutexId);
}
@@ -564,6 +615,7 @@ static int motorAxisVelocityMove(AXIS_HDL pAxis, double min_velocity, double vel
/* Ensure that the motor record's next status update sees motorAxisDone = False. */
motorParam->setInteger(pAxis->params, motorAxisDone, 0);
motorParam->callCallback(pAxis->params);
epicsThreadSleep(0.010); /* Delay status update so controller has time to set MoveDone false. */
epicsMutexUnlock(pAxis->mutexId);
}
@@ -595,8 +647,12 @@ static int motorAxisStop(AXIS_HDL pAxis, double acceleration)
/* 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");
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");
ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff));
return ret_status;
}
@@ -660,14 +716,7 @@ static void A3200Poller(A3200Controller *pController)
pAxis->axisName);
status = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff));
if (status != asynSuccess)
{
motorParam->setInteger(pAxis->params, motorAxisCommError, 1);
epicsMutexUnlock(pAxis->mutexId);
continue;
}
if (inputBuff[0] != ASCII_ACK_CHAR)
if (status != asynSuccess || inputBuff[0] != ASCII_ACK_CHAR)
{
motorParam->setInteger(pAxis->params, motorAxisCommError, 1);
epicsMutexUnlock(pAxis->mutexId);
@@ -678,7 +727,7 @@ static void A3200Poller(A3200Controller *pController)
motorParam->setInteger(params, motorAxisCommError, 0);
move_active = drive_status & DRIVESTATUS_MoveActive;
move_active = !(axis_status & AXISSTATUS_MoveDone);
motorParam->setInteger(params, motorAxisDone, !move_active);
if (move_active)
anyMoving = true;
@@ -711,10 +760,9 @@ static void A3200Poller(A3200Controller *pController)
if (axis_fault && axis_fault != pAxis->lastFault)
{
PRINT(pAxis->logParam, TERROR, "A3200Poller: controller fault on axis=%s fault=0x%X\n", pAxis->axisName, axis_fault);
pAxis->lastFault = axis_fault;
}
pAxis->lastFault = axis_fault;
vfbk /= fabs(pAxis->stepSize);
motorParam->setDouble(pAxis->params, motorAxisActualVel, vfbk);
@@ -760,7 +808,6 @@ 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 taskNumber, /* the task number to use for motion commands */
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 */
@@ -791,7 +838,6 @@ int A3200AsynConfig(int card, /* Controller number */
pController = &pA3200Controller[card];
pController->numAxes = numAxes;
pController->taskNumber = taskNumber;
pController->movingPollPeriod = movingPollPeriod / 1000.;
pController->idlePollPeriod = idlePollPeriod / 1000.;
@@ -820,7 +866,7 @@ int A3200AsynConfig(int card, /* Controller number */
do
{
sprintf(outputBuff, "~TASK %u", pController->taskNumber);
sprintf(outputBuff, "~TASK 2");
status = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff));
retry++;
} while(status != asynSuccess && retry < 3);
@@ -892,6 +938,12 @@ int A3200AsynConfig(int card, /* Controller number */
sendAndReceive(pController, "~INITQUEUE", inputBuff, sizeof(inputBuff));
/* 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));
sendAndReceive(pController, (char *) "WAIT MODE AUTO", inputBuff, sizeof(inputBuff));
sendAndReceive(pController, (char *) "~TASK 2", inputBuff, sizeof(inputBuff));
pController->pollEventId = epicsEventMustCreate(epicsEventEmpty);
/* Create the poller thread for this controller */
motorApp/AerotechSrc/drvA3200Asyn.h
-1
View File
@@ -53,7 +53,6 @@ extern "C" {
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 taskNumber, /* The task number to use for motion commands */
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 */