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

Start

Browse Source
This commit is contained in:
Ron Sluiter
2007-05-08 14:45:17 +00:00
parent d74e91350e
commit cb10b6ffb4
6 changed files with 1181 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/SmartMotorSrc/Makefile
+19
View File
@@ -0,0 +1,19 @@
# Makefile
TOP = ../..
include $(TOP)/configure/CONFIG
# The following are used for debugging messages.
#!USR_CXXFLAGS += -DDEBUG
DBD += devSmartMotorMotor.dbd
LIBRARY_IOC = SmartMotor
# Intelligent Motion Systems driver support.
SRCS += devSmartMotor.cc drvSmartMotor.cc
SmartMotor_LIBS += motor asyn
SmartMotor_LIBS += $(EPICS_BASE_IOC_LIBS)
include $(TOP)/configure/RULES
motorApp/SmartMotorSrc/README
+19
View File
@@ -0,0 +1,19 @@
The Animatics SmartMotor EPICS device driver supports both single
and daisy chain serial connections.
For daisy chain applications, the following program must be
down loaded to each motor in the chain. Set "#" below to
the address of each consecutive motor; starting with 1.
SADDR#
ECHO
LIMD <- Omit from firmware 4.77 and above.
LIMH
END
Design Restriction and Limitations
==================================
- This device driver only supports SmartMotor firmware versions 4.15
and above.
motorApp/SmartMotorSrc/devSmartMotor.cc
+351
View File
@@ -0,0 +1,351 @@
/*
FILENAME... devSmartMotor.cc
USAGE... Motor record driver level support for Animatics Corporation SmartMotors.
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2007-05-08 14:45:16 $
*/
/*
* Original Author: Shifu Xu
* Date: 03/21/03
*
* 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
*
* NOTES
* -----
* - the velocity and acceleration conversion factors are based on the default
* PID update rate of 4,068.969 updates per second.
*
* Modification Log:
* -----------------
* .01 02/20/07 ses copied from drvSmartMotorPL.c
* .02 04/18/07 rls fix velocity and acceleration conversion factors.
*/
#include <string.h>
#include "motorRecord.h"
#include "motor.h"
#include "motordevCom.h"
#include "drvSmartMotor.h"
#include "epicsExport.h"
/*
* SmartMotor velocity command (V) units; scaled encoder counts / PID sample.
* VCONFAC converts encoder counts / sec. to scaled encoder counts / PID sample.
* VCONFAC = (65,536 s-e-cts / 1 e-ct) * (1s / 4068.969 PID samples)
*/
#define VCONFAC 16.1063
/*
* SmartMotor acceleration command (A) units; scaled encoder counts / PID sample^2.
* ACONFAC converts encoder counts / sec.^2 to scaled encoder counts / PID sample^2.
* ACONFAC = (65,536 s-e-cts / 1 e-ct) * (1s / 4068.969^2 PID samples)
*/
#define ACONFAC 3.958322E-3
extern struct driver_table SmartMotor_access;
/* ----------------Create the dsets for devSmartMotor----------------- */
static struct driver_table *drvtabptr;
static long SmartMotor_init(void *);
static long SmartMotor_init_record(void *);
static long SmartMotor_start_trans(struct motorRecord *);
static RTN_STATUS SmartMotor_build_trans(motor_cmnd, double *, struct motorRecord *);
static RTN_STATUS SmartMotor_end_trans(struct motorRecord *);
struct motor_dset devSmartMotor =
{
{8, NULL, (DEVSUPFUN) SmartMotor_init, (DEVSUPFUN) SmartMotor_init_record, NULL},
motor_update_values,
SmartMotor_start_trans,
SmartMotor_build_trans,
SmartMotor_end_trans
};
extern "C"
{
epicsExportAddress(dset, devSmartMotor);
}
/* --------------------------- program data --------------------- */
/* This table is used to define the command types */
/* WARNING! this must match "motor_cmnd" in motor.h */
static msg_types SmartMotor_table[] = {
MOTION, /* MOVE_ABS */
MOTION, /* MOVE_REL */
MOTION, /* HOME_FOR */
MOTION, /* HOME_REV */
IMMEDIATE, /* LOAD_POS */
IMMEDIATE, /* SET_VEL_BASE */
IMMEDIATE, /* SET_VELOCITY */
IMMEDIATE, /* SET_ACCEL */
IMMEDIATE, /* GO */
IMMEDIATE, /* SET_ENC_RATIO */
INFO, /* GET_INFO */
MOVE_TERM, /* STOP_AXIS */
VELOCITY, /* JOG */
IMMEDIATE, /* SET_PGAIN */
IMMEDIATE, /* SET_IGAIN */
IMMEDIATE, /* SET_DGAIN */
IMMEDIATE, /* ENABLE_TORQUE */
IMMEDIATE, /* DISABL_TORQUE */
IMMEDIATE, /* PRIMITIVE */
IMMEDIATE, /* SET_HIGH_LIMIT */
IMMEDIATE, /* SET_LOW_LIMIT */
VELOCITY /* JOG_VELOCITY */
};
static struct board_stat **SmartMotor_cards;
/* --------------------------- program data --------------------- */
/* initialize device support for SmartMotor stepper motor */
static long SmartMotor_init(void *arg)
{
long rtnval;
int after = (int) arg;
if (after == 0)
{
drvtabptr = &SmartMotor_access;
(drvtabptr->init) ();
}
rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &SmartMotor_cards);
return(rtnval);
}
/* initialize a record instance */
static long SmartMotor_init_record(void *arg)
{
struct motorRecord *mr = (struct motorRecord *) arg;
return(motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, drvtabptr, SmartMotor_cards));
}
/* start building a transaction */
static long SmartMotor_start_trans(struct motorRecord * mr)
{
return(OK);
}
/* end building a transaction */
static RTN_STATUS SmartMotor_end_trans(struct motorRecord * mr)
{
return(OK);
}
/* add a part to the transaction */
static RTN_STATUS SmartMotor_build_trans(motor_cmnd command, double *parms, struct motorRecord * mr)
{
struct motor_trans *trans = (struct motor_trans *) mr->dpvt;
struct mess_node *motor_call;
struct controller *brdptr;
struct SmartMotorcontroller *cntrl;
char buff[110];
int card, intval, cvel, cacc;
unsigned int size;
double dval;
RTN_STATUS rtnval;
bool send;
msta_field msta;
static bool invalid_accmsg_latch = false;
send = true; /* Default to send motor command. */
rtnval = OK;
buff[0] = '\0';
/* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */
dval = (parms == NULL) ? 0.0 : *parms;
intval = NINT(dval);
cvel = NINT(dval * VCONFAC);
cacc = NINT(dval * ACONFAC);
msta.All = mr->msta;
motor_start_trans_com(mr, SmartMotor_cards);
motor_call = &(trans->motor_call);
card = motor_call->card;
brdptr = (*trans->tabptr->card_array)[card];
if (brdptr == NULL)
return(rtnval = ERROR);
cntrl = (struct SmartMotorcontroller *) brdptr->DevicePrivate;
if (SmartMotor_table[command] > motor_call->type)
motor_call->type = SmartMotor_table[command];
if (trans->state != BUILD_STATE)
return(rtnval = ERROR);
if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0)
{
strcat(motor_call->message, " ");
strcat(motor_call->message, mr->init);
}
switch (command)
{
case MOVE_ABS:
sprintf(buff, "P=%d", intval);
break;
case MOVE_REL:
sprintf(buff, "D=%d", intval);
break;
case HOME_FOR:
/* Animatics SmartMotors do not use home positions */
send = false;
rtnval = ERROR;
break;
case HOME_REV:
/* Animatics SmartMotors do not use home positions */
send = false;
rtnval = ERROR;
break;
case LOAD_POS:
sprintf(buff, "O=%d", intval);
break;
case SET_VEL_BASE:
/* Animatics SmartMotors do not support stepper motors. */
send = false;
rtnval = ERROR;
break;
case SET_VELOCITY:
sprintf(buff, "V=%d", cvel);
break;
case SET_ACCEL:
if (cacc <= 1)
{
cacc = 2;
if (invalid_accmsg_latch == false)
{
invalid_accmsg_latch = true; /* Ouput msg. one time. */
errPrintf(-1, __FILE__, __LINE__,
"Overriding invalid acceleration; A < 2.\n");
}
}
sprintf(buff, "A=%d", cacc);
break;
case GO:
sprintf(buff, "G");
break;
case PRIMITIVE:
/* Animatics SmartMotors do not use 'primatives' */
send = false;
rtnval = ERROR;
break;
case GET_INFO:
break;
case STOP_AXIS:
sprintf(buff, "S");
break;
case JOG_VELOCITY:
send = false;
rtnval = ERROR;
break;
case JOG:
sprintf(buff, "MV\rV=%d\rG", cvel);
break;
case SET_PGAIN:
send = false;
rtnval = ERROR;
break;
case SET_IGAIN:
send = false;
rtnval = ERROR;
break;
case SET_DGAIN:
send = false;
rtnval = ERROR;
break;
case ENABLE_TORQUE:
sprintf(buff, "MP\ra=@P\rP=a\rG");
break;
case DISABL_TORQUE:
sprintf(buff, "OFF");
break;
case SET_HIGH_LIMIT:
send = false;
rtnval = ERROR;
break;
case SET_LOW_LIMIT:
send = false;
rtnval = ERROR;
break;
case SET_ENC_RATIO:
trans->state = IDLE_STATE; /* No command sent to the controller. */
send = false;
break;
default:
send = false;
rtnval = ERROR;
}
size = strlen(buff);
if (send == false)
return(rtnval);
else if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE)
errlogMessage("SmartMotor_build_trans(): buffer overflow.\n");
else
{
strcat(motor_call->message, buff);
motor_end_trans_com(mr, drvtabptr);
}
return(rtnval);
}
motorApp/SmartMotorSrc/devSmartMotorMotor.dbd
+6
View File
@@ -0,0 +1,6 @@
# Smart Motor driver support.
device(motor,VME_IO,devSmartMotor, "SmartMotor")
driver(drvSmartMotor)
registrar(SmartMotormotorRegister)
variable(drvSmartMotordebug)
motorApp/SmartMotorSrc/drvSmartMotor.cc
+719
View File
@@ -0,0 +1,719 @@
/*
FILENAME... drvSmartMotor.cc
USAGE... Motor record driver level support for Animatics Corporation SmartMotors.
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2007-05-08 14:45:16 $
*/
/*
* Original Author: Shifu Xu
* Date: 03/21/03
*
* 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
*
* NOTES
* -----
* - This device driver only supports SmartMotor firmware versions 4.15
* and above.
*
* Modification Log:
* -----------------
* .01 02/20/07 ses copied from drvSmartMotorPL.c
*/
#include <string.h>
#include <epicsThread.h>
#include <drvSup.h>
#include <iocsh.h>
#include "motor.h"
#include "motorRecord.h"
#include "drvSmartMotor.h"
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#include "epicsTime.h"
#define SmartMotor_NUM_CARDS 7
#define BUFF_SIZE 20 /* Maximum length of string to/from SmartMotor */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvSmartMotordebug) printf(f, ## args);}
#else
#define Debug(l, f, args...)
#endif
#else
#define Debug()
#endif
volatile int drvSmartMotordebug = 0;
extern "C"
{
epicsExportAddress(int, drvSmartMotordebug);
}
/* --- Local data. --- */
int SmartMotor_num_cards = 0;
static char *SmartMotor_addr[] = {"129", "130", "131", "132", "133", "134",
"135", "136", "137", "138", "139", "140",
"141", "142", "143", "144", "145", "146",
"147", "148", "149", "150", "151", "152",
"153", "154", "155", "156", "157", "158",
"159", "160"};
/* 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 *);
static int set_status(int, int);
static long report(int);
static long init();
static int motor_init();
static void query_done(int, int, struct mess_node *);
/*----------------functions-----------------*/
struct driver_table SmartMotor_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,
SmartMotor_addr
};
struct
{
long number;
long (*report) (int);
long (*init) (void);
} drvSmartMotor =
{
2, report, init
};
extern "C"
{
epicsExportAddress(drvet, drvSmartMotor);
}
static struct thread_args targs = {SCAN_RATE, &SmartMotor_access, 0.0};
/*********************************************************
* Print out driver status report
*********************************************************/
static long report(int level)
{
int card;
if (SmartMotor_num_cards <= 0)
printf(" No SmartMotor controllers configured.\n");
else
{
for (card = 0; card < SmartMotor_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" SmartMotor controller %d connection failed.\n", card);
else
{
struct SmartMotorcontroller *cntrl;
cntrl = (struct SmartMotorcontroller *) brdptr->DevicePrivate;
printf(" SmartMotorSM controller #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, 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 (SmartMotor_num_cards <= 0)
{
Debug(1, "init(): SmartMotor driver disabled. SmartMotorSetup() missing from startup script.\n");
}
return((long) 0);
}
static void query_done(int card, int axis, struct mess_node * nodeptr)
{
}
/*****************************************************************************
*
* FUNCTION NAME: set_status
*
* LOGIC:
* Initialize.
* Send "Moving Status" query.
* Read response.
* IF normal response to query.
* Set communication status to NORMAL.
* ELSE
* IF communication status is NORMAL.
* Set communication status to RETRY.
* NORMAL EXIT.
* ELSE
* Set communication status error.
* ERROR EXIT.
* ENDIF
* ENDIF
*
* IF "Moving Status" indicates any motion (i.e. status != 0).
* Clear "Done Moving" status bit.
* ELSE
* Set "Done Moving" status bit.
* ENDIF
*
*****************************************************************************/
static int set_status(int card, int signal)
{
struct SmartMotorcontroller *cntrl;
struct mess_node *nodeptr;
register struct mess_info *motor_info;
/* Message parsing variables */
char buff[BUFF_SIZE];
int rtnval, rtn_state, r_RBt;
double motorData;
int Lswitch;
bool plusdir, ls_active = false;
msta_field status;
cntrl = (struct SmartMotorcontroller *) motor_state[card]->DevicePrivate;
motor_info = &(motor_state[card]->motor_info[signal]);
nodeptr = motor_info->motor_motion;
status.All = motor_info->status.All;
buff[0] = '\0';
send_mess(card, "RBt", SmartMotor_addr[signal]);
rtn_state = recv_mess(card, buff, 1);
r_RBt = atoi(buff);
if (rtn_state > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = OK;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
/* decide if move is done */
status.Bits.RA_DONE = (r_RBt == 0) ? 1 : 0;
send_mess(card, "RP", SmartMotor_addr[signal]);
recv_mess(card, buff, 1);
motorData = atof(buff);
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
if (nodeptr != 0) /* If moving, set direction based on commanded positon. */
{
struct motorRecord *mr = (struct motorRecord *) nodeptr->mrecord;
status.Bits.RA_DIRECTION = mr->cdir;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
/* Set limit switch indicators */
send_mess(card, "RBp", SmartMotor_addr[signal]);
recv_mess(card, buff, 1);
Lswitch = atoi(buff);
if (Lswitch != 0)
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
else
status.Bits.RA_PLUS_LS = 0;
send_mess(card, "RBm", SmartMotor_addr[signal]);
recv_mess(card, buff, 1);
Lswitch = atoi(buff);
if (Lswitch != 0)
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
else
status.Bits.RA_MINUS_LS = 0;
send_mess(card, "RBr", SmartMotor_addr[signal]);
recv_mess(card, buff, 1);
Lswitch = atoi(buff);
if (Lswitch)
send_mess(card, "Zr", SmartMotor_addr[signal]);
send_mess(card, "RBl", SmartMotor_addr[signal]);
recv_mess(card, buff, 1);
Lswitch = atoi(buff);
if (Lswitch)
send_mess(card, "Zl", SmartMotor_addr[signal]);
send_mess(card, "RBo", SmartMotor_addr[signal]);
rtn_state = recv_mess(card, buff, 1);
rtnval = atoi(buff);
status.Bits.EA_POSITION = (rtnval != 0) ? 0 : 1;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
if (motor_state[card]->motor_info[signal].encoder_present == NO)
motor_info->encoder_position = 0;
else
{
send_mess(card, "RP", SmartMotor_addr[signal]);
recv_mess(card, buff, 1);
motorData = atof(buff);
motor_info->encoder_position = (int32_t) motorData;
}
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
send_mess(card, "RV", SmartMotor_addr[signal]);
recv_mess(card, buff, 1);
rtnval = atoi(buff);
motor_info->velocity = rtnval;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
/* Test for post-move string. */
if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 &&
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, SmartMotor_addr[signal]);
nodeptr->postmsgptr = NULL;
}
/* Bug fix for DC servo moving away from limit switch, but move is not far enough to
* get off limit switch; resulting in limit error. Fix is to force CDIR to match
* MSTA.RA_DIRECTION.
*/
if (ls_active == true && status.Bits.GAIN_SUPPORT &&
status.Bits.EA_POSITION == 0 && nodeptr != 0)
{
struct motorRecord *mr = (struct motorRecord *) nodeptr->mrecord;
if (mr->cdir != (short) status.Bits.RA_DIRECTION)
mr->cdir = status.Bits.RA_DIRECTION;
}
exit:
motor_info->status.All = status.All;
return(rtn_state);
}
/****************************************************/
/* send a message to the SmartMotor board */
/* send_mess() */
/****************************************************/
static RTN_STATUS send_mess(int card, char const * com, char *name)
{
char local_buff[MAX_MSG_SIZE];
char echo_buff[BUFF_SIZE];
struct SmartMotorcontroller *cntrl;
int comsize, namesize, addr;
size_t nwrite;
static const char input_terminator[] = "\r";
static const char echo_terminator[] = "\n";
local_buff[0] = '\0';
echo_buff[0] = '\0';
comsize = (com == NULL) ? 0 : strlen(com);
namesize = (name == NULL) ? 0 : strlen(name);
if ((comsize + namesize) > MAX_MSG_SIZE)
{
errlogMessage("drvSmartMotor.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (comsize == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvSmartMotor.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
cntrl = (struct SmartMotorcontroller *) motor_state[card]->DevicePrivate;
if (cntrl->num_motors != 1) /* Test for daisy chain; echo mode. */
{
/* convert addr to binary character */
sprintf(local_buff, "%c", atoi(name));
strcat(local_buff, com);
addr = atoi(name) - 128;
}
else
strcpy(local_buff, com);
Debug(2, "send_mess(): message = %s for card# %d, addr# %d\n", local_buff, card, addr);
pasynOctetSyncIO->write(cntrl->pasynUser, local_buff, strlen(local_buff),
COMM_TIMEOUT, &nwrite);
if (cntrl->num_motors == 1)
{
/* do not strip off echoed command */
}
else
{ /* strip off echoed command to get device response */
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, echo_terminator,
strlen(echo_terminator));
recv_mess(card, echo_buff, 1);
}
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
return(OK);
}
/****************************************************/
/* receive a message from the SmartMotor board */
/* recv_mess() */
/****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct SmartMotorcontroller *cntrl;
const double timeout = 0.25;/* !!! orginal 1.0 */
size_t nread = 0;
asynStatus status = asynError;
int eomReason;
char recv_buf[20], *recv_string = recv_buf;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
cntrl = (struct SmartMotorcontroller *) motor_state[card]->DevicePrivate;
recv_string[0] = '\0';
com[0] = '\0';
if (flag == FLUSH)
pasynOctetSyncIO->flush(cntrl->pasynUser);
else
status = pasynOctetSyncIO->read(cntrl->pasynUser, recv_string, BUFF_SIZE,
timeout, &nread, &eomReason);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
}
else
{
strcpy(com, recv_string);
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
return(nread);
}
/*****************************************************/
/* Setup system configuration */
/* SmartMotorSetup() */
/*****************************************************/
RTN_STATUS SmartMotorSetup(int num_cards, int scan_rate) /* maximum number of
* chains in system. */
/* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > SmartMotor_NUM_CARDS)
SmartMotor_num_cards = SmartMotor_NUM_CARDS;
else
SmartMotor_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 SmartMotorConfig 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(SmartMotor_num_cards *
sizeof(struct controller *));
for (itera = 0; itera < SmartMotor_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
/*****************************************************/
/* Configure a controller */
/* SmartMotorConfig() */
/*****************************************************/
RTN_STATUS SmartMotorConfig(int card, const char *name) /* (chain #, ASYN port) */
{
struct SmartMotorcontroller *cntrl;
if (card < 0 || card >= SmartMotor_num_cards)
return(ERROR);
motor_state[card] = (struct controller *) malloc(sizeof(struct controller));
motor_state[card]->DevicePrivate = malloc(sizeof(struct SmartMotorcontroller));
cntrl = (struct SmartMotorcontroller *) motor_state[card]->DevicePrivate;
strcpy(cntrl->asyn_port, name);
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 SmartMotorcontroller *cntrl;
int card_index, motor_index, total_motors;
char buff[BUFF_SIZE];
int status;
asynStatus success_rtn;
static const char output_terminator[] = "\n";
static const char input_terminator[] = "\r";
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (SmartMotor_num_cards <= 0)
return(ERROR);
for (card_index = 0; card_index < SmartMotor_num_cards; card_index++)
{
if (!motor_state[card_index])
continue;
brdptr = motor_state[card_index];
brdptr->ident[0] = (char) NULL; /* No controller identification message. */
brdptr->total_axis = 0; /* Default to zero motors. */
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct SmartMotorcontroller *) brdptr->DevicePrivate;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
{
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
/* Send a message to the motor, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
cntrl->num_motors = 1; /* Default 1 motor (No Echo) for send_mess. */
for (total_motors = 0; total_motors < MAX_AXIS; total_motors++)
{
/* Try 3 times to connect to motor. */
int retry = 0;
do
{
send_mess(card_index, "RBe", SmartMotor_addr[total_motors]);
status = recv_mess(card_index, buff, 1);
/* 1st iteration + No Echo mode + non-null response + cmnd echoed. */
if (total_motors == 0 && cntrl->num_motors == 1 && status > 0 &&
strncmp(buff, "RBe", 3) == 0)
{
cntrl->num_motors = 2; /* Echo ON. */
status = retry = 0; /* start over. */
pasynOctetSyncIO->flush(cntrl->pasynUser);
}
if (buff[0] != '0' && buff[0] != '1')
status = 0;
retry++;
} while (status == 0 && retry < 3);
if (status <= 0)
break;
if (cntrl->num_motors == 1 && status > 0)
{
total_motors = 1;
break;
}
}
}
if (success_rtn == asynSuccess && total_motors > 0)
{
cntrl->num_motors = brdptr->total_axis = total_motors;
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
for (motor_index = 0; motor_index < brdptr->total_axis; 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;
/* Assume no encoder support. */
motor_info->encoder_present = NO;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 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 *) "SmartMotor_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(OK);
}
extern "C"
{
// SmartMotor Setup arguments
static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt};
static const iocshArg setupArg1 = {"Polling rate", iocshArgInt};
// SmartMotor Config arguments
static const iocshArg configArg0 = {"Card being configured", iocshArgInt};
static const iocshArg configArg1 = {"asyn port name", iocshArgString};
static const iocshArg * const SmartMotorSetupArgs[2] = {&setupArg0, &setupArg1};
static const iocshArg * const SmartMotorConfigArgs[2] = {&configArg0, &configArg1};
static const iocshFuncDef setupSmartMotor = {"SmartMotorSetup", 2, SmartMotorSetupArgs};
static const iocshFuncDef configSmartMotor = {"SmartMotorConfig", 2, SmartMotorConfigArgs};
static void setupSmartMotorCallFunc(const iocshArgBuf *args)
{
SmartMotorSetup(args[0].ival, args[1].ival);
}
static void configSmartMotorCallFunc(const iocshArgBuf *args)
{
SmartMotorConfig(args[0].ival, args[1].sval);
}
static void SmartMotormotorRegister(void)
{
iocshRegister(&setupSmartMotor, setupSmartMotorCallFunc);
iocshRegister(&configSmartMotor, configSmartMotorCallFunc);
}
epicsExportRegistrar(SmartMotormotorRegister);
} // extern "C"
motorApp/SmartMotorSrc/drvSmartMotor.h
+67
View File
@@ -0,0 +1,67 @@
/*
FILENAME... drvSmartMotor.h
USAGE... This file contains driver "include" information that is specific to
Animatics Corporation SmartMotors.
Version: 1.0
Modified By: shoaf
Last Modified: 2007/02/20
*/
/*
* Original Author: Ron Sluiter
* Date: 02/10/2000
*
* 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 02/20/2007 ses copied from drvMM4000.h
*/
#ifndef INCdrvSmartMotorh
#define INCdrvSmartMotorh 1
#include "motordrvCom.h"
#include "asynDriver.h"
#include "asynOctetSyncIO.h"
#define COMM_TIMEOUT 2 /* Timeout in seconds */
/* Animatics SmartMotor specific data is stored in this structure. */
struct SmartMotorcontroller
{
asynUser *pasynUser; /* For RS-232 */
char asyn_port[80]; /* asyn port name */
int num_motors; /* # daisy chained motors on link. */
CommStatus status; /* Controller communication status. */
};
/* Function prototypes. */
extern RTN_STATUS SmartMotorSetup(int, int);
extern RTN_STATUS SmartMotorConfig(int, const char *);
#endif /* INCdrvSmartMotor.h */