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

Added Soloist and 'asyn motor' Ensemble version.

Browse Source
This commit is contained in:
Ron Sluiter
2010-03-02 21:07:00 +00:00
parent b5192084c3
commit 2e6290ceaa
11 changed files with 2012 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/AerotechSrc/AerotechRegister.cc
+56 -5
View File
@@ -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);
motorApp/AerotechSrc/AerotechRegister.h
+9 -6
View File
@@ -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);
motorApp/AerotechSrc/Makefile
+3 -1
View File
@@ -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)
motorApp/AerotechSrc/README
+12
View File
@@ -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.
motorApp/AerotechSrc/devAerotech.dbd
+8
View File
@@ -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)
motorApp/AerotechSrc/devEnsemble.cc
+7 -25
View File
@@ -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:
motorApp/AerotechSrc/drvEnsemble.cc
+133 -142
View File
@@ -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;
motorApp/AerotechSrc/drvEnsembleAsyn.c
Executable
+923
View File
@@ -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 <stddef.h>
#include "epicsThread.h"
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#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; i<numEnsembleControllers; i++)
{
if (level)
{
printf(" moving poll period: %f\n", pEnsembleController[i].movingPollPeriod);
printf(" idle poll period: %f\n", pEnsembleController[i].idlePollPeriod);
}
for (j=0; j<pEnsembleController[i].numAxes; j++)
motorAxisReportAxis(&pEnsembleController[i].pAxis[j], level);
}
}
static int motorAxisInit(void)
{
int controller, axis;
for (controller = 0; controller < numEnsembleControllers; controller++)
{
AXIS_HDL pAxis;
for (axis = 0; axis < pEnsembleController[controller].numAxes; axis++)
{
pAxis = &pEnsembleController[controller].pAxis[axis];
if (!pAxis->mutexId)
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);
}
motorApp/AerotechSrc/drvEnsembleAsyn.h
Executable
+20
View File
@@ -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
motorApp/AerotechSrc/drvSoloist.cc
Executable
+760
View File
@@ -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 <string.h>
#include <math.h>
#include <stdio.h>
#include <epicsThread.h>
#include <epicsString.h>
#include <drvSup.h>
#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);
}
motorApp/AerotechSrc/drvSoloist.h
Executable
+81
View File
@@ -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 */