diff --git a/motorApp/AerotechSrc/AerotechRegister.cc b/motorApp/AerotechSrc/AerotechRegister.cc index 73bd1b26..98a47a19 100644 --- a/motorApp/AerotechSrc/AerotechRegister.cc +++ b/motorApp/AerotechSrc/AerotechRegister.cc @@ -2,9 +2,10 @@ FILENAME... AerotechRegister.cc USAGE... Register Aerotech motor device driver shell commands. -Version: 1.0 -Modified By: weimer -Last Modified: 2008/04/10 05:52:37 PM +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ */ /***************************************************************** @@ -25,10 +26,10 @@ of this distribution. extern "C" { -// Aerotech Setup arguments +// Aerotech non-asyn Setup arguments static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; -// Aerotech Config arguments +// Aerotech non-asyn Config arguments static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; static const iocshArg configArg1 = {"asyn port name", iocshArgString}; static const iocshArg configArg2 = {"asyn address", iocshArgInt}; @@ -39,6 +40,33 @@ static const iocshArg * const EnsembleConfigArgs[3] = {&configArg0, &configArg1, static const iocshFuncDef setupEnsemble = {"EnsembleSetup", 2, EnsembleSetupArgs}; static const iocshFuncDef configEnsemble = {"EnsembleConfig", 2, EnsembleConfigArgs}; +static const iocshArg * const SoloistSetupArgs[2] = {&setupArg0, &setupArg1}; +static const iocshArg * const SoloistConfigArgs[3] = {&configArg0, &configArg1, &configArg2}; + +static const iocshFuncDef setupSoloist = {"SoloistSetup", 2, SoloistSetupArgs}; +static const iocshFuncDef configSoloist = {"SoloistConfig", 2, SoloistConfigArgs}; + +// Ensemble Asyn Setup arguments +static const iocshArg asynSetupArg0 = {"Max. controller count", iocshArgInt}; +// Ensemble Asyn Config arguments +static const iocshArg asynConfigArg0 = {"Card being configured", iocshArgInt}; +static const iocshArg asynConfigArg1 = {"asyn port name", iocshArgString}; +static const iocshArg asynConfigArg2 = {"asyn address (GPIB)", iocshArgInt}; +static const iocshArg asynConfigArg3 = {"Number of Axes", iocshArgInt}; +static const iocshArg asynConfigArg4 = {"Moving poll rate", iocshArgInt}; +static const iocshArg asynConfigArg5 = {"Idle poll rate", iocshArgInt}; + +static const iocshArg * const EnsembleAsynSetupArgs[2] = {&asynSetupArg0}; +static const iocshArg * const EnsembleAsynConfigArgs[6] = {&asynConfigArg0, + &asynConfigArg1, + &asynConfigArg2, + &asynConfigArg3, + &asynConfigArg4, + &asynConfigArg5}; + +static const iocshFuncDef setupEnsembleAsyn = {"EnsembleAsynSetup",1, EnsembleAsynSetupArgs}; +static const iocshFuncDef configEnsembleAsyn = {"EnsembleAsynConfig", 6, EnsembleAsynConfigArgs}; + static void setupEnsembleCallFunc(const iocshArgBuf *args) { EnsembleSetup(args[0].ival, args[1].ival); @@ -48,10 +76,33 @@ static void configEnsembleCallFunc(const iocshArgBuf *args) EnsembleConfig(args[0].ival, args[1].sval, args[2].ival); } +static void setupSoloistCallFunc(const iocshArgBuf *args) +{ + SoloistSetup(args[0].ival, args[1].ival); +} +static void configSoloistCallFunc(const iocshArgBuf *args) +{ + SoloistConfig(args[0].ival, args[1].sval, args[2].ival); +} + +static void setupEnsembleAsynCallFunc(const iocshArgBuf *args) +{ + EnsembleAsynSetup(args[0].ival); +} +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); +} + static void AerotechRegister(void) { iocshRegister(&setupEnsemble, setupEnsembleCallFunc); iocshRegister(&configEnsemble, configEnsembleCallFunc); + iocshRegister(&setupSoloist, setupSoloistCallFunc); + iocshRegister(&configSoloist, configSoloistCallFunc); + iocshRegister(&setupEnsembleAsyn, setupEnsembleAsynCallFunc); + iocshRegister(&configEnsembleAsyn, configEnsembleAsynCallFunc); } epicsExportRegistrar(AerotechRegister); diff --git a/motorApp/AerotechSrc/AerotechRegister.h b/motorApp/AerotechSrc/AerotechRegister.h index 48f08078..0e91a819 100644 --- a/motorApp/AerotechSrc/AerotechRegister.h +++ b/motorApp/AerotechSrc/AerotechRegister.h @@ -2,9 +2,10 @@ FILENAME... AerotechRegister.h USAGE... This file contains function prototypes for ACS IOC shell commands. -Version: 1.0 -Modified By: weimer -Last Modified: 2008/04/10 05:52:48 PM +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ */ /* @@ -34,13 +35,15 @@ Last Modified: 2008/04/10 05:52:48 PM * * Modification Log: * ----------------- + * .01 2008/04/10 caw Initial support for Ensemble + * .02 2009/04/29 hcf Added support for Soloist + * .03 2009/07/28 cjb Added support for Ensemble asynMotor */ #include "motor.h" #include "motordrvCom.h" +#include "drvEnsembleAsyn.h" #include "drvEnsemble.h" +#include "drvSoloist.h" -/* Function prototypes. */ -//extern RTN_STATUS EnsembleSetup(int, int); -//extern RTN_STATUS EnsembleConfig(int, const char *, int); diff --git a/motorApp/AerotechSrc/Makefile b/motorApp/AerotechSrc/Makefile index 69c6d5cc..2d49c5b5 100644 --- a/motorApp/AerotechSrc/Makefile +++ b/motorApp/AerotechSrc/Makefile @@ -3,7 +3,7 @@ TOP = ../.. include $(TOP)/configure/CONFIG # The following are used for debugging messages. -#USR_CXXFLAGS += -DDEBUG +#!USR_CXXFLAGS += -DDEBUG DBD += devAerotech.dbd @@ -12,6 +12,8 @@ LIBRARY_IOC = Aerotech # Aerotech driver support. SRCS += AerotechRegister.cc SRCS += devEnsemble.cc drvEnsemble.cc +SRCS += devSoloist.cc drvSoloist.cc +SRCS += drvEnsembleAsyn.c Aerotech_LIBS += motor asyn Aerotech_LIBS += $(EPICS_BASE_IOC_LIBS) diff --git a/motorApp/AerotechSrc/README b/motorApp/AerotechSrc/README new file mode 100644 index 00000000..186c6d61 --- /dev/null +++ b/motorApp/AerotechSrc/README @@ -0,0 +1,12 @@ +DESIGN NOTES +============ + +- The EPICS driver neither reads nor writes the Ensemble's soft limits. +It is recommended that the Ensemble soft limits be disabled and the +user rely on the EPICS motor record soft limits. + +- Since the Ensemble network connection requires period communication +from the host to prevent the Ensemble from closing the network socket, +the Ensemble support based on the old device driver architecture will +be removed after R5-6. The asyn motor architecture support continuous, +periodic updates; the old architecture does not. diff --git a/motorApp/AerotechSrc/devAerotech.dbd b/motorApp/AerotechSrc/devAerotech.dbd index ab832d73..e92fbab1 100644 --- a/motorApp/AerotechSrc/devAerotech.dbd +++ b/motorApp/AerotechSrc/devAerotech.dbd @@ -1,5 +1,13 @@ # Aerotech Ensemble Driver support. device(motor,VME_IO,devEnsemble,"Ensemble") driver(drvEnsemble) + +# Aerotech Soloist Driver support +device(motor,VME_IO,devSoloist,"Soloist") +driver(drvSoloist) + +# Aerotech Ensemble asynMotor support +driver(motorEnsemble) + registrar(AerotechRegister) diff --git a/motorApp/AerotechSrc/devEnsemble.cc b/motorApp/AerotechSrc/devEnsemble.cc index 8d40d9f7..b509cf5b 100644 --- a/motorApp/AerotechSrc/devEnsemble.cc +++ b/motorApp/AerotechSrc/devEnsemble.cc @@ -2,9 +2,10 @@ * FILENAME... devEnsemble.cc * USAGE... Motor record device level support for Aerotech Ensemble. * - * Version: $Revision: 1.8 $ - * Modified By: $Author: sluiter $ - * Last Modified: $Date: 2009-07-15 19:05:58 $ + * Version: $Revision$ + * Modified By: $Author$ + * Last Modified: $Date$ + * HeadURL: $URL$ */ /* @@ -43,6 +44,8 @@ * firmware 2.5.2. * .04 05-01-09 rls - Fix for jog velocity not adjusted by * cntrl->drive_resolution. + * .05 03-02-10 rls - removed setting controller's soft limits; see README + * - Depreciated version; use the asyn motor version. */ @@ -341,29 +344,8 @@ static RTN_STATUS Ensemble_build_trans (motor_cmnd command, double *parms, break; case SET_HIGH_LIMIT: - sprintf(buff, "SETPARM(@%d, 48, %.*f)", axis, maxdigits, cntrl_units); //ThresholdSoftCW - //motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis]; - //trans->state = IDLE_STATE; // No command sent to the controller. - - //if(cntrl_units > motor_info->high_limit) - //{ - // mr->dhlm = motor_info->high_limit; - // rtnval = ERROR; - //} - //send = false; - break; - case SET_LOW_LIMIT: - sprintf(buff, "SETPARM(@%d, 47, %.*f)", axis, maxdigits, cntrl_units); //ThresholdSoftCCW - //motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis]; - //trans->state = IDLE_STATE; // No command sent to the controller. - - //if(cntrl_units < motor_info->low_limit) - //{ - // mr->dllm = motor_info->low_limit; - // rtnval = ERROR; - //} - //send = false; + send = false; break; case SET_RESOLUTION: diff --git a/motorApp/AerotechSrc/drvEnsemble.cc b/motorApp/AerotechSrc/drvEnsemble.cc index 14462847..ba664dbd 100644 --- a/motorApp/AerotechSrc/drvEnsemble.cc +++ b/motorApp/AerotechSrc/drvEnsemble.cc @@ -2,9 +2,10 @@ FILENAME... drvEnsemble.cc USAGE... Motor record driver level support for Aerotech Ensemble. -Version: $Revision: 1.7 $ -Modified By: $Author: sluiter $ -Last Modified: $Date: 2009-09-08 18:24:33 $ +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ */ /* @@ -47,7 +48,10 @@ Last Modified: $Date: 2009-09-08 18:24:33 $ * - More extensive comm. error checks in set_status(); handle * ASCII_ACK_CHAR as error. * - cntrl->drive_resolution must be initialized with fabs(). - * + * .03 03-02-10 rls - Use sign(drive_resolution) to determine +/- limit switch + * status. Removed reading controller's soft limits; see + * README file. This version is depreciated; use the asyn + * motor version. */ @@ -275,7 +279,11 @@ static int set_status(int card, int signal) status.Bits.EA_SLIP_STALL = 0; // fill in the status - status.Bits.RA_DIRECTION = axis_status & DIRECTION_BIT ? 0 : 1; + if (cntrl->drive_resolution[signal] > 0.0) + status.Bits.RA_DIRECTION = axis_status & DIRECTION_BIT ? 1 : 0; + else + status.Bits.RA_DIRECTION = axis_status & DIRECTION_BIT ? 0 : 1; + status.Bits.RA_DONE = axis_status & IN_POSITION_BIT ? 1 : 0; status.Bits.RA_HOME = axis_status & HOME_LIMIT_BIT ? 1 : 0; status.Bits.EA_POSITION = axis_status & ENABLED_BIT ? 1 : 0; @@ -287,8 +295,16 @@ static int set_status(int card, int signal) send_mess(card, buff, (char) NULL); comm_status = recv_mess(card, buff, 1); axis_status = atoi(&buff[1]); - status.Bits.RA_PLUS_LS = axis_status & CCW_FAULT_BIT ? 1 : 0; - status.Bits.RA_MINUS_LS = axis_status & CW_FAULT_BIT ? 1 : 0; + if (cntrl->drive_resolution[signal] > 0.0) + { + status.Bits.RA_PLUS_LS = axis_status & CW_FAULT_BIT ? 1 : 0; + status.Bits.RA_MINUS_LS = axis_status & CCW_FAULT_BIT ? 1 : 0; + } + else + { + status.Bits.RA_PLUS_LS = axis_status & CCW_FAULT_BIT ? 1 : 0; + status.Bits.RA_MINUS_LS = axis_status & CW_FAULT_BIT ? 1 : 0; + } plusdir = status.Bits.RA_DIRECTION ? true : false; if ((status.Bits.RA_PLUS_LS && plusdir) || (status.Bits.RA_MINUS_LS && !plusdir)) @@ -311,7 +327,7 @@ static int set_status(int card, int signal) } // fill in the position - motorData = pfbk / cntrl->drive_resolution[signal]; + motorData = pfbk / fabs(cntrl->drive_resolution[signal]); if (motorData == motor_info->position) { @@ -572,16 +588,14 @@ static int motor_init() initialized = true; if (Ensemble_num_cards <= 0) - { return(ERROR); - } for (card_index = 0; card_index < Ensemble_num_cards; card_index++) { + int retry = 0; + if (!motor_state[card_index]) - { continue; - } brdptr = motor_state[card_index]; brdptr->cmnd_response = true; @@ -591,154 +605,131 @@ static int motor_init() // Initialize communications channel success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, cntrl->asyn_address, &cntrl->pasynUser, NULL); - if (success_rtn == asynSuccess) + if (success_rtn != asynSuccess) + continue; + + // Send a message to the baord, see if it exists + // flush any junk at input port - should not be any data available + pasynOctetSyncIO->flush(cntrl->pasynUser); + + do { - int retry = 0; + // we only care if we get a response + // so we don't need to send a valid command + strcpy(buff, "NONE"); + send_mess(card_index, buff, (char) NULL); + status = recv_mess(card_index, buff, 1); + retry++; + } while (!status && retry < 3); - // Send a message to the baord, see if it exists - // flush any junk at input port - should not be any data available - pasynOctetSyncIO->flush(cntrl->pasynUser); + if (status > 0) + { + brdptr->localaddr = (char *) NULL; + brdptr->motor_in_motion = 0; + // Read controller ID string + strcpy(buff, "GETPARM(CONTROL, 265)"); //UserString1 + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + strcpy(brdptr->ident, &buff[1]); + else + sprintf(brdptr->ident, "Ensemble%d", card_index); - do + // Get the number of axes + brdptr->total_axis = 0; + for (motor_index = 0; motor_index < 10; motor_index++) { - // we only care if we get a response - // so we don't need to send a valid command - strcpy(buff, "NONE"); - send_mess(card_index, buff, (char) NULL); - status = recv_mess(card_index, buff, 1); - - retry++; - } while (!status && retry < 3); - - if (status > 0) - { - brdptr->localaddr = (char *) NULL; - brdptr->motor_in_motion = 0; - // Read controller ID string - strcpy(buff, "GETPARM(CONTROL, 265)"); //UserString1 + // Does this axis actually exist? + sprintf(buff, "GETPARM(@%d, 257)", motor_index); //AxisName send_mess(card_index, buff, (char) NULL); recv_mess(card_index, buff, 1); + + // We know the axis exists if we got an ACK response if (buff[0] == ASCII_ACK_CHAR) { - strcpy(brdptr->ident, &buff[1]); - } - else - { - sprintf(brdptr->ident, "Ensemble%d", card_index); + cntrl->axes[motor_index] = 1; + brdptr->total_axis++; } + } - // Get the number of axes - brdptr->total_axis = 0; - for (motor_index = 0; motor_index < 10; motor_index++) + for (motor_index = 0; motor_index < 10; motor_index++) + { + if (cntrl->axes[motor_index]) { - // Does this axis actually exist? - sprintf(buff, "GETPARM(@%d, 257)", motor_index); //AxisName + struct mess_info *motor_info = &brdptr->motor_info[motor_index]; + + motor_info->status.All = 0; + motor_info->no_motion_count = 0; + motor_info->encoder_position = 0; + motor_info->position = 0; + brdptr->motor_info[motor_index].motor_motion = NULL; + + // Determine if encoder present based on open/closed loop mode. + sprintf(buff, "GETPARM(@%d, 58)", motor_index); //CfgFbkPosType + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + if (atoi(&buff[1]) > 0) + { + motor_info->encoder_present = YES; + motor_info->status.Bits.EA_PRESENT = 1; + } + } + + // Determine if gains are supported based on the motor type. + sprintf(buff, "GETPARM(@%d, 33)", motor_index); //CfgMotType + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + if (atoi(&buff[1]) != 3) + { + motor_info->pid_present = YES; + motor_info->status.Bits.GAIN_SUPPORT = 1; + } + } + + // Stop all motors + sprintf(buff, "ABORT @%d", motor_index); send_mess(card_index, buff, (char) NULL); recv_mess(card_index, buff, 1); - // We know the axis exists if we got an ACK response + // Determive drive resolution + sprintf(buff, "GETPARM(@%d, 3)", motor_index); //PosScaleFactor + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); if (buff[0] == ASCII_ACK_CHAR) - { - cntrl->axes[motor_index] = 1; - brdptr->total_axis++; - } + cntrl->drive_resolution[motor_index] = 1 / atof(&buff[1]); + else + cntrl->drive_resolution[motor_index] = 1; + + digits = (int) -log10(fabs(cntrl->drive_resolution[motor_index])) + 2; + if (digits < 1) + digits = 1; + cntrl->res_decpts[motor_index] = digits; + + // Save home preset position + sprintf(buff, "GETPARM(@%d, 108)", motor_index); //HomeOffset + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + cntrl->home_preset[motor_index] = atof(&buff[1]); + + // Save the HomeDirection parameter + sprintf(buff, "GETPARM(@%d, 106)", motor_index); //HomeDirection + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + cntrl->home_dparam[motor_index] = atoi(&buff[1]); + + // Read status of each motor + set_status(card_index, motor_index); } - - for (motor_index = 0; motor_index < 10; motor_index++) - { - if (cntrl->axes[motor_index]) - { - struct mess_info *motor_info = &brdptr->motor_info[motor_index]; - - motor_info->status.All = 0; - motor_info->no_motion_count = 0; - motor_info->encoder_position = 0; - motor_info->position = 0; - brdptr->motor_info[motor_index].motor_motion = NULL; - - // Determine if encoder present based on open/closed loop mode. - sprintf(buff, "GETPARM(@%d, 58)", motor_index); //CfgFbkPosType - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - if (buff[0] == ASCII_ACK_CHAR) - { - if (atoi(&buff[1]) > 0) - { - motor_info->encoder_present = YES; - motor_info->status.Bits.EA_PRESENT = 1; - } - } - - // Determine if gains are supported based on the motor type. - sprintf(buff, "GETPARM(@%d, 33)", motor_index); //CfgMotType - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - if (buff[0] == ASCII_ACK_CHAR) - { - if (atoi(&buff[1]) != 3) - { - motor_info->pid_present = YES; - motor_info->status.Bits.GAIN_SUPPORT = 1; - } - } - - // Stop all motors - sprintf(buff, "ABORT @%d", motor_index); - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - - // Determive drive resolution - sprintf(buff, "GETPARM(@%d, 3)", motor_index); //PosScaleFactor - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - if (buff[0] == ASCII_ACK_CHAR) - cntrl->drive_resolution[motor_index] = 1 / fabs(atof(&buff[1])); - else - cntrl->drive_resolution[motor_index] = 1; - - digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2; - if (digits < 1) - digits = 1; - cntrl->res_decpts[motor_index] = digits; - - // Save home preset position - sprintf(buff, "GETPARM(@%d, 108)", motor_index); //HomeOffset - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - if (buff[0] == ASCII_ACK_CHAR) - cntrl->home_preset[motor_index] = atof(&buff[1]); - - // Determine low limit - sprintf(buff, "GETPARM(@%d, 47)", motor_index); //ThresholdSoftCCW - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - if (buff[0] == ASCII_ACK_CHAR) - motor_info->low_limit = atof(&buff[1]); - - // Determine high limit - sprintf(buff, "GETPARM(@%d, 48)", motor_index); //ThresholdSoftCW - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - if (buff[0] == ASCII_ACK_CHAR) - motor_info->high_limit = atof(&buff[1]); - - // Save the HomeDirection parameter - sprintf(buff, "GETPARM(@%d, 106)", motor_index); //HomeDirection - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); - if (buff[0] == ASCII_ACK_CHAR) - cntrl->home_dparam[motor_index] = atoi(&buff[1]); - - // Read status of each motor - set_status(card_index, motor_index); - } - } - } - else - { - motor_state[card_index] = (struct controller *) NULL; } } + else + motor_state[card_index] = (struct controller *) NULL; } any_motor_in_motion = 0; diff --git a/motorApp/AerotechSrc/drvEnsembleAsyn.c b/motorApp/AerotechSrc/drvEnsembleAsyn.c new file mode 100755 index 00000000..830cf4c6 --- /dev/null +++ b/motorApp/AerotechSrc/drvEnsembleAsyn.c @@ -0,0 +1,923 @@ +/* +FILENAME... drvEnsembleAsyn.c +USAGE... Motor record asyn driver level support for Aerotech Ensemble. + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* +* Original Author: Mark Rivers +* Date: 07/28/09 +* Current Author: Aerotech, Inc. +* +----------------------------------------------------------------------------- +COPYRIGHT NOTICE +----------------------------------------------------------------------------- +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 +in file LICENSE that is included with this distribution. +----------------------------------------------------------------------------- +* +* Modification Log: +* ----------------- +* +* .01 07-28-09 cjb Initialized from drvMM4000Asyn.c (Newport) +* .02 03-02-10 rls Original check into version control. +* +*/ + + +#include +#include "epicsThread.h" +#include +#include +#include +#include +#include + +#include "drvEnsembleAsyn.h" +#include "paramLib.h" + +#include "epicsFindSymbol.h" +#include "epicsTime.h" +#include "epicsThread.h" +#include "epicsEvent.h" +#include "epicsString.h" +#include "epicsMutex.h" +#include "ellLib.h" +#include "asynDriver.h" +#include "asynOctetSyncIO.h" +#include "errlog.h" + +#include "drvSup.h" +#include "epicsExport.h" +#define DEFINE_MOTOR_PROTOTYPES 1 +#include "motor_interface.h" + +motorAxisDrvSET_t motorEnsemble = +{ + 14, + motorAxisReport, /**< Standard EPICS driver report function (optional) */ + motorAxisInit, /**< Standard EPICS dirver initialisation function (optional) */ + motorAxisSetLog, /**< Defines an external logging function (optional) */ + motorAxisOpen, /**< Driver open function */ + motorAxisClose, /**< Driver close function */ + motorAxisSetCallback, /**< Provides a callback function the driver can call when the status updates */ + motorAxisSetDouble, /**< Pointer to function to set a double value */ + motorAxisSetInteger, /**< Pointer to function to set an integer value */ + motorAxisGetDouble, /**< Pointer to function to get a double value */ + motorAxisGetInteger, /**< Pointer to function to get an integer value */ + motorAxisHome, /**< Pointer to function to execute a more to reference or home */ + motorAxisMove, /**< Pointer to function to execute a position move */ + motorAxisVelocityMove, /**< Pointer to function to execute a velocity mode move */ + motorAxisStop, /**< Pointer to function to stop motion */ + motorAxisforceCallback, /**< Pointer to function to request a poller status update */ + motorAxisProfileMove, /**< Pointer to function to execute a profile move */ + motorAxisTriggerProfile /**< Pointer to function to trigger a profile move */ +}; + +epicsExportAddress(drvet, motorEnsemble); + +typedef struct +{ + epicsMutexId EnsembleLock; + asynUser *pasynUser; + int numAxes; + double movingPollPeriod; + double idlePollPeriod; + epicsEventId pollEventId; + epicsMutexId sendReceiveMutex; + AXIS_HDL pAxis; /* array of axes */ +} EnsembleController; + +typedef struct motorAxisHandle +{ + EnsembleController *pController; + PARAMS params; + double currentPosition; + double stepSize; + double homePreset; + int homeDirection; + int closedLoop; + int axisStatus; + int card; + int axis; + int maxDigits; + motorAxisLogFunc print; + void *logParam; + epicsMutexId mutexId; +} motorAxis; + +typedef struct +{ + AXIS_HDL pFirst; + epicsThreadId motorThread; + motorAxisLogFunc print; + void *logParam; + epicsTimeStamp now; +} motorEnsemble_t; + + +static int motorEnsembleLogMsg(void * param, const motorAxisLogMask_t logMask, const char *pFormat, ...); +static asynStatus sendAndReceive(EnsembleController *pController, char *outputString, char *inputString, int inputSize); + +#define PRINT (drv.print) +#define FLOW motorAxisTraceFlow +#define TERROR motorAxisTraceError +#define IODRIVER motorAxisTraceIODriver + +#define ENSEMBLE_MAX_AXES 10 +#define BUFFER_SIZE 100 /* Size of input and output buffers */ +#define TIMEOUT 2.0 /* Timeout for I/O in seconds */ + + +/* Status byte bits */ +#define ENABLED_BIT 0x00000001 +#define IN_POSITION_BIT 0x00000004 +#define IN_MOTION_BIT 0x00000008 +#define DIRECTION_BIT 0x00000200 +#define HOME_LIMIT_BIT 0x01000000 +#define HOME_MARKER_BIT 0x02000000 + + +/* Fault status bits */ +#define CW_FAULT_BIT 0x004 +#define CCW_FAULT_BIT 0x008 + + +/* The following should be defined to have the same value as +the Ensemble parameters specified */ +#define ASCII_EOS_CHAR '\n' /* AsciiCmdEOSChar */ +#define ASCII_EOS_STR "\n" +#define ASCII_ACK_CHAR '%' /* AsciiCmdAckChar */ +#define ASCII_NAK_CHAR '!' /* AsciiCmdNakChar */ +#define ASCII_FAULT_CHAR '#' /* AsciiCmdFaultChar */ +#define ASCII_TIMEOUT_CHAR '$' /* AsciiCmdTimeoutChar */ + +#define TCP_TIMEOUT 2.0 +static motorEnsemble_t drv={ NULL, NULL, motorEnsembleLogMsg, 0, { 0, 0}}; +static int numEnsembleControllers; +/* Pointer to array of controller strutures */ +static EnsembleController *pEnsembleController=NULL; + +#define MAX(a,b) ((a)>(b)? (a): (b)) +#define MIN(a,b) ((a)<(b)? (a): (b)) + +static void motorAxisReportAxis(AXIS_HDL pAxis, int level) +{ + if (level > 0) + { + printf("Axis %d\n", pAxis->axis); + printf(" axisStatus: 0x%x\n", pAxis->axisStatus); + printf(" home preset: %f\n", pAxis->homePreset); + printf(" step size: %f\n", pAxis->stepSize); + printf(" max digits: %d\n", pAxis->maxDigits); + } +} + +static void motorAxisReport(int level) +{ + int i, j; + + for (i=0; imutexId) + break; + epicsMutexLock(pAxis->mutexId); + + /*Set GAIN_SUPPORT on so that at least, CNEN functions. */ + motorParam->setInteger(pAxis->params, motorAxisHasClosedLoop, 1); + + motorParam->callCallback(pAxis->params); + epicsMutexUnlock(pAxis->mutexId); + } + } + return(MOTOR_AXIS_OK); +} + +static int motorAxisSetLog( AXIS_HDL pAxis, motorAxisLogFunc logFunc, void * param ) +{ + if (pAxis == NULL) + { + if (logFunc == NULL) + { + drv.print=motorEnsembleLogMsg; + drv.logParam = NULL; + } + else + { + drv.print=logFunc; + drv.logParam = param; + } + } + else + { + if (logFunc == NULL) + { + pAxis->print=motorEnsembleLogMsg; + pAxis->logParam = NULL; + } + else + { + pAxis->print=logFunc; + pAxis->logParam = param; + } + } + return (MOTOR_AXIS_OK); +} + +static AXIS_HDL motorAxisOpen(int card, int axis, char * param) +{ + AXIS_HDL pAxis; + + if (card > numEnsembleControllers) + return(NULL); + if (axis > pEnsembleController[card].numAxes) + return(NULL); + pAxis = &pEnsembleController[card].pAxis[axis]; + return (pAxis); +} + +static int motorAxisClose(AXIS_HDL pAxis) +{ + return (MOTOR_AXIS_OK); +} + +static int motorAxisGetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int * value) +{ + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + else + return (motorParam->getInteger(pAxis->params, (paramIndex) function, value)); +} + +static int motorAxisGetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double * value) +{ + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + else + return (motorParam->getDouble(pAxis->params, (paramIndex) function, value)); +} + +static int motorAxisSetCallback(AXIS_HDL pAxis, motorAxisCallbackFunc callback, void * param) +{ + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + else + return (motorParam->setCallback(pAxis->params, callback, param)); +} + +static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double value) +{ + int ret_status = MOTOR_AXIS_ERROR; + double deviceValue; + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + else + { + epicsMutexLock(pAxis->mutexId); + switch (function) + { + case motorAxisPosition: + { + deviceValue = value * fabs(pAxis->stepSize); + sprintf(outputBuff, "SETPOSCMD @%d, %.*f", pAxis->axis, pAxis->maxDigits, deviceValue); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + break; + } + case motorAxisEncoderRatio: + { + PRINT(pAxis->logParam, TERROR, "motorAxisSetDouble: Ensemble does not support setting encoder ratio\n"); + break; + } + case motorAxisResolution: + { + /* we need to scale over a dozen other parameters if this changed in some cases, so the user should just use + * the Configuration Manager to change this setting to ensure that this is done correctly */ + PRINT(pAxis->logParam, TERROR, "motorAxisSetDouble: Ensemble does not support setting motor resolution\n"); + break; + } + case motorAxisLowLimit: + { + PRINT(pAxis->logParam, TERROR, "Driver does not set Ensemble's Low Limit\n"); + break; + } + case motorAxisHighLimit: + { + PRINT(pAxis->logParam, TERROR, "Driver does not set Ensemble's High Limit\n"); + break; + } + case motorAxisPGain: + { + PRINT(pAxis->logParam, TERROR, "Ensemble does not support setting proportional gain\n"); + break; + } + case motorAxisIGain: + { + PRINT(pAxis->logParam, TERROR, "Ensemble does not support setting integral gain\n"); + break; + } + case motorAxisDGain: + { + PRINT(pAxis->logParam, TERROR, "Ensemble does not support setting derivative gain\n"); + break; + } + default: + PRINT(pAxis->logParam, TERROR, "motorAxisSetDouble: unknown function %d\n", function); + break; + } + if (ret_status == MOTOR_AXIS_OK ) + { + motorParam->setDouble(pAxis->params, function, value); + motorParam->callCallback(pAxis->params); + } + epicsMutexUnlock(pAxis->mutexId); + } + return (ret_status); +} + +static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int value) +{ + int ret_status = MOTOR_AXIS_ERROR; + int status; + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + + epicsMutexLock(pAxis->mutexId); + + switch (function) + { + case motorAxisClosedLoop: + if (value == 0) + sprintf(outputBuff, "DISABLE @%d", pAxis->axis); + else + sprintf(outputBuff, "ENABLE @%d", pAxis->axis); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + break; + default: + PRINT(pAxis->logParam, TERROR, "motorAxisSetInteger: unknown function %d\n", function); + break; + } + if (ret_status != MOTOR_AXIS_ERROR) + { + status = motorParam->setInteger(pAxis->params, function, value); + motorParam->callCallback(pAxis->params); + } + epicsMutexUnlock(pAxis->mutexId); + return (ret_status); +} + + +static int motorAxisMove(AXIS_HDL pAxis, double position, int relative, + double min_velocity, double max_velocity, double acceleration) +{ + int ret_status; + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + char *moveCommand; + int posdir; + int axis, maxDigits; + + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + + axis = pAxis->axis; + maxDigits = pAxis->maxDigits; + + PRINT(pAxis->logParam, FLOW, "Set card %d, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f\n", + pAxis->card, axis, position, min_velocity, max_velocity, acceleration); + + posdir = 0; + + if (relative) + { + if (position >= 0.0) + posdir = 1; + else + posdir = 0; + moveCommand = "MOVEINC"; + } + else + { + if (position >= pAxis->currentPosition) + posdir = 1; + else + posdir = 0; + moveCommand = "MOVEABS"; + } + + if (acceleration > 0) + { /* only use the acceleration if > 0 */ + sprintf(outputBuff, "SETPARM @%d, 103, %.*f", axis, maxDigits, acceleration * fabs(pAxis->stepSize)); /* DefaultRampRate */ + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + } + + sprintf(outputBuff, "%s @%d %.*f @%dF%.*f", moveCommand, axis, maxDigits, + position * fabs(pAxis->stepSize), axis, maxDigits, + max_velocity * fabs(pAxis->stepSize)); + + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (ret_status) + return (MOTOR_AXIS_ERROR); + + if (epicsMutexLock(pAxis->mutexId) == epicsMutexLockOK) + { + motorParam->setInteger(pAxis->params, motorAxisDirection, posdir); + /* Ensure that the motor record's next status update sees motorAxisDone = False. */ + motorParam->setInteger(pAxis->params, motorAxisDone, 0); + motorParam->callCallback(pAxis->params); + epicsMutexUnlock(pAxis->mutexId); + } + + /* Send a signal to the poller task which will make it do a poll, and switch to the moving poll rate */ + epicsEventSignal(pAxis->pController->pollEventId); + + return (MOTOR_AXIS_OK); +} + +static int motorAxisHome(AXIS_HDL pAxis, double min_velocity, double max_velocity, double acceleration, int forwards) +{ + int ret_status; + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + epicsUInt32 hparam; + int axis; + + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + + axis = pAxis->axis; + + PRINT(pAxis->logParam, FLOW, "motorAxisHome: set card %d, axis %d to home, forwards = %d\n", + pAxis->card, axis, forwards); + + if (max_velocity > 0) + { + sprintf(outputBuff, "SETPARM @%d, 107, %.*f", axis, pAxis->maxDigits, max_velocity * fabs(pAxis->stepSize)); /* HomeFeedRate */ + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + } + + if (acceleration > 0) + sprintf(outputBuff, "SETPARM @%d, 109, %.*f", axis, pAxis->maxDigits, acceleration * fabs(pAxis->stepSize)); /* HomeAccelDecelRate */ + + hparam = pAxis->homeDirection; + if (forwards == 1) + hparam |= 0x00000001; + else + hparam &= 0xFFFFFFFE; + pAxis->homeDirection = hparam; + + sprintf(outputBuff, "SETPARM @%d, 106, %d", axis, hparam); /* HomeDirection */ + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + + sprintf(outputBuff, "HOME @%d", axis); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (ret_status) + return(MOTOR_AXIS_ERROR); + if (epicsMutexLock(pAxis->mutexId) == epicsMutexLockOK) + { + motorParam->setInteger(pAxis->params, motorAxisDirection, forwards); + /* Ensure that the motor record's next status update sees motorAxisDone = False. */ + motorParam->setInteger(pAxis->params, motorAxisDone, 0); + motorParam->callCallback(pAxis->params); + epicsMutexUnlock(pAxis->mutexId); + } + + /* Send a signal to the poller task which will make it do a poll, and switch to the moving poll rate */ + epicsEventSignal(pAxis->pController->pollEventId); + + return (MOTOR_AXIS_OK); +} + + +static int motorAxisVelocityMove(AXIS_HDL pAxis, double min_velocity, double velocity, double acceleration) +{ + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + int ret_status; + + if (pAxis == NULL) + return(MOTOR_AXIS_ERROR); + + sprintf(outputBuff, "SETPARM @%d, 103, %.*f", pAxis->axis, pAxis->maxDigits, acceleration); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + sprintf(outputBuff, "FREERUN @%d %.*f", pAxis->axis, pAxis->maxDigits, velocity); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + + if (epicsMutexLock(pAxis->mutexId) == epicsMutexLockOK) + { + motorParam->setInteger(pAxis->params, motorAxisDirection, (velocity > 0.0 ? 1 : 0)); + /* Ensure that the motor record's next status update sees motorAxisDone = False. */ + motorParam->setInteger(pAxis->params, motorAxisDone, 0); + motorParam->callCallback(pAxis->params); + epicsMutexUnlock(pAxis->mutexId); + } + + /* Send a signal to the poller task which will make it do a poll, and switch to the moving poll rate */ + epicsEventSignal(pAxis->pController->pollEventId); + + return (ret_status); +} + +static int motorAxisProfileMove(AXIS_HDL pAxis, int npoints, double positions[], double times[], int relative, int trigger) +{ + return (MOTOR_AXIS_ERROR); +} + +static int motorAxisTriggerProfile(AXIS_HDL pAxis) +{ + return (MOTOR_AXIS_ERROR); +} + +static int motorAxisStop(AXIS_HDL pAxis, double acceleration) +{ + int ret_status; + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + + if (pAxis == NULL) + return (MOTOR_AXIS_ERROR); + + PRINT(pAxis->logParam, FLOW, "Set 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, "ABORT @%d", pAxis->axis); + ret_status = sendAndReceive(pAxis->pController, outputBuff, inputBuff, sizeof(inputBuff)); + return (ret_status); +} + +static int motorAxisforceCallback(AXIS_HDL pAxis) +{ + if (pAxis == NULL) + return(MOTOR_AXIS_ERROR); + + PRINT(pAxis->logParam, FLOW, "motorAxisforceCallback: request card %d, axis %d status update\n", + pAxis->card, pAxis->axis); + + /* Force a status update. */ + motorParam->forceCallback(pAxis->params); + + /* Send a signal to the poller task which will make it do a status update */ + epicsEventSignal(pAxis->pController->pollEventId); + return(MOTOR_AXIS_OK); +} + + +static void EnsemblePoller(EnsembleController *pController) +{ + /* This is the task that polls the Ensemble */ + double timeout; + AXIS_HDL pAxis; + int status, itera, anyMoving, comStatus, axisStatus; + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + + timeout = pController->idlePollPeriod; + epicsEventSignal(pController->pollEventId); /* Force on poll at startup */ + + while (1) + { + if (timeout != 0.) + status = epicsEventWaitWithTimeout(pController->pollEventId, timeout); + else + status = epicsEventWait(pController->pollEventId); + + anyMoving = 0; + + for (itera = 0; itera < pController->numAxes; itera++) + { + pAxis = &pController->pAxis[itera]; + if (!pAxis->mutexId) + break; + epicsMutexLock(pAxis->mutexId); + sprintf(outputBuff, "AXISSTATUS(@%d)", pAxis->axis); + comStatus = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (comStatus != asynSuccess || strlen(inputBuff) <= 1) + { + PRINT(pAxis->logParam, TERROR, "EnsemblePoller: error reading status=%d\n", comStatus); + motorParam->setInteger(pAxis->params, motorAxisCommError, 1); + epicsMutexUnlock(pAxis->mutexId); + continue; + } + else + { + PARAMS params = pAxis->params; + axisStatus = 0; + if (inputBuff[0] != ASCII_ACK_CHAR) + { + epicsMutexUnlock(pAxis->mutexId); + continue; + } + else + { + motorParam->setInteger(params, motorAxisCommError, 0); + axisStatus = atoi(&inputBuff[1]); + motorParam->setInteger(params, motorAxisDone, axisStatus & IN_MOTION_BIT ? 0 : 1); + if (axisStatus & IN_MOTION_BIT) + anyMoving = 1; + motorParam->setInteger(pAxis->params, motorAxisPowerOn, axisStatus & ENABLED_BIT ? 1 : 0); + motorParam->setInteger(pAxis->params, motorAxisHomeSignal, axisStatus & HOME_MARKER_BIT ? 1 : 0); + if (pAxis->stepSize > 0.0) + motorParam->setInteger(pAxis->params, motorAxisDirection, axisStatus & DIRECTION_BIT ? 1 : 0); + else + motorParam->setInteger(pAxis->params, motorAxisDirection, axisStatus & DIRECTION_BIT ? 0 : 1); + } + pAxis->axisStatus = axisStatus; + } + sprintf(outputBuff, "PFBKPROG(@%d)", pAxis->axis); + comStatus = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (comStatus != asynSuccess) + { + PRINT(pAxis->logParam, TERROR, "EnsemblePoller: error reading position=%d\n", comStatus); + motorParam->setInteger(pAxis->params, motorAxisCommError, 1); + epicsMutexUnlock(pAxis->mutexId); + continue; + } + else + { + double pfdbk; + if (inputBuff[0] != ASCII_ACK_CHAR) + pfdbk = 0; + else + pfdbk = atof(&inputBuff[1]); + pAxis->currentPosition = pfdbk / fabs(pAxis->stepSize); + motorParam->setDouble(pAxis->params, motorAxisPosition, pAxis->currentPosition); + motorParam->setDouble(pAxis->params, motorAxisEncoderPosn, pAxis->currentPosition); + PRINT(pAxis->logParam, IODRIVER, "EnsemblePoller: axis %d axisStatus=%x, position=%f\n", + pAxis->axis, pAxis->axisStatus, pAxis->currentPosition); + } + sprintf(outputBuff, "AXISFAULT(@%d)", pAxis->axis); + comStatus = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (comStatus != asynSuccess) + { + PRINT(pAxis->logParam, TERROR, "EnsemblePoller: error reading axisfault axis=%d error=%d\n", itera, comStatus); + motorParam->setInteger(pAxis->params, motorAxisCommError, 1); + epicsMutexUnlock(pAxis->mutexId); + continue; + } + else + { + if (inputBuff[0] != ASCII_ACK_CHAR) + { + motorParam->setInteger(pAxis->params, motorAxisProblem, 1); + motorParam->setInteger(pAxis->params, motorAxisCommError, 1); + epicsMutexUnlock(pAxis->mutexId); + continue; + } + else + { + axisStatus = atoi(&inputBuff[1]); + if (pAxis->stepSize > 0.0) + { + motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, axisStatus & CW_FAULT_BIT ? 1 : 0); + motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, axisStatus & CCW_FAULT_BIT ? 1 : 0); + } + else + { + motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, axisStatus & CCW_FAULT_BIT ? 1 : 0); + motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, axisStatus & CW_FAULT_BIT ? 1 : 0); + } + } + } + motorParam->callCallback(pAxis->params); + epicsMutexUnlock(pAxis->mutexId); + } /* Next axis */ + + if (anyMoving) + timeout = pController->movingPollPeriod; + else + timeout = pController->idlePollPeriod; + } /* End while */ +} + +static int motorEnsembleLogMsg(void * param, const motorAxisLogMask_t mask, const char *pFormat, ...) +{ + + va_list pvar; + int nchar; + + va_start(pvar, pFormat); + nchar = vfprintf(stdout,pFormat,pvar); + va_end (pvar); + printf("\n"); + return(nchar); +} + +int EnsembleAsynSetup(int num_controllers) /* number of Ensemble controllers in system. */ +{ + + if (num_controllers < 1) + { + printf("EnsembleSetup, num_controllers must be > 0\n"); + return (MOTOR_AXIS_ERROR); + } + numEnsembleControllers = num_controllers; + pEnsembleController = (EnsembleController *)calloc(numEnsembleControllers, sizeof(EnsembleController)); + return (MOTOR_AXIS_OK); +} + +int EnsembleAsynConfig(int card, /* Controller number */ + const char *portName, /* asyn port name of serial or GPIB port */ + int asynAddress, /* asyn subaddress for GPIB */ + int numAxes, /* Number of axes this controller supports */ + 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 */ + +{ + AXIS_HDL pAxis; + int axis; + EnsembleController *pController; + char threadName[20]; + int status; + int digits; + int retry = 0; + char inputBuff[BUFFER_SIZE]; + char outputBuff[BUFFER_SIZE]; + int numAxesFound; + + if (numEnsembleControllers < 1) + { + printf("EnsembleConfig: no Ensemble controllers allocated, call EnsembleSetup first\n"); + return (MOTOR_AXIS_ERROR); + } + if ((card < 0) || (card >= numEnsembleControllers)) + { + printf("EnsembleConfig: card must in range 0 to %d\n", numEnsembleControllers-1); + return (MOTOR_AXIS_ERROR); + } + if ((numAxes < 1) || (numAxes > ENSEMBLE_MAX_AXES)) + { + printf("EnsembleConfig: numAxes must in range 1 to %d\n", ENSEMBLE_MAX_AXES); + return (MOTOR_AXIS_ERROR); + } + + pController = &pEnsembleController[card]; + pController->pAxis = (AXIS_HDL) calloc(numAxes, sizeof(motorAxis)); + pController->numAxes = numAxes; + pController->movingPollPeriod = movingPollPeriod/1000.; + pController->idlePollPeriod = idlePollPeriod/1000.; + + pController->sendReceiveMutex = epicsMutexMustCreate(); + + status = pasynOctetSyncIO->connect(portName, asynAddress, &pController->pasynUser, NULL); + + if (status != asynSuccess) + { + printf("EnsembleAsynConfig: cannot connect to asyn port %s\n", portName); + return (MOTOR_AXIS_ERROR); + } + + /* Set command End-of-string */ + pasynOctetSyncIO->setInputEos(pController->pasynUser, ASCII_EOS_STR, strlen(ASCII_EOS_STR)); + pasynOctetSyncIO->setOutputEos(pController->pasynUser, ASCII_EOS_STR, strlen(ASCII_EOS_STR)); + + retry = 0; + + do + { + /* we only care if we get a response + so we don't need to send a valid command */ + strcpy(outputBuff, "NONE"); + status = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + + retry++; + } while (status != asynSuccess && retry < 3); + + numAxesFound = 0; + + if (status != asynSuccess) + return (MOTOR_AXIS_ERROR); + + /* Get the number of axes */ + for (axis = 0; axis < ENSEMBLE_MAX_AXES && numAxesFound < numAxes; axis++) + { + /* Does this axis actually exist? */ + sprintf(outputBuff, "GETPARM(@%d, 257)", axis); /* AxisName */ + sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + + /* We know the axis exists if we got an ACK response */ + if (inputBuff[0] == ASCII_ACK_CHAR) + { + pAxis = &pController->pAxis[numAxesFound]; + pAxis->pController = pController; + pAxis->card = card; + pAxis->axis = axis; + pAxis->mutexId = epicsMutexMustCreate(); + pAxis->params = motorParam->create(0, MOTOR_AXIS_NUM_PARAMS); + + sprintf(outputBuff, "GETPARM(@%d, 58)", axis); /* CfgFbkPosType */ + sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (inputBuff[0] == ASCII_ACK_CHAR) + { + if (atoi(&inputBuff[1]) > 0) + pAxis->closedLoop = 1; + } + + sprintf(outputBuff, "GETPARM(@%d, 3)", axis); /* PosScaleFactor */ + sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (inputBuff[0] == ASCII_ACK_CHAR) + pAxis->stepSize = 1 / atof(&inputBuff[1]); + else + pAxis->stepSize = 1; + digits = (int) -log(10) * (fabs(pAxis->stepSize)) + 2; + if (digits < 1) + digits = 1; + pAxis->maxDigits = digits; + + sprintf(outputBuff, "GETPARM(@%d, 108)", axis); /* HomeOffset */ + sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (inputBuff[0] == ASCII_ACK_CHAR) + pAxis->homePreset = atof(&inputBuff[1]); + + sprintf(outputBuff, "GETPARM(@%d, 106)", axis); /* HomeDirection */ + sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff)); + if (inputBuff[0] == ASCII_ACK_CHAR) + pAxis->homeDirection = atoi(&inputBuff[1]); + numAxesFound++; + } + } + + pController->pollEventId = epicsEventMustCreate(epicsEventEmpty); + + /* Create the poller thread for this controller */ + epicsSnprintf(threadName, sizeof(threadName), "Ensemble:%d", card); + epicsThreadCreate(threadName, epicsThreadPriorityMedium, epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) EnsemblePoller, (void *) pController); + + return (MOTOR_AXIS_OK); +} + +static asynStatus sendAndReceive(EnsembleController *pController, char *outputBuff, char *inputBuff, int inputSize) +{ + char outputCopy[BUFFER_SIZE]; + int nWriteRequested; + size_t nWrite, nRead; + int eomReason; + asynStatus status; + + strcpy(outputCopy, outputBuff); + if (outputCopy[strlen(outputCopy) - 1] != ASCII_EOS_CHAR) + strcat(outputCopy, ASCII_EOS_STR); + nWriteRequested=strlen(outputCopy); + + /* sendAndReceive is intended only for "fast" read-write operations (such as getting parameter/status values), + * so we don't expect much latency on read/writes */ + epicsMutexLock(pController->sendReceiveMutex); + + status = pasynOctetSyncIO->writeRead(pController->pasynUser, + outputCopy, nWriteRequested, inputBuff, inputSize, TIMEOUT, &nWrite, &nRead, &eomReason); + if (nWrite != nWriteRequested) + status = asynError; + if (status != asynSuccess) + asynPrint(pController->pasynUser, ASYN_TRACE_ERROR, + "drvEnsembleAsyn:sendAndReceive error calling write, output=%s status=%d, error=%s\n", + outputCopy, status, pController->pasynUser->errorMessage); + else + { + /* read until we have an ACK followed by a string (most likely will be the numeric value we're looking for) */ + while (status == asynSuccess && nRead > 1 && inputBuff[0] == ASCII_ACK_CHAR && inputBuff[1] == ASCII_EOS_CHAR) + status = pasynOctetSyncIO->read(pController->pasynUser, inputBuff, inputSize, TIMEOUT, &nRead, &eomReason); + if (status != asynSuccess) + asynPrint(pController->pasynUser, ASYN_TRACE_ERROR, + "drvEnsembleAsyn:sendAndReceive error calling read, status=%d, error=%s\n", + status, pController->pasynUser->errorMessage); + } + epicsMutexUnlock(pController->sendReceiveMutex); + return(status); +} + diff --git a/motorApp/AerotechSrc/drvEnsembleAsyn.h b/motorApp/AerotechSrc/drvEnsembleAsyn.h new file mode 100755 index 00000000..f51493a8 --- /dev/null +++ b/motorApp/AerotechSrc/drvEnsembleAsyn.h @@ -0,0 +1,20 @@ +#ifndef DRV_MOTOR_ENSEMBLE_ASYN_H +#define DRV_MOTOR_ENSEMBLE_ASYN_H + +#ifdef __cplusplus +extern "C" { +#endif + + int EnsembleAsynSetup(int numControllers); /* number of Ensemble controllers in system. */ + + int EnsembleAsynConfig(int card, /* Controller number */ + const char *portName, /* asyn port name of serial or GPIB port */ + int asynAddress, /* asyn subaddress for GPIB */ + int numAxes, /* Number of axes this controller supports */ + 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 */ + +#ifdef __cplusplus +} +#endif +#endif diff --git a/motorApp/AerotechSrc/drvSoloist.cc b/motorApp/AerotechSrc/drvSoloist.cc new file mode 100755 index 00000000..35dcd6fc --- /dev/null +++ b/motorApp/AerotechSrc/drvSoloist.cc @@ -0,0 +1,760 @@ +/* +* FILENAME... drvSoloist.cc +* USAGE... Motor record driver level support for Aerotech Soloist. +* +* Version: 1.1 +* Modified By: cbonnell +* Last Modified: 2009/07/07 03:00:23 PM +*/ + +/* +* Original Author: Hong Fung +* Date: 04/29/09 +* Current Author: Aerotech, Inc. +* +* Experimental Physics and Industrial Control System (EPICS) +* +* Copyright 1991, the Regents of the University of California, +* and the University of Chicago Board of Governors. +* +* This software was produced under U.S. Government contracts: +* (W-7405-ENG-36) at the Los Alamos National Laboratory, +* and (W-31-109-ENG-38) at Argonne National Laboratory. +* +* Initial development by: +* The Controls and Automation Group (AT-8) +* Ground Test Accelerator +* Accelerator Technology Division +* Los Alamos National Laboratory +* +* Co-developed with +* The Controls and Computing Group +* Accelerator Systems Division +* Advanced Photon Source +* Argonne National Laboratory +* +* Modification Log: +* ----------------- +* .01 04-29-09 hcf - initialized from drvEnsemble.cc (Aerotech) +* .02 07-07-09 cjb - merged fixes from Ensemble code (drvEnsemble.cc) in R6-4-4 +*/ + + +#include +#include +#include +#include +#include +#include +#include "motor.h" +#include "drvSoloist.h" +#include "asynOctetSyncIO.h" +#include "epicsExport.h" + +/* Status byte bits */ +#define ENABLED_BIT 0x00000001 +#define IN_POSITION_BIT 0x00000004 +#define IN_MOTION_BIT 0x00000008 +#define DIRECTION_BIT 0x00000200 +#define HOME_LIMIT_BIT 0x01000000 +#define HOME_MARKER_BIT 0x02000000 + +/* Fault status bits */ +#define CW_FAULT_BIT 0x004 +#define CCW_FAULT_BIT 0x008 + +// This can really be any number, because there isn't any theoretical +// restriction on the number of Soloist that can be on a network. +// However, a relatively low number was picked for practical purposes +#define Soloist_NUM_CARDS 10 + +// Due to the way some of the commands are implemented in the Soloist, +// such as HOME, the command response is not returned until the command +// has actually finished executing. This requires a fairly large timeout +// value. However, even a large value will not guarantee that the response will +// be sent back in the time alloted. +#define TIMEOUT 20.0 /* Command timeout in sec. */ + +/*----------------debugging-----------------*/ +#ifdef __GNUG__ +#ifdef DEBUG +#define Debug(l, f, args...) { if (l<=drvSoloistdebug) printf(f,## args); } +#else +#define Debug(l, f, args...) +#endif +#else +#define Debug() +#endif +volatile int drvSoloistdebug = 0; +extern "C" {epicsExportAddress(int, drvSoloistdebug);} + +/* --- Local data. --- */ +int Soloist_num_cards = 0; +int Soloist_num_cmds = 0; + +/* Local data required for every driver; see "motordrvComCode.h" */ +#include "motordrvComCode.h" + + +/*----------------functions-----------------*/ +static int recv_mess(int, char *, int); +static RTN_STATUS send_mess(int, char const *, char *name); +static int set_status(int card, int signal); +static long report(int level); +static long init(); +static int motor_init(); +static void query_done(int, int, struct mess_node *); + +/*----------------functions-----------------*/ + +struct driver_table Soloist_access = +{ + motor_init, + motor_send, + motor_free, + motor_card_info, + motor_axis_info, + &mess_queue, + &queue_lock, + &free_list, + &freelist_lock, + &motor_sem, + &motor_state, + &total_cards, + &any_motor_in_motion, + send_mess, + recv_mess, + set_status, + query_done, + NULL, + &initialized, + NULL +}; + +struct +{ + long number; + long (*report) (int); + long (*init) (void); +} drvSoloist = {2, report, init}; + +extern "C" {epicsExportAddress(drvet, drvSoloist);} + +static struct thread_args targs = {SCAN_RATE, &Soloist_access, 0.0}; + +/********************************************************* +* Print out driver status report +*********************************************************/ +static long report(int level) +{ + int card; + + if (Soloist_num_cards <=0) + printf(" No Soloist controllers configured.\n"); + else + { + for (card = 0; card < Soloist_num_cards; card++) + { + struct controller *brdptr = motor_state[card]; + + if (brdptr == NULL) + printf(" Soloist controller %d connection failed.\n", card); + else + { + struct Soloistcontroller *cntrl; + + cntrl = (struct Soloistcontroller *) brdptr->DevicePrivate; + printf(" Soloist controller %d, port=%s, address=%d, id: %s \n", + card, cntrl->asyn_port, cntrl->asyn_address, + brdptr->ident); + } + } + } + return(OK); +} + + +static long init() +{ + /* + * We cannot call motor_init() here, because that function can do GPIB I/O, + * and hence requires that the drvGPIB have already been initialized. + * That cannot be guaranteed, so we need to call motor_init from device + * support + */ + /* Check for setup */ + if (Soloist_num_cards <= 0) + { + Debug(1, "init(): Soloist driver disabled. SoloistSetup() missing from startup script.\n"); + } + return((long) 0); +} + + +static void query_done(int card, int axis, struct mess_node *nodeptr) +{ +} + + +/************************************************************** +* Parse status and position strings for a card and signal +* set_status() +************************************************************/ + +static int set_status(int card, int signal) +{ + struct Soloistcontroller *cntrl; + struct mess_node *nodeptr; + register struct mess_info *motor_info; + double motorData, pfbk; + bool plusdir, ls_active = false; + msta_field status; + int rtn_state, comm_status, axis_status; + char buff[BUFF_SIZE]; + + cntrl = (struct Soloistcontroller *) motor_state[card]->DevicePrivate; + motor_info = &(motor_state[card]->motor_info[signal]); + status.All = motor_info->status.All; + + // get the axis status + sprintf(buff, "AXISSTATUS()"); + send_mess(card, buff, (char) NULL); + comm_status = recv_mess(card, buff, 1); + if (comm_status > 0 && buff[0] == ASCII_ACK_CHAR) + { + cntrl->status = NORMAL; + status.Bits.CNTRL_COMM_ERR = 0; + } + else if (comm_status <= 0) + { + if (cntrl->status == NORMAL) + { + cntrl->status = RETRY; + rtn_state = OK; + } + else + { + cntrl->status = COMM_ERR; + status.Bits.CNTRL_COMM_ERR = 1; + status.Bits.RA_PROBLEM = 1; + rtn_state = 1; + } + goto exit; + } + else if (buff[0] != ASCII_ACK_CHAR) + { + cntrl->status = COMM_ERR; + status.Bits.CNTRL_COMM_ERR = 1; + status.Bits.RA_PROBLEM = 1; + rtn_state = 1; + goto exit; + } + + cntrl->status = NORMAL; + status.Bits.CNTRL_COMM_ERR = 0; + + // convert to an integer + axis_status = atoi(&buff[1]); + + nodeptr = motor_info->motor_motion; + + status.Bits.EA_SLIP = 0; + status.Bits.EA_SLIP_STALL = 0; + + // fill in the status + status.Bits.RA_DIRECTION = axis_status & DIRECTION_BIT ? 1 : 0; + status.Bits.RA_DONE = axis_status & IN_POSITION_BIT ? 1 : 0; + status.Bits.RA_HOME = axis_status & HOME_LIMIT_BIT ? 1 : 0; + status.Bits.EA_POSITION = axis_status & ENABLED_BIT ? 1 : 0; + status.Bits.EA_HOME = axis_status & HOME_MARKER_BIT ? 1 : 0; + status.Bits.RA_MOVING = axis_status & IN_MOTION_BIT ? 1 : 0; + + /* get the axis fault status */ + sprintf(buff, "AXISFAULT()"); + send_mess(card, buff, (char) NULL); + comm_status = recv_mess(card, buff, 1); + axis_status = atoi(&buff[1]); + status.Bits.RA_PLUS_LS = axis_status & CW_FAULT_BIT ? 1 : 0; + status.Bits.RA_MINUS_LS = axis_status & CCW_FAULT_BIT ? 1 : 0; + + plusdir = status.Bits.RA_DIRECTION ? true : false; + if ((status.Bits.RA_PLUS_LS && plusdir) || (status.Bits.RA_MINUS_LS && !plusdir)) + { + ls_active = true; + } + + // get the position + sprintf(buff, "PFBKPROG()"); + send_mess(card, buff, (char) NULL); + recv_mess(card, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + // convert to an integer + pfbk = atof(&buff[1]); + } + else + { + pfbk = 0; + } + + // fill in the position + motorData = pfbk / cntrl->drive_resolution[signal]; + + if (motorData == motor_info->position) + { + // only increment the counter if the motor is moving + if (nodeptr != 0) + { + motor_info->no_motion_count++; + } + } + else + { + motor_info->position = NINT(motorData); + if (motor_info->encoder_present == YES) + { + motor_info->encoder_position = NINT(motorData); + } + else + { + motor_info->encoder_position = 0; + } + + motor_info->no_motion_count = 0; + } + + // velocity is not used, so don't bother doing a command down + // to the controller to get it + motor_info->velocity = 0; + + // do this "last", so that we know no errors occurred + status.Bits.RA_PROBLEM = 0; + + rtn_state = (!motor_info->no_motion_count || ls_active || + status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; + + // test for post-move string + if ((status.Bits.RA_DONE || ls_active) && nodeptr != 0 && nodeptr->postmsgptr != 0) + { + strcpy(buff, nodeptr->postmsgptr); + send_mess(card, buff, (char) NULL); + nodeptr->postmsgptr = NULL; + } + +exit: + motor_info->status.All = status.All; + return(rtn_state); +} + + +/*****************************************************/ +/* send a message to the Soloist board */ +/* send_mess() */ +/*****************************************************/ +static RTN_STATUS send_mess(int card, char const *com, char *name) +{ + struct Soloistcontroller *cntrl; + int size; + size_t nwrite; + char *eos_tok, com_cpy[BUFF_SIZE]; + asynStatus status; + + size = strlen(com); + + if (size > MAX_MSG_SIZE) + { + errlogMessage("drvSoloist.c:send_mess(); message size violation.\n"); + return(ERROR); + } + else if (size == 0) + { + return(OK); + } + + if (!motor_state[card]) + { + errlogMessage("drvSoloist.c:send_mess() - invalid card #%d\n"); + return(ERROR); + } + + if (name != NULL) + { + errlogMessage("drvSoloist.c:send_mess() - invalid argument = %s\n"); + return(ERROR); + } + + Debug(2, "send_mess(): message = %s\n", com); + + // We need to track the number of individual Soloist commands that are + // being sent, so that we can read back the same number of responses. + // This is necessary, because the Soloist will send individual responses + // terminated by the EOS char for each command sent, even if those commands + // were all sent as part of one motor record command, such as LOAD_POS + + Soloist_num_cmds = 0; + strcpy(com_cpy, com); + eos_tok = strtok(com_cpy, ASCII_EOS_STR); + while (eos_tok != NULL) + { + Soloist_num_cmds++; + eos_tok = strtok(NULL, ASCII_EOS_STR); + } + + cntrl = (struct Soloistcontroller *) motor_state[card]->DevicePrivate; + + status = pasynOctetSyncIO->write(cntrl->pasynUser, com, size, TIMEOUT, &nwrite); + + if (status != asynSuccess || nwrite <= 0) + { + errlogMessage(cntrl->pasynUser->errorMessage); + return(ERROR); + } + + return(OK); +} + + +/* +* FUNCTION... recv_mess(int card, char *com, int flag) +* +* INPUT ARGUMENTS... +* card - controller card # (0,1,...). +* *com - caller's response buffer. +* flag | FLUSH = flush controller's output buffer; set timeout = 0. +* | !FLUSH = retrieve response into caller's buffer; set timeout. +* +* LOGIC... +* IF controller card does not exist. +* ERROR RETURN. +* ENDIF +* NORMAL RETURN. +*/ + +static int recv_mess(int card, char *com, int flag) +{ + struct Soloistcontroller *cntrl; + double timeout = 0; + size_t nread = 0; + asynStatus status; + int eomReason, i; + char buff[BUFF_SIZE]; + + if (!motor_state[card]) + { + return(ERROR); + } + + cntrl = (struct Soloistcontroller *) motor_state[card]->DevicePrivate; + + timeout = TIMEOUT; + com[0] = '\0'; + for (i = 0; i < Soloist_num_cmds; i++) + { + status = pasynOctetSyncIO->read(cntrl->pasynUser, buff, BUFF_SIZE, timeout, &nread, &eomReason); + + if (status == asynSuccess && nread > 0) + { + strcat(com, buff); + } + else + { + com[0] = '\0'; + nread = 0; + break; + } + } + + Debug(2, "recv_mess(): message = \"%s\"\n", com); + return(nread); +} + + +/*****************************************************/ +/* Setup system configuration */ +/* SoloistSetup() */ +/*****************************************************/ +RTN_STATUS SoloistSetup(int num_cards, /* maximum number of controllers in system. */ + int scan_rate) /* polling rate - 1/60 sec units. */ +{ + int i; + + if (num_cards < 1 || num_cards > Soloist_NUM_CARDS) + { + Soloist_num_cards = Soloist_NUM_CARDS; + } + else + { + Soloist_num_cards = num_cards; + } + + // Set motor polling task rate + if (scan_rate >= 1 && scan_rate <= 60) + { + targs.motor_scan_rate = scan_rate; + } + else + { + targs.motor_scan_rate = SCAN_RATE; + } + + /* + * Allocate space for motor_state structures. Note this must be done + * before SoloistConfig is called, so it cannot be done in motor_init() + * This means that we must allocate space for a card without knowing + * if it really exists, which is not a serious problem + */ + motor_state = (struct controller **) malloc(Soloist_num_cards * sizeof(struct controller *)); + + for (i = 0; i < Soloist_num_cards; i++) + { + motor_state[i] = (struct controller *) NULL; + } + + return(OK); +} + + +/*****************************************************/ +/* Configure a controller */ +/* SoloistConfig() */ +/*****************************************************/ +RTN_STATUS SoloistConfig(int card, /* card being configured */ + const char *name, /* asyn port name */ + int addr) /* asyn address (GPIB) */ +{ + struct Soloistcontroller *cntrl; + + if (card < 0 || card >= Soloist_num_cards) + { + return(ERROR); + } + + motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); + motor_state[card]->DevicePrivate = malloc(sizeof(struct Soloistcontroller)); + cntrl = (struct Soloistcontroller *) motor_state[card]->DevicePrivate; + + strcpy(cntrl->asyn_port, name); + cntrl->asyn_address = addr; + + return(OK); +} + + + +/*****************************************************/ +/* initialize all software and hardware */ +/* This is called from the initialization routine in */ +/* device support. */ +/* motor_init() */ +/*****************************************************/ +static int motor_init() +{ + struct controller *brdptr; + struct Soloistcontroller *cntrl; + int card_index, motor_index, status, digits; + char buff[BUFF_SIZE]; + asynStatus success_rtn; + + // Indicates that the driver is initialized + initialized = true; + + if (Soloist_num_cards <= 0) + { + return(ERROR); + } + + for (card_index = 0; card_index < Soloist_num_cards; card_index++) + { + if (!motor_state[card_index]) + { + continue; + } + + brdptr = motor_state[card_index]; + brdptr->cmnd_response = true; + total_cards = card_index + 1; + cntrl = (struct Soloistcontroller *)brdptr->DevicePrivate; + + // Initialize communications channel + success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, cntrl->asyn_address, &cntrl->pasynUser, NULL); + + if (success_rtn == asynSuccess) + { + int retry = 0; + + // Send a message to the baord, see if it exists + // flush any junk at input port - should not be any data available + pasynOctetSyncIO->flush(cntrl->pasynUser); + + do + { + // we only care if we get a response + // so we don't need to send a valid command + strcpy(buff, "NONE"); + send_mess(card_index, buff, (char) NULL); + status = recv_mess(card_index, buff, 1); + + retry++; + } while (!status && retry < 3); + + if (status > 0) + { + brdptr->localaddr = (char *) NULL; + brdptr->motor_in_motion = 0; + // Read controller ID string + strcpy(buff, "GETPARM(265)"); //UserString1 + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + strcpy(brdptr->ident, &buff[1]); + } + else + { + sprintf(brdptr->ident, "Soloist%d", card_index); + } + + // Get the number of axes + brdptr->total_axis = 0; + for (motor_index = 0; motor_index < 1; motor_index++) // one motor (axis) per card + { + // Does this axis actually exist? + sprintf(buff, "GETPARM(257)"); //AxisName + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + + // We know the axis exists if we got an ACK response + if (buff[0] == ASCII_ACK_CHAR) + { + cntrl->axes[motor_index] = 1; + brdptr->total_axis++; + } + } + + for (motor_index = 0; motor_index < 1; motor_index++) // one motor (axis) per card + { + if (cntrl->axes[motor_index]) + { + struct mess_info *motor_info = &brdptr->motor_info[motor_index]; + + motor_info->status.All = 0; + motor_info->no_motion_count = 0; + motor_info->encoder_position = 0; + motor_info->position = 0; + brdptr->motor_info[motor_index].motor_motion = NULL; + + // Determine if encoder present based on open/closed loop mode. + sprintf(buff, "GETPARM(58)"); //CfgFbkPosType + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + if (atoi(&buff[1]) > 0) + { + motor_info->encoder_present = YES; + motor_info->status.Bits.EA_PRESENT = 1; + } + } + + // Determine if gains are supported based on the motor type. + sprintf(buff, "GETPARM(33)"); //CfgMotType + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + if (atoi(&buff[1]) != 3) + { + motor_info->pid_present = YES; + motor_info->status.Bits.GAIN_SUPPORT = 1; + } + } + + // Stop all motors + sprintf(buff, "ABORT"); + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + + // Determine drive resolution + sprintf(buff, "GETPARM(3)"); //PosScaleFactor + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + cntrl->drive_resolution[motor_index] = 1 / fabs(atof(&buff[1])); + } + else + { + cntrl->drive_resolution[motor_index] = 1; + } + + digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2; + if (digits < 1) + { + digits = 1; + } + cntrl->res_decpts[motor_index] = digits; + + // Save home preset position + sprintf(buff, "GETPARM(108)"); //HomeOffset + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + cntrl->home_preset[motor_index] = atof(&buff[1]); + } + + // Determine low limit + sprintf(buff, "GETPARM(47)"); //ThresholdSoftCCW + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + motor_info->low_limit = atof(&buff[1]); + } + + // Determine high limit + sprintf(buff, "GETPARM(48)"); //ThresholdSoftCW + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + motor_info->high_limit = atof(&buff[1]); + } + + // Save the HomeDirection parameter + sprintf(buff, "GETPARM(106)"); //HomeDirection + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); + if (buff[0] == ASCII_ACK_CHAR) + { + cntrl->home_dparam[motor_index] = atoi(&buff[1]); + } + + // Read status of each motor + set_status(card_index, motor_index); + } + } + } + else + { + motor_state[card_index] = (struct controller *) NULL; + } + } + } + + any_motor_in_motion = 0; + + mess_queue.head = (struct mess_node *) NULL; + mess_queue.tail = (struct mess_node *) NULL; + + free_list.head = (struct mess_node *) NULL; + free_list.tail = (struct mess_node *) NULL; + + epicsThreadCreate((char *) "Soloist_motor", + epicsThreadPriorityMedium, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motor_task, (void *) &targs); + + return(OK); +} + diff --git a/motorApp/AerotechSrc/drvSoloist.h b/motorApp/AerotechSrc/drvSoloist.h new file mode 100755 index 00000000..a564b0be --- /dev/null +++ b/motorApp/AerotechSrc/drvSoloist.h @@ -0,0 +1,81 @@ +/* +FILENAME... drvSoloist.h +USAGE... This file contains Aerotech Soloist driver "include" information. + +Version: 1.0 +Modified By: cbonnell +Last Modified: 2009/07/07 03:06:56 PM +*/ + +/* + * Original Author: Mark Rivers + * Date: 10/16/97 + * Current Author: Aerotech, Inc. + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the Regents of the University of California, + * and the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contracts: + * (W-7405-ENG-36) at the Los Alamos National Laboratory, + * and (W-31-109-ENG-38) at Argonne National Laboratory. + * + * Initial development by: + * The Controls and Automation Group (AT-8) + * Ground Test Accelerator + * Accelerator Technology Division + * Los Alamos National Laboratory + * + * Co-developed with + * The Controls and Computing Group + * Accelerator Systems Division + * Advanced Photon Source + * Argonne National Laboratory + * + * Modification Log: + * ----------------- + * .01 04-29-09 hcf initialized from drvEnsemble.h (Aerotech) + * .02 07-07-09 cjb merged fixes from Ensemble code (drvEnsemble.h) in R6-4-4 + */ + +#ifndef INCdrvSoloisth +#define INCdrvSploisth 1 + +#include "motor.h" +#include "motordrvCom.h" +#include "asynDriver.h" +#include "asynOctetSyncIO.h" + +// The following should be defined to have the same value as +// the Soloist parameters specified +#define ASCII_EOS_CHAR '\n' // AsciiCmdEOSChar +#define ASCII_EOS_STR "\n" +#define ASCII_ACK_CHAR '%' // AsciiCmdAckChar +#define ASCII_NAK_CHAR '!' // AsciiCmdNakChar +#define ASCII_FAULT_CHAR '#' // AsciiCmdFaultChar + +#define BUFF_SIZE 100 /* Maximum length of string to/from Soloist */ + + +/* Soloist specific data is stored in this structure. */ +struct Soloistcontroller +{ + asynUser *pasynUser; /* For RS-232/Ethernet */ + int asyn_address; /* Use for GPIB or other address with asyn */ + char asyn_port[80]; /* asyn port name */ + int axes[MAX_AXIS]; + double drive_resolution[MAX_AXIS]; + int res_decpts[MAX_AXIS]; /* Drive resolution significant dec. pts. */ + double home_preset[MAX_AXIS]; /* Controller's home preset position (XF command). */ + epicsUInt32 home_dparam[MAX_AXIS]; /* Controller's HomeDirection parameter. */ + CommStatus status; /* Controller communication status. */ +}; + +/* Function prototypes. */ +extern RTN_STATUS SoloistSetup(int, int); +extern RTN_STATUS SoloistConfig(int, const char *, int); + + +#endif /* INCdrvSoloisth */ +