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

Corrected compiler error when passing Asyn parameter(s).

Browse Source
This commit is contained in:
David Kline
2005-04-14 18:52:48 +00:00
parent 94b50f4f38
commit ce49cec901
9 changed files with 2075 additions and 2095 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/ImsSrc/drvIM483PL.cc
+211 -218
View File
@@ -1,11 +1,11 @@
/*
FILENAME... drvIM483PL.cc
USAGE... Motor record driver level support for Intelligent Motion
Systems, Inc. IM483(I/IE).
FILENAME... drvIM483PL.cc
USAGE... Motor record driver level support for Intelligent Motion
Systems, Inc. IM483(I/IE).
Version: $Revision: 1.13 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-03-30 19:01:27 $
Version: $Revision: 1.14 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:51:51 $
*/
/*****************************************************************
@@ -28,9 +28,9 @@ of this distribution.
* .01 07/10/00 rls copied from drvIM483SM.c
* .02 10/02/01 rls allow one retry after a communication error.
* .03 04/15/02 rls Bug fix for limit switches. Set RA_DIRECTION in
* set_status() based on (new - old) commanded position.
* Removed support for "ASCII record separator (IS2) = /x1E"
* from send_mess().
* set_status() based on (new - old) commanded position.
* Removed support for "ASCII record separator (IS2) = /x1E"
* from send_mess().
* .04 03/07/03 rls R3.14 conversion.
* .05 02/03/04 rls Eliminate erroneous "Motor motion timeout ERROR".
* .06 07/01/04 rls Converted from MPF to asyn.
@@ -38,22 +38,22 @@ of this distribution.
* - support for 32axes/controller.
* - remove '?' command line padding.
* .08 12/14/04 rls - asyn R4.0 support.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - retry on initial communication.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - retry on initial communication.
*/
/*
DESIGN LIMITATIONS...
1 - Like all controllers, the IM483 must be powered-on when EPICS is first
booted up.
booted up.
2 - The IM483 cannot be power cycled while EPICS is up and running. The
consequences are permanent communication lose with the IM483 until
EPICS is rebooted.
consequences are permanent communication lose with the IM483 until
EPICS is rebooted.
3 - Like the Newport MM3000, the IM483's position can only be set to zero.
4 - The IM483 uses an internal look-up table for acceleration/deceleration.
Translation between the IM483 and the ACCL/BACC fields is not obvious.
Translation between the IM483 and the ACCL/BACC fields is not obvious.
*/
#include <string.h>
@@ -65,22 +65,22 @@ DESIGN LIMITATIONS...
#include "epicsExport.h"
/* Read Limit Status response values. */
#define L_ALIMIT 1
#define L_BLIMIT 2
#define L_BOTH_LIMITS 3
#define L_ALIMIT 1
#define L_BLIMIT 2
#define L_BOTH_LIMITS 3
#define IM483PL_NUM_CARDS 8
#define MAX_AXES 8
#define BUFF_SIZE 50 /* Maximum length of string to/from IM483PL */
#define IM483PL_NUM_CARDS 8
#define MAX_AXES 8
#define BUFF_SIZE 50 /* Maximum length of string to/from IM483PL */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvIM483PLdebug) printf(f, ## args);}
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvIM483PLdebug) printf(f, ## args);}
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -93,7 +93,7 @@ int IM483PL_num_cards = 0;
static char *IM483PL_axis[] = {"A", "B", "C", "D", "E", "F", "G", "H"};
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
@@ -149,24 +149,24 @@ static long report(int level)
int card;
if (IM483PL_num_cards <=0)
printf(" No IM483PL controllers configured.\n");
printf(" No IM483PL controllers configured.\n");
else
{
for (card = 0; card < IM483PL_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < IM483PL_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" IM483PL controller %d connection failed.\n", card);
else
{
struct IM483controller *cntrl;
if (brdptr == NULL)
printf(" IM483PL controller %d connection failed.\n", card);
else
{
struct IM483controller *cntrl;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
printf(" IM483PL controller #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
printf(" IM483PL controller #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
}
return(OK);
}
@@ -174,7 +174,7 @@ static long report(int level)
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
@@ -183,7 +183,7 @@ static long init()
/* Check for setup */
if (IM483PL_num_cards <= 0)
{
Debug(1, "init(): IM483PL driver disabled. IM483PLSetup() missing from startup script.\n");
Debug(1, "init(): IM483PL driver disabled. IM483PLSetup() missing from startup script.\n");
}
return((long) 0);
}
@@ -195,32 +195,32 @@ 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 *
* *
* *
* *
* 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)
@@ -244,32 +244,32 @@ static int set_status(int card, int signal)
rtn_state = recv_mess(card, buff, 1);
if (rtn_state > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
rtnval = atoi(&buff[4]);
status.Bits.RA_DONE = (rtnval != 0) ? 0 : 1;
/*
/*
* Parse motor position
* Position string format: 1TP5.012,2TP1.123,3TP-100.567,...
* Skip to substring for this motor, convert to double
@@ -282,17 +282,17 @@ static int set_status(int card, int signal)
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
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;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
@@ -304,21 +304,21 @@ static int set_status(int card, int signal)
/* Set limit switch error indicators. */
if (rtnval & 1)
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
else
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_PLUS_LS = 0;
if (rtnval & 2)
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
else
status.Bits.RA_MINUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
send_mess(card, " ] 1", IM483PL_axis[signal]);
recv_mess(card, buff, 1);
@@ -330,21 +330,21 @@ static int set_status(int card, int signal)
status.Bits.EA_POSITION = 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_HOME = 0;
if (motor_state[card]->motor_info[signal].encoder_present == NO)
motor_info->encoder_position = 0;
motor_info->encoder_position = 0;
else
{
send_mess(card, " z 0", IM483PL_axis[signal]);
recv_mess(card, buff, 1);
motorData = atof(&buff[5]);
motor_info->encoder_position = (int32_t) motorData;
send_mess(card, " z 0", IM483PL_axis[signal]);
recv_mess(card, buff, 1);
motorData = atof(&buff[5]);
motor_info->encoder_position = (int32_t) motorData;
}
status.Bits.RA_PROBLEM = 0;
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
/* NEEDS WORK */
@@ -352,18 +352,17 @@ static int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 && nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, IM483PL_axis[signal]);
nodeptr->postmsgptr = NULL;
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, IM483PL_axis[signal]);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -373,47 +372,46 @@ exit:
/*****************************************************/
/* send a message to the IM483PL board */
/* send_mess() */
/* send a message to the IM483PL board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
char local_buff[MAX_MSG_SIZE];
struct IM483controller *cntrl;
int comsize, namesize;
int nwrite;
size_t nwrite;
comsize = (com == NULL) ? 0 : strlen(com);
namesize = (name == NULL) ? 0 : strlen(name);
if ((comsize + namesize) > MAX_MSG_SIZE)
{
errlogMessage("drvIM483PL.c:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvIM483PL.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (comsize == 0) /* Normal exit on empty input message. */
return(OK);
else if (comsize == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvIM483PL.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvIM483PL.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
/* Make a local copy of the string and add the command line terminator. */
if (namesize != 0)
{
strcpy(local_buff, name); /* put in axis */
strcat(local_buff, com);
strcpy(local_buff, name); /* put in axis */
strcat(local_buff, com);
}
else
strcpy(local_buff, com);
strcpy(local_buff, com);
Debug(2, "send_mess(): message = %s\n", local_buff);
cntrl = (struct IM483controller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, local_buff, strlen(local_buff),
COMM_TIMEOUT, &nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, local_buff, strlen(local_buff), COMM_TIMEOUT, &nwrite);
return(OK);
}
@@ -421,31 +419,30 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
/*****************************************************/
/* receive a message from the IM483 board */
/* recv_mess() */
/* recv_mess() */
/*****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct IM483controller *cntrl;
int nread = 0;
size_t nread = 0;
asynStatus status = asynError;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct IM483controller *) motor_state[card]->DevicePrivate;
if (flag == FLUSH)
pasynOctetSyncIO->flush(cntrl->pasynUser);
pasynOctetSyncIO->flush(cntrl->pasynUser);
else
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
COMM_TIMEOUT, &nread, &eomReason);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, COMM_TIMEOUT, &nread, &eomReason);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -458,33 +455,32 @@ static int recv_mess(int card, char *com, int flag)
/* IM483PLSetup() */
/*****************************************************/
RTN_STATUS
IM483PLSetup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
IM483PLSetup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > IM483PL_NUM_CARDS)
IM483PL_num_cards = IM483PL_NUM_CARDS;
IM483PL_num_cards = IM483PL_NUM_CARDS;
else
IM483PL_num_cards = num_cards;
IM483PL_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before IM483Config 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(IM483PL_num_cards *
sizeof(struct controller *));
motor_state = (struct controller **) malloc(IM483PL_num_cards * sizeof(struct controller *));
for (itera = 0; itera < IM483PL_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
@@ -495,8 +491,8 @@ IM483PLSetup(int num_cards, /* maximum number of controllers in system. */
/* IM483PLConfig() */
/*****************************************************/
RTN_STATUS
IM483PLConfig(int card, /* card being configured */
const char *name) /* asyn server task name */
IM483PLConfig(int card, /* card being configured */
const char *name) /* asyn server task name */
{
struct IM483controller *cntrl;
@@ -513,10 +509,10 @@ IM483PLConfig(int card, /* card being configured */
/*****************************************************/
/* initialize all software and hardware */
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
@@ -530,86 +526,83 @@ static int motor_init()
static const char output_terminator[] = "\n";
static const char input_terminator[] = "\n";
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (IM483PL_num_cards <= 0)
return(ERROR);
return(ERROR);
for (card_index = 0; card_index < IM483PL_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->cmnd_response = true;
total_cards = card_index + 1;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
if (!motor_state[card_index])
continue;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
brdptr = motor_state[card_index];
brdptr->ident[0] = (char) NULL; /* No controller identification message. */
brdptr->cmnd_response = true;
total_cards = card_index + 1;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
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 board, see if it exists */
/* flush any junk at input port - should not be any data available */
/* 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 board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
for (total_axis = 0; total_axis < MAX_AXES; total_axis++)
{
int retry = 0;
do
{
send_mess(card_index, " Z 0", IM483PL_axis[total_axis]);
status = recv_mess(card_index, buff, 1);
retry++;
} while (status <= 0 && retry < 3);
for (total_axis = 0; total_axis < MAX_AXES; total_axis++)
{
int retry = 0;
if (status <= 0)
break;
}
brdptr->total_axis = total_axis;
}
do
{
send_mess(card_index, " Z 0", IM483PL_axis[total_axis]);
status = recv_mess(card_index, buff, 1);
retry++;
} while (status <= 0 && retry < 3);
if (success_rtn == asynSuccess && total_axis > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
if (status <= 0)
break;
}
brdptr->total_axis = total_axis;
}
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
int loop_state;
if (success_rtn == asynSuccess && total_axis > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
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 encoder support, i.e., IM483IE. */
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
int loop_state;
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 encoder support, i.e., IM483IE. */
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
/* Determine if encoder present based on open/closed loop mode. */
loop_state = 0;
if (loop_state != 0)
{
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
loop_state = 0;
if (loop_state != 0)
{
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -621,9 +614,9 @@ static int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "IM483PL_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(OK);
}
motorApp/ImsSrc/drvIM483SM.cc
+243 -250
View File
@@ -1,13 +1,13 @@
/*
FILENAME... drvIM483SM.cc
USAGE... Motor record driver level support for Intelligent Motion
Systems, Inc. IM483(I/IE).
FILENAME... drvIM483SM.cc
USAGE... Motor record driver level support for Intelligent Motion
Systems, Inc. IM483(I/IE).
Version: $Revision: 1.14 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-03-30 19:01:27 $
Version: $Revision: 1.15 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:51:52 $
*/
/*****************************************************************
COPYRIGHT NOTIFICATION
*****************************************************************
@@ -28,30 +28,30 @@ of this distribution.
* .01 02/10/00 rls copied from drvMM4000.c
* .02 10/02/01 rls allow one retry after a communication error.
* .03 04/15/02 rls Bug fix for limit switches. Set RA_DIRECTION in
* set_status() based on (new - old) commanded position.
* Removed support for "ASCII record separator (IS2) = /x1E"
* from send_mess().
* set_status() based on (new - old) commanded position.
* Removed support for "ASCII record separator (IS2) = /x1E"
* from send_mess().
* .04 03/07/03 rls R3.14 conversion.
* .05 02/03/04 rls Eliminate erroneous "Motor motion timeout ERROR".
* .06 07/01/04 rls Converted from MPF to asyn.
* .07 09/20/04 rls support for 32axes/controller.
* .08 12/14/04 rls - asyn R4.0 support.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - retry on initial communication.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - retry on initial communication.
*/
/*
DESIGN LIMITATIONS...
1 - Like all controllers, the IM483 must be powered-on when EPICS is first
booted up.
booted up.
2 - The IM483 cannot be power cycled while EPICS is up and running. The
consequences are permanent communication lose with the IM483 until
EPICS is rebooted.
consequences are permanent communication lose with the IM483 until
EPICS is rebooted.
3 - Like the Newport MM3000, the IM483's position can only be set to zero.
4 - The IM483 uses an internal look-up table for acceleration/deceleration.
Translation between the IM483 and the ACCL/BACC fields is not obvious.
Translation between the IM483 and the ACCL/BACC fields is not obvious.
*/
#include <string.h>
@@ -64,20 +64,20 @@ DESIGN LIMITATIONS...
#include "epicsExport.h"
/* Read Limit Status response values. */
#define L_ALIMIT 1
#define L_BLIMIT 2
#define L_BOTH_LIMITS 3
#define L_ALIMIT 1
#define L_BLIMIT 2
#define L_BOTH_LIMITS 3
#define IM483SM_NUM_CARDS 8
#define IM483SM_NUM_CARDS 8
#define BUFF_SIZE 50 /* Maximum length of string to/from IM483 */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvIM483SMdebug) printf(f, ## args);}
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvIM483SMdebug) printf(f, ## args);}
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -89,7 +89,7 @@ extern "C" {epicsExportAddress(int, drvIM483SMdebug);}
int IM483SM_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
@@ -146,24 +146,24 @@ static long report(int level)
int card;
if (IM483SM_num_cards <=0)
printf(" No IM483SM controllers configured.\n");
printf(" No IM483SM controllers configured.\n");
else
{
for (card = 0; card < IM483SM_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < IM483SM_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" IM483SM controller %d connection failed.\n", card);
else
{
struct IM483controller *cntrl;
if (brdptr == NULL)
printf(" IM483SM controller %d connection failed.\n", card);
else
{
struct IM483controller *cntrl;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
printf(" IM483SM controller #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
printf(" IM483SM controller #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
}
return(OK);
}
@@ -171,7 +171,7 @@ static long report(int level)
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
@@ -180,7 +180,7 @@ static long init()
/* Check for setup */
if (IM483SM_num_cards <= 0)
{
Debug(1, "init(): IM483SM driver disabled. IM483SMSetup() missing from startup script.\n");
Debug(1, "init(): IM483SM driver disabled. IM483SMSetup() missing from startup script.\n");
}
return((long) 0);
}
@@ -192,32 +192,32 @@ 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 *
* *
* *
* *
* 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)
@@ -241,32 +241,32 @@ static int set_status(int card, int signal)
rtn_state = recv_mess(card, buff, 1);
if (rtn_state > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
rtnval = atoi(&buff[4]);
status.Bits.RA_DONE = (rtnval != 0) ? 0 : 1;
/*
/*
* Parse motor position
* Position string format: 1TP5.012,2TP1.123,3TP-100.567,...
* Skip to substring for this motor, convert to double
@@ -279,17 +279,17 @@ static int set_status(int card, int signal)
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
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;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
@@ -301,21 +301,21 @@ static int set_status(int card, int signal)
/* Set limit switch error indicators. */
if (rtnval & 1)
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
else
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_PLUS_LS = 0;
if (rtnval & 2)
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
else
status.Bits.RA_MINUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
send_mess(card, "] 1", (char) NULL);
recv_mess(card, buff, 1);
@@ -327,21 +327,21 @@ static int set_status(int card, int signal)
status.Bits.EA_POSITION = 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_HOME = 0;
if (motor_state[card]->motor_info[signal].encoder_present == NO)
motor_info->encoder_position = 0;
motor_info->encoder_position = 0;
else
{
send_mess(card, "z 0", (char) NULL);
recv_mess(card, buff, 1);
motorData = atof(&buff[5]);
motor_info->encoder_position = (int32_t) motorData;
send_mess(card, "z 0", (char) NULL);
recv_mess(card, buff, 1);
motorData = atof(&buff[5]);
motor_info->encoder_position = (int32_t) motorData;
}
status.Bits.RA_PROBLEM = 0;
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
/* NEEDS WORK */
@@ -349,18 +349,17 @@ static int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -370,42 +369,41 @@ exit:
/*****************************************************/
/* send a message to the IM483SM board */
/* send_mess() */
/* send a message to the IM483SM board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
struct IM483controller *cntrl;
int size;
int nwrite;
size_t nwrite;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvIM483SM.c:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvIM483SM.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvIM483SM.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvIM483SM.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
if (name != NULL)
{
errlogPrintf("drvIM483SM.c:send_mess() - invalid argument = %s\n", name);
return(ERROR);
errlogPrintf("drvIM483SM.c:send_mess() - invalid argument = %s\n", name);
return(ERROR);
}
Debug(2, "send_mess(): message = %s\n", com);
cntrl = (struct IM483controller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT,
&nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT, &nwrite);
return(OK);
}
@@ -413,31 +411,30 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
/*****************************************************/
/* receive a message from the IM483 board */
/* recv_mess() */
/* recv_mess() */
/*****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct IM483controller *cntrl;
int nread = 0;
size_t nread = 0;
asynStatus status = asynError;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct IM483controller *) motor_state[card]->DevicePrivate;
if (flag == FLUSH)
pasynOctetSyncIO->flush(cntrl->pasynUser);
pasynOctetSyncIO->flush(cntrl->pasynUser);
else
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
COMM_TIMEOUT, &nread, &eomReason);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, COMM_TIMEOUT, &nread, &eomReason);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -450,33 +447,32 @@ static int recv_mess(int card, char *com, int flag)
/* IM483SMSetup() */
/*****************************************************/
RTN_STATUS
IM483SMSetup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
IM483SMSetup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > IM483SM_NUM_CARDS)
IM483SM_num_cards = IM483SM_NUM_CARDS;
IM483SM_num_cards = IM483SM_NUM_CARDS;
else
IM483SM_num_cards = num_cards;
IM483SM_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before IM483SMConfig 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(IM483SM_num_cards *
sizeof(struct controller *));
motor_state = (struct controller **) malloc(IM483SM_num_cards * sizeof(struct controller *));
for (itera = 0; itera < IM483SM_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
@@ -487,7 +483,7 @@ IM483SMSetup(int num_cards, /* maximum number of controllers in system. */
/* IM483SMConfig() */
/*****************************************************/
RTN_STATUS
IM483SMConfig(int card, /* card being configured */
IM483SMConfig(int card, /* card being configured */
const char *name) /* asyn server task name */
{
struct IM483controller *cntrl;
@@ -505,10 +501,10 @@ IM483SMConfig(int card, /* card being configured */
/*****************************************************/
/* initialize all software and hardware */
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
@@ -522,125 +518,122 @@ static int motor_init()
static const char output_terminator[] = "\r";
static const char input_terminator[] = "\r\n";
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (IM483SM_num_cards <= 0)
return(ERROR);
return(ERROR);
for (card_index = 0; card_index < IM483SM_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 IM483controller *) brdptr->DevicePrivate;
if (!motor_state[card_index])
continue;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
brdptr = motor_state[card_index];
brdptr->cmnd_response = true;
total_cards = card_index + 1;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
if (success_rtn == asynSuccess)
{
int itera, retry = 0;
char *src, *dest;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0, &cntrl->pasynUser, NULL);
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
do
{
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
if (success_rtn == asynSuccess)
{
int itera, retry = 0;
char *src, *dest;
send_mess(card_index, "\003", (char) NULL); /* Reset device. */
epicsThreadSleep(1.0);
send_mess(card_index, " ", (char) NULL);
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator, strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator, strlen(input_terminator));
do
{
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
/* Save controller identification message. */
src = buff;
dest = brdptr->ident;
*src = (char) NULL;
send_mess(card_index, "\003", (char) NULL); /* Reset device. */
epicsThreadSleep(1.0);
send_mess(card_index, " ", (char) NULL);
for (itera = 0; itera < 50; itera++)
{
if (*src == (char) NULL)
{
status = recv_mess(card_index, buff, 1);
if (status <= 0)
{
if (itera != 0)
{
*dest = (char) NULL;
status = 1;
}
break;
}
src = buff;
while (isspace(*src++));
--src;
if (itera != 0)
{
*dest++ = ' ';
itera++;
}
}
else if (isspace(*src))
{
while (isspace(*src++));
src -= 2;
}
else if (strncmp(src, "AD", 2) == 0)
{
strcpy(dest, "AMS");
src += 22;
dest += 3;
itera += 3;
}
*dest++ = *src++;
}
retry++;
/* Return value is length of response string */
} while (status == 0 && retry < 3);
}
/* Save controller identification message. */
src = buff;
dest = brdptr->ident;
*src = (char) NULL;
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
for (itera = 0; itera < 50; itera++)
{
if (*src == (char) NULL)
{
status = recv_mess(card_index, buff, 1);
if (status <= 0)
{
if (itera != 0)
{
*dest = (char) NULL;
status = 1;
}
break;
}
src = buff;
while (isspace(*src++));
--src;
if (itera != 0)
{
*dest++ = ' ';
itera++;
}
}
else if (isspace(*src))
{
while (isspace(*src++));
src -= 2;
}
else if (strncmp(src, "AD", 2) == 0)
{
strcpy(dest, "AMS");
src += 22;
dest += 3;
itera += 3;
}
*dest++ = *src++;
}
retry++;
/* Return value is length of response string */
} while (status == 0 && retry < 3);
}
brdptr->total_axis = total_axis = 1;
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
int loop_state;
brdptr->total_axis = total_axis = 1;
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;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
int loop_state;
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. */
loop_state = 0;
if (loop_state != 0)
{
loop_state = 0;
if (loop_state != 0)
{
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -652,8 +645,8 @@ static int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "IM483SM_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(OK);
}
motorApp/ImsSrc/drvMDrive.cc
+275 -275
View File
@@ -1,11 +1,11 @@
/*
FILENAME... drvMDrive.cc
USAGE... Motor record driver level support for Intelligent Motion
Systems, Inc. MDrive series; M17, M23, M34.
FILENAME... drvMDrive.cc
USAGE... Motor record driver level support for Intelligent Motion
Systems, Inc. MDrive series; M17, M23, M34.
Version: $Revision: 1.16 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-03-30 19:01:26 $
Version: $Revision: 1.17 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:51:52 $
*/
/*
@@ -22,16 +22,16 @@ Last Modified: $Date: 2005-03-30 19:01:26 $
* 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
* 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
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
@@ -44,20 +44,20 @@ Last Modified: $Date: 2005-03-30 19:01:26 $
* .05 09/20/04 rls - support for 32axes/controller.
* - remove '?' command string padding.
* .06 12/16/04 rls - asyn R4.0 support.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* .07 03/18/05 rls - Flexible MDrive I/O configuration.
* - Change Echo mode to 2; eliminate eat_garbage().
* - Change Echo mode to 2; eliminate eat_garbage().
*/
/*
DESIGN LIMITATIONS...
1 - Like all controllers, the MDrive must be powered-on when EPICS is first
booted up.
booted up.
2 - The MDrive cannot be power cycled while EPICS is up and running. The
consequences are permanent communication lose with the MDrive until
EPICS is rebooted.
consequences are permanent communication lose with the MDrive until
EPICS is rebooted.
*/
#include <string.h>
@@ -68,16 +68,16 @@ DESIGN LIMITATIONS...
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#define MDrive_NUM_CARDS 8
#define MAX_AXES 8
#define BUFF_SIZE 13 /* Maximum length of string to/from MDrive */
#define MDrive_NUM_CARDS 8
#define MAX_AXES 8
#define BUFF_SIZE 13 /* Maximum length of string to/from MDrive */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvMDrivedebug) printf(f, ## args);}
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvMDrivedebug) printf(f, ## args);}
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -90,7 +90,7 @@ int MDrive_num_cards = 0;
static char *MDrive_axis[] = {"1", "2", "3", "4", "5", "6", "7", "8"};
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
@@ -147,24 +147,24 @@ static long report(int level)
int card;
if (MDrive_num_cards <=0)
printf(" No MDrive controllers configured.\n");
printf(" No MDrive controllers configured.\n");
else
{
for (card = 0; card < MDrive_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < MDrive_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" MDrive controller %d connection failed.\n", card);
else
{
struct IM483controller *cntrl;
if (brdptr == NULL)
printf(" MDrive controller %d connection failed.\n", card);
else
{
struct IM483controller *cntrl;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
printf(" IM483SM controller #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
printf(" IM483SM controller #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
}
return(OK);
}
@@ -172,7 +172,7 @@ static long report(int level)
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
@@ -181,7 +181,7 @@ static long init()
/* Check for setup */
if (MDrive_num_cards <= 0)
{
Debug(1, "init(): MDrive driver disabled. MDriveSetup() missing from startup script.\n");
Debug(1, "init(): MDrive driver disabled. MDriveSetup() missing from startup script.\n");
}
return((long) 0);
}
@@ -193,32 +193,32 @@ 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 *
* *
* *
* *
* 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)
@@ -244,32 +244,32 @@ static int set_status(int card, int signal)
rtn_state = recv_mess(card, buff, 1);
if (rtn_state > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 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;
}
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;
}
}
rtnval = atoi(buff);
status.Bits.RA_DONE = (rtnval != 0) ? 0 : 1;
/*
/*
* Parse motor position
* Position string format: 1TP5.012,2TP1.123,3TP-100.567,...
* Skip to substring for this motor, convert to double
@@ -282,25 +282,25 @@ static int set_status(int card, int signal)
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
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;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
if (confptr->plusLS == 0 || confptr->minusLS == 0)
{
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
}
else
{
@@ -310,60 +310,60 @@ static int set_status(int card, int signal)
Lswitch = atoi(buff);
/* Set limit Lswitch error indicators. */
if (Lswitch != 0)
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
else
status.Bits.RA_PLUS_LS = 0;
if (Lswitch != 0)
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
else
status.Bits.RA_PLUS_LS = 0;
sprintf(buff, "PR I%d", confptr->minusLS);
send_mess(card, buff, MDrive_axis[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;
if (Lswitch != 0)
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
else
status.Bits.RA_MINUS_LS = 0;
}
if (confptr->homeLS == 0)
status.Bits.RA_HOME = 0;
status.Bits.RA_HOME = 0;
else
{
sprintf(buff, "PR I%d", confptr->homeLS);
send_mess(card, buff, MDrive_axis[signal]);
recv_mess(card, buff, 1);
Lswitch = atoi(buff);
status.Bits.RA_HOME = (Lswitch) ? 1 : 0;
status.Bits.RA_HOME = (Lswitch) ? 1 : 0;
}
/* !!! Assume no closed-looped control!!!*/
status.Bits.EA_POSITION = 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_HOME = 0;
if (motor_state[card]->motor_info[signal].encoder_present == NO)
motor_info->encoder_position = 0;
motor_info->encoder_position = 0;
else
{
send_mess(card, "PR C2", MDrive_axis[signal]);
recv_mess(card, buff, 1);
motorData = atof(buff);
motor_info->encoder_position = (int32_t) motorData;
send_mess(card, "PR C2", MDrive_axis[signal]);
recv_mess(card, buff, 1);
motorData = atof(buff);
motor_info->encoder_position = (int32_t) motorData;
}
status.Bits.RA_PROBLEM = 0;
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
/* NEEDS WORK */
@@ -371,18 +371,18 @@ static int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, MDrive_axis[signal]);
nodeptr->postmsgptr = NULL;
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, MDrive_axis[signal]);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -392,47 +392,47 @@ exit:
/*****************************************************/
/* send a message to the MDrive board */
/* send_mess() */
/* send a message to the MDrive board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
char local_buff[MAX_MSG_SIZE];
struct IM483controller *cntrl;
int comsize, namesize;
int nwrite;
size_t nwrite;
comsize = (com == NULL) ? 0 : strlen(com);
namesize = (name == NULL) ? 0 : strlen(name);
if ((comsize + namesize) > MAX_MSG_SIZE)
{
errlogMessage("drvMDrive.c:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvMDrive.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (comsize == 0) /* Normal exit on empty input message. */
return(OK);
else if (comsize == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvMDrive.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvMDrive.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
/* Make a local copy of the string and add the command line terminator. */
if (namesize != 0)
{
strcpy(local_buff, name); /* put in axis */
strcat(local_buff, com);
strcpy(local_buff, name); /* put in axis */
strcat(local_buff, com);
}
else
strcpy(local_buff, com);
strcpy(local_buff, com);
Debug(2, "send_mess(): message = %s\n", local_buff);
cntrl = (struct IM483controller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, local_buff, strlen(local_buff),
COMM_TIMEOUT, &nwrite);
COMM_TIMEOUT, &nwrite);
return(OK);
}
@@ -440,32 +440,32 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
/*****************************************************/
/* receive a message from the MDrive board */
/* recv_mess() */
/* recv_mess() */
/*****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct IM483controller *cntrl;
const double timeout = 1.0;
int nread = 0;
size_t nread = 0;
asynStatus status = asynError;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct IM483controller *) motor_state[card]->DevicePrivate;
if (flag == FLUSH)
pasynOctetSyncIO->flush(cntrl->pasynUser);
pasynOctetSyncIO->flush(cntrl->pasynUser);
else
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
timeout, &nread, &eomReason);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
timeout, &nread, &eomReason);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -478,33 +478,33 @@ static int recv_mess(int card, char *com, int flag)
/* MDriveSetup() */
/*****************************************************/
RTN_STATUS
MDriveSetup(int num_cards, /* maximum number of chains in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
MDriveSetup(int num_cards, /* maximum number of chains in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > MDrive_NUM_CARDS)
MDrive_num_cards = MDrive_NUM_CARDS;
MDrive_num_cards = MDrive_NUM_CARDS;
else
MDrive_num_cards = num_cards;
MDrive_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before IM483Config 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(MDrive_num_cards *
sizeof(struct controller *));
sizeof(struct controller *));
for (itera = 0; itera < MDrive_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
@@ -515,8 +515,8 @@ MDriveSetup(int num_cards, /* maximum number of chains in system. */
/* MDriveConfig() */
/*****************************************************/
RTN_STATUS
MDriveConfig(int card, /* chain being configured */
const char *name) /* ASYN port name */
MDriveConfig(int card, /* chain being configured */
const char *name) /* ASYN port name */
{
struct IM483controller *cntrl;
@@ -533,10 +533,10 @@ MDriveConfig(int card, /* chain being configured */
/*****************************************************/
/* initialize all software and hardware */
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
@@ -550,137 +550,137 @@ static int motor_init()
static const char output_terminator[] = "\n";
static const char input_terminator[] = "\r\n";
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (MDrive_num_cards <= 0)
return(ERROR);
return(ERROR);
for (card_index = 0; card_index < MDrive_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->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
if (!motor_state[card_index])
continue;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
brdptr = motor_state[card_index];
brdptr->ident[0] = (char) NULL; /* No controller identification message. */
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct IM483controller *) brdptr->DevicePrivate;
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 board, see if it exists */
/* flush any junk at input port - should not be any data available */
/* 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 board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
for (total_axis = 0; total_axis < MAX_AXES; total_axis++)
{
int retry = 0;
/* Try 3 times to connect to controller. */
do
{
send_mess(card_index, "PR VR", MDrive_axis[total_axis]);
status = recv_mess(card_index, buff, 1);
retry++;
} while (status == 0 && retry < 3);
if (status <= 0)
break;
else if (total_axis == 0)
strcpy(brdptr->ident, buff);
}
brdptr->total_axis = total_axis;
cntrl->inconfig = (input_config *) malloc(
sizeof(struct IM483controller) * total_axis);
}
for (total_axis = 0; total_axis < MAX_AXES; total_axis++)
{
int retry = 0;
if (success_rtn == asynSuccess && total_axis > 0)
{
input_config *confptr = cntrl->inconfig;
/* Try 3 times to connect to controller. */
do
{
send_mess(card_index, "PR VR", MDrive_axis[total_axis]);
status = recv_mess(card_index, buff, 1);
retry++;
} while (status == 0 && retry < 3);
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
if (status <= 0)
break;
else if (total_axis == 0)
strcpy(brdptr->ident, buff);
}
brdptr->total_axis = total_axis;
cntrl->inconfig = (input_config *) malloc(
sizeof(struct IM483controller) * total_axis);
}
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
int itera;
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
if (success_rtn == asynSuccess && total_axis > 0)
{
input_config *confptr = cntrl->inconfig;
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;
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
int itera;
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;
/* Determine if encoder present based last character of "ident". */
if (brdptr->ident[strlen(brdptr->ident) - 1] == 'E')
{
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
}
if (brdptr->ident[strlen(brdptr->ident) - 1] == 'E')
{
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
}
/* Determine input configuration. */
confptr->homeLS = confptr->minusLS = confptr->plusLS = 0;
confptr->homeLS = confptr->minusLS = confptr->plusLS = 0;
for (itera = 1; itera <= 4; itera++)
{
int type, active;
for (itera = 1; itera <= 4; itera++)
{
int type, active;
sprintf(buff, "PR S%d", itera);
send_mess(card_index, buff, MDrive_axis[motor_index]);
status = recv_mess(card_index, buff, 1);
if (status == 0)
{
errlogPrintf("Error reading I/O configuration.\n");
break;
}
status = sscanf(buff, "%d,%d", &type, &active);
switch (type)
{
case 0:
break;
case 1: // Home switch.
confptr->homeLS = itera;
break;
case 2: // Plus limit switch.
confptr->plusLS = itera;
break;
case 3: // Minus limit switch.
confptr->minusLS = itera;
break;
default:
errlogPrintf("Invalid I/O type: %d.\n", type);
sprintf(buff, "PR S%d", itera);
send_mess(card_index, buff, MDrive_axis[motor_index]);
status = recv_mess(card_index, buff, 1);
if (status == 0)
{
errlogPrintf("Error reading I/O configuration.\n");
break;
}
}
}
// Test for missing configuration.
if (confptr->minusLS == 0 || confptr->plusLS == 0)
{
const char p_label[6] = "Plus", m_label[6] = "Minus";
errlogPrintf("MDrive chain #%d, motor #%d %s LS not configured.\n",
card_index, motor_index,
(confptr->minusLS == 0) ? m_label : p_label);
}
status = sscanf(buff, "%d,%d", &type, &active);
switch (type)
{
case 0:
break;
case 1: // Home switch.
confptr->homeLS = itera;
break;
case 2: // Plus limit switch.
confptr->plusLS = itera;
break;
case 3: // Minus limit switch.
confptr->minusLS = itera;
break;
default:
errlogPrintf("Invalid I/O type: %d.\n", type);
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
}
// Test for missing configuration.
if (confptr->minusLS == 0 || confptr->plusLS == 0)
{
const char p_label[6] = "Plus", m_label[6] = "Minus";
errlogPrintf("MDrive chain #%d, motor #%d %s LS not configured.\n",
card_index, motor_index,
(confptr->minusLS == 0) ? m_label : p_label);
}
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -692,8 +692,8 @@ static int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "MDrive_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(OK);
}
motorApp/MicosSrc/drvMicos.cc
+56 -57
View File
@@ -14,9 +14,9 @@
* .04 07-12-2004 rls Converted from MPF to asyn.
* .05 09-20-2004 rls support for 32axes/controller.
* .08 12-16-2004 rls - asyn R4.0 support.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
*/
@@ -29,7 +29,7 @@
#define WAIT 1
#define COMM_TIMEOUT 2 /* Command timeout in seconds. */
#define COMM_TIMEOUT 2 /* Command timeout in seconds. */
#define BUFF_SIZE 100 /* Maximum length of string to/from Micos */
@@ -42,10 +42,10 @@ struct mess_queue
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvMicosDebug) printf(f, ## args);}
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= drvMicosDebug) printf(f, ## args);}
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -56,10 +56,10 @@ extern "C" {epicsExportAddress(int, drvMicosDebug);}
/* Debugging notes:
* drvMicosDebug == 0 No debugging information is printed
* drvMicosDebug >= 1 Warning information is printed
* drvMicosDebug >= 2 Time-stamped messages are printed for each string
* drvMicosDebug >= 2 Time-stamped messages are printed for each string
* sent to and received from the controller
* drvMicosDebug >= 3 Additional debugging messages
*/
*/
volatile int Micos_num_cards = 0;
volatile int Micos_num_axis = 0;
@@ -224,22 +224,22 @@ static int set_status(int card, int signal)
/* check limits */
status.Bits.RA_PLUS_LS = status.Bits.RA_MINUS_LS = 0;
if ((bytes[5] & 0x04) & (bytes[3] & 0x04)) { /* if +lim AND pos move */
status.Bits.RA_PLUS_LS = 1;
ls_active = true;
status.Bits.RA_PLUS_LS = 1;
ls_active = true;
}
if ((bytes[5] & 0x01) & !(bytes[3] & 0x04)) { /* if -lim AND neg move */
status.Bits.RA_MINUS_LS = 1;
ls_active = true;
status.Bits.RA_MINUS_LS = 1;
ls_active = true;
}
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_POSITION = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_HOME = 0;
if ((bytes[3] & 0x08) | (bytes[3] & 0x40)) {
printf("drvMicos: set_status: EA_SLIP_STALL = 1, %ld\n", bytes[3]);
status.Bits.EA_SLIP_STALL = 1;
status.Bits.EA_SLIP_STALL = 1;
}
/* Request the position of this motor */
@@ -255,12 +255,12 @@ static int set_status(int card, int signal)
/* derive direction information */
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
status.Bits.RA_DIRECTION = (bytes[3] & 0x04) ? 1 : 0;
status.Bits.RA_DIRECTION = (bytes[3] & 0x04) ? 1 : 0;
motor_info->position = motorData;
motor_info->encoder_position = motorData;
motor_info->no_motion_count = 0;
@@ -275,11 +275,11 @@ static int set_status(int card, int signal)
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
@@ -301,7 +301,7 @@ static RTN_STATUS send_mess(int card, const char *com, char *name)
{
struct MicosController *cntrl;
int size;
int nwrite;
size_t nwrite;
size = strlen(com);
@@ -318,8 +318,7 @@ static RTN_STATUS send_mess(int card, const char *com, char *name)
Debug(2, "send_mess: sending message to card %d, message=%s\n", card, buff);
cntrl = (struct MicosController *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT,
&nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT, &nwrite);
return (OK);
}
@@ -332,7 +331,7 @@ static RTN_STATUS send_mess(int card, const char *com, char *name)
static int recv_mess(int card, char *com, int flag)
{
struct MicosController *cntrl;
int nread = 0;
size_t nread = 0;
asynStatus status = asynError;
int eomReason;
@@ -347,23 +346,23 @@ static int recv_mess(int card, char *com, int flag)
Debug(3, "recv_mess entry: card %d, flag=%d\n", card, flag);
if (flag == FLUSH)
pasynOctetSyncIO->flush(cntrl->pasynUser);
pasynOctetSyncIO->flush(cntrl->pasynUser);
else
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
COMM_TIMEOUT, &nread, &eomReason);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
COMM_TIMEOUT, &nread, &eomReason);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
if (flag != FLUSH)
{
Debug(1, "recv_mess: card %d ERROR: no response\n", card);
}
com[0] = '\0';
nread = 0;
if (flag != FLUSH)
{
Debug(1, "recv_mess: card %d ERROR: no response\n", card);
}
}
else
{
Debug(2, "recv_mess: card %d, message = \"%s\"\n", card, com);
Debug(2, "recv_mess: card %d, message = \"%s\"\n", card, com);
}
return (nread);
}
@@ -393,9 +392,9 @@ MicosSetup(int num_cards, /* maximum number of "controllers" in system */
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
* Allocate space for motor_state structure pointers. Note this must be done
@@ -418,8 +417,8 @@ MicosSetup(int num_cards, /* maximum number of "controllers" in system */
/* MicosConfig() */
/*****************************************************/
RTN_STATUS
MicosConfig(int card, /* "controller" being configured */
const char *name) /* asyn server task name */
MicosConfig(int card, /* "controller" being configured */
const char *name) /* asyn server task name */
{
struct MicosController *cntrl;
@@ -428,7 +427,7 @@ MicosConfig(int card, /* "controller" being configured */
motor_state[card] = (struct controller *) malloc(sizeof(struct controller));
motor_state[card]->DevicePrivate = malloc(sizeof(struct MicosController));
cntrl = (struct MicosController *) motor_state[card]->DevicePrivate;
strcpy(cntrl->asyn_port, name);
return(OK);
@@ -477,17 +476,17 @@ static int motor_init()
/* Initialize communications channel */
errind = false;
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
{
int retry = 0;
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
/* Each "controller" can have max 16 axes. */
total_axis = Micos_num_axis;
@@ -529,12 +528,12 @@ static int motor_init()
sprintf(buff, "%c%def", CTLA, motor_index);
send_mess(card_index, buff, 0);
/* Don't turn on motor power, too dangerous */
/*sprintf(buff,"#%02dW=1", motor_index); */
/*sprintf(buff,"#%02dW=1", motor_index); */
/* send_mess(card_index, buff, 0); */
/* Stop motor */
sprintf(buff,"%c%dab1", CTLA, motor_index);
send_mess(card_index, buff, 0);
/* recv_mess(card_index, buff, WAIT); Throw away response */
/* recv_mess(card_index, buff, WAIT); Throw away response */
strcpy(brdptr->ident, "MICOS");
motor_info->status.All = 0;
@@ -543,9 +542,9 @@ static int motor_init()
motor_info->position = 0;
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
set_status(card_index, motor_index); /* Read status of each motor */
}
@@ -556,7 +555,7 @@ static int motor_init()
}
Debug(3, "motor_init: spawning motor task\n");
any_motor_in_motion = 0;
mess_queue.head = (struct mess_node *) NULL;
@@ -566,8 +565,8 @@ static int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "tMicos", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return (0);
}
motorApp/MicroMoSrc/drvMVP2001.cc
+281 -282
View File
@@ -1,11 +1,11 @@
/*
FILENAME... drvMVP2001.cc
USAGE... Motor record driver level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
FILENAME... drvMVP2001.cc
USAGE... Motor record driver level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
Version: $Revision: 1.7 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-03-30 19:07:09 $
Version: $Revision: 1.8 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:52:29 $
*/
/*
@@ -48,7 +48,7 @@ Last Modified: $Date: 2005-03-30 19:07:09 $
* documentation and/or other materials provided with the distribution.
*
*
* Neither the names of UNICAT, Frederick Seitz Materials Research
* Neither the names of UNICAT, Frederick Seitz Materials Research
* Laboratory, University of Illinois at Urbana-Champaign,
* nor the names of its contributors may be used to endorse or promote
* products derived from this Software without specific prior written
@@ -66,58 +66,58 @@ Last Modified: $Date: 2005-03-30 19:07:09 $
*
* Modification Log:
* -----------------
* .01 08/27/02 kmp copied from drvIM483PL.c (rev 1.7, mod .03) and
* customized for the MVP2001.
* .02 08/27/02 kmp changed message construction to allow for addresses
* larger than 9.
* .03 09/06/02 kmp added an extra loop to motor_init() that sends the HO
* command a second time to ensure the position is set to
* zero. Previously, saved positions would not be loaded
* if the controller was power-cycled.
* .01 08/27/02 kmp copied from drvIM483PL.c (rev 1.7, mod .03) and
* customized for the MVP2001.
* .02 08/27/02 kmp changed message construction to allow for addresses
* larger than 9.
* .03 09/06/02 kmp added an extra loop to motor_init() that sends the HO
* command a second time to ensure the position is set to
* zero. Previously, saved positions would not be loaded
* if the controller was power-cycled.
* .04 02/06/04 rls Eliminate erroneous "Motor motion timeout ERROR".
* .05 02/13/04 rls port to R3.14.x
* .06 07/12/04 rls Converted from MPF to asyn.
* .05 02/13/04 rls port to R3.14.x
* .06 07/12/04 rls Converted from MPF to asyn.
* .07 12/16/04 rls - asyn R4.0 support.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - retry on initial communication.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - retry on initial communication.
*
*/
/*
DESIGN LIMITATIONS...
1 - Like all controllers, the MVP2001 must be powered-on when EPICS is first
booted up.
booted up.
2 - The MVP2001 cannot be power cycled while EPICS is up and running. The
consequences are permanent communication loss with the MVP2001 until
EPICS is rebooted.
3 - Translation between the MVP2001 and the ACCL/BACC fields is not obvious.
consequences are permanent communication loss with the MVP2001 until
EPICS is rebooted.
3 - Translation between the MVP2001 and the ACCL/BACC fields is not obvious.
*/
/*
MORE DESIGN LIMITATIONS
1 - For the most part the standard terminology (card-signal-axis) has been
used here so that this code resembles other drivers. Unfortunately,
the terminology is not the best for this controller. The MVP2001 is a
single-axis, RS-485-daisy-chainable, DC controller. The following
equations succinctly illustrate the relationship between the physical
setup and the standard terminology:
card = chain of MVP2001 controllers
signal = axis = one of the MVP2001 controllers on a chain
2 - Strtol and strtoul have been switched in the vxWorks that KMP used
at the time that this was being written. If your vxWorks functions
behave correctly, then they will have to be switched in the code.
used here so that this code resembles other drivers. Unfortunately,
the terminology is not the best for this controller. The MVP2001 is a
single-axis, RS-485-daisy-chainable, DC controller. The following
equations succinctly illustrate the relationship between the physical
setup and the standard terminology:
card = chain of MVP2001 controllers
signal = axis = one of the MVP2001 controllers on a chain
2 - Strtol and strtoul have been switched in the vxWorks that KMP used
at the time that this was being written. If your vxWorks functions
behave correctly, then they will have to be switched in the code.
3 - Factors that currently limit the number of controllers on one chain:
A. MVP2001 addresses
The MVP2001 can have an address of 1-64 for serial communication.
B. RS-485 communication degradation
There is a practical limit to how many controllers can be on one chain
C. The motor_info array of the controller structure in motordrvCom.h
For a chain to work correctly, there needs to be one element in the
motor_info array for every controller on the chain. The limit is
set by the constant MAX_AXIS, which is defined in motor.h. The end
result is that the number of controllers is limited by the motor
record. The current maximum number of controllers is 10.
A. MVP2001 addresses
The MVP2001 can have an address of 1-64 for serial communication.
B. RS-485 communication degradation
There is a practical limit to how many controllers can be on one chain
C. The motor_info array of the controller structure in motordrvCom.h
For a chain to work correctly, there needs to be one element in the
motor_info array for every controller on the chain. The limit is
set by the constant MAX_AXIS, which is defined in motor.h. The end
result is that the number of controllers is limited by the motor
record. The current maximum number of controllers is 10.
*/
#include <string.h>
@@ -127,15 +127,15 @@ MORE DESIGN LIMITATIONS
#include "drvMVP2001.h"
#include "epicsExport.h"
#define MVP2001_NUM_CARDS 8
#define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */
#define MVP2001_NUM_CARDS 8
#define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvMVP2001debug) printf(f,## args); }
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvMVP2001debug) printf(f,## args); }
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -147,7 +147,7 @@ extern "C" {epicsExportAddress(int, drvMVP2001debug);}
int MVP2001_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
@@ -203,23 +203,23 @@ static long report(int level)
int card;
if (MVP2001_num_cards <=0)
printf(" No MVP2001 CHAINS configured.\n");
printf(" No MVP2001 CHAINS configured.\n");
else
{
for (card = 0; card < MVP2001_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < MVP2001_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" MVP2001 controller chain #%d connection failed.\n", card);
else
{
struct MVPcontroller *cntrl;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
printf(" MVP2001 controller chain #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
if (brdptr == NULL)
printf(" MVP2001 controller chain #%d connection failed.\n", card);
else
{
struct MVPcontroller *cntrl;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
printf(" MVP2001 controller chain #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
}
return (0);
}
@@ -229,12 +229,12 @@ static long init()
{
/* initialize all hardware and software */
motor_init();
/* Check for setup */
if (MVP2001_num_cards <= 0)
{
Debug(1, "init(): MVP2001 driver disabled. MVP2001Setup() missing \
from startup script.\n");
Debug(1, "init(): MVP2001 driver disabled. MVP2001Setup() missing \
from startup script.\n");
}
return ((long) 0);
@@ -247,35 +247,35 @@ 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 *
* *
* *
* *
* 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 *
* *
* *
********************************************************************************/
/*
/*
* When the motor record calls set_status it passes it card and signal values
* that are found in the mvpMotors template (or the OUT field of the M.R.)
*/
@@ -306,25 +306,25 @@ static int set_status(int card, int signal)
rtn_state = recv_mess(card, buff, 1);
if (rtn_state > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
/*
@@ -342,53 +342,53 @@ static int set_status(int card, int signal)
send_mess(card, buff, (char) NULL);
recv_mess(card, buff, 1);
/*
/*
* Parse motor position
* Position string format: 0001 FFFFFFFF
* Skip to position substring for this motor, convert from hex to int
*/
strncat(positionStr, &buff[5], 8);
motorData = (epicsInt32) strtoul(positionStr, NULL, 16);
buff[0] = '\0';
buff[0] = '\0';
/*
* Set direction by comparing positions since the MVP2001
* Set direction by comparing positions since the MVP2001
* does not have a direction bit.
*/
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
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;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
/* Set limit switch error indicators. */
if (mstat.Bits.plusLS == false)
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_PLUS_LS = 0;
else
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
if (mstat.Bits.minusLS == false)
status.Bits.RA_MINUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
else
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
/* The MVP2001 doesn't have a home feature */
@@ -398,19 +398,19 @@ static int set_status(int card, int signal)
status.Bits.EA_POSITION = 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_HOME = 0;
if (motor_state[card]->motor_info[signal].encoder_present == NO)
motor_info->encoder_position = 0;
motor_info->encoder_position = 0;
else
{
/*
* There is not a seperate call for "encoder_position" as every call
* for the position of the DC motor reads the encoder.
*/
motor_info->encoder_position = motorData;
/*
* There is not a seperate call for "encoder_position" as every call
* for the position of the DC motor reads the encoder.
*/
motor_info->encoder_position = motorData;
}
status.Bits.RA_PROBLEM = 0;
@@ -421,18 +421,18 @@ static int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -442,36 +442,35 @@ exit:
/*****************************************************/
/* send a message to the MVP2001 board */
/* send_mess() */
/* send a message to the MVP2001 board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
struct MVPcontroller *cntrl;
int size;
int nwrite;
size_t nwrite;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvMVP2001.c:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvMVP2001.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvMVP2001.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvMVP2001.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
Debug(2, "send_mess(): message = %s\n", com);
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT,
&nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT, &nwrite);
return(OK);
}
@@ -479,41 +478,41 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
/*****************************************************/
/* receive a message from the MVP2001 board */
/* recv_mess() */
/* recv_mess() */
/*****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct MVPcontroller *cntrl;
char temp[BUFF_SIZE];
int nread = 0;
size_t nread = 0, lenTemp = 0;
asynStatus status = asynError;
int timeout;
int flush = 0;
int len=0, lenTemp=0;
int len=0;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return (-1);
return (-1);
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
if (flag == FLUSH)
timeout = 0;
timeout = 0;
else
timeout = COMM_TIMEOUT;
timeout = COMM_TIMEOUT;
len = pasynOctetSyncIO->read(cntrl->pasynUser, temp, BUFF_SIZE,
COMM_TIMEOUT, &lenTemp, &eomReason);
COMM_TIMEOUT, &lenTemp, &eomReason);
len = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
COMM_TIMEOUT, &nread, &eomReason);
COMM_TIMEOUT, &nread, &eomReason);
Debug(5, "bytes: 1st call: %d\t2nd call: %d\n", lenTemp, nread);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -526,44 +525,44 @@ static int recv_mess(int card, char *com, int flag)
/* MVP2001Setup() */
/*****************************************************/
RTN_STATUS
MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
{
if (num_cards < 1 || num_cards > MVP2001_NUM_CARDS)
MVP2001_num_cards = MVP2001_NUM_CARDS;
MVP2001_num_cards = MVP2001_NUM_CARDS;
else
MVP2001_num_cards = num_cards;
MVP2001_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before MVP2001Config 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 **) calloc(MVP2001_num_cards,
sizeof(struct controller *));
sizeof(struct controller *));
return(OK);
}
/*******************************************************
* Configure a CHAIN of controllers *
* *
* Configure a CHAIN of controllers *
* *
* Note: Addresses of controllers on a chain must *
* begin at 1 and follow sequentially. *
* *
* MVP2001Config() *
* begin at 1 and follow sequentially. *
* *
* MVP2001Config() *
********************************************************/
RTN_STATUS
MVP2001Config(int card, /* CHAIN being configured */
const char *name) /* asyn server task name */
MVP2001Config(int card, /* CHAIN being configured */
const char *name) /* asyn server task name */
{
struct MVPcontroller *cntrl;
@@ -580,8 +579,8 @@ MVP2001Config(int card, /* CHAIN being configured */
/*****************************************************/
/* initialize all software and hardware */
/* motor_init() */
/* initialize all software and hardware */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
@@ -597,124 +596,124 @@ static int motor_init()
buff[0] = limitStr[0] = '\0';
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (MVP2001_num_cards <= 0)
return (ERROR);
return (ERROR);
for (card_index = 0; card_index < MVP2001_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->cmnd_response = false; /* The MVP doesn't respond to every command */
total_cards = card_index + 1;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
if (!motor_state[card_index])
continue;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
brdptr = motor_state[card_index];
brdptr->ident[0] = (char) NULL; /* No controller identification message. */
brdptr->cmnd_response = false; /* The MVP doesn't respond to every command */
total_cards = card_index + 1;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
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 board, see if it exists */
for (total_axis = 0; total_axis < MAX_AXIS; total_axis++)
{
int retry = 0;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
/* flush any junk at input port - should not be any data available */
do
recv_mess(card_index, buff, FLUSH);
while (strlen(buff) != 0);
if (success_rtn == asynSuccess)
{
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
do
{
sprintf(buff, "%d ST", (total_axis + 1));
send_mess(card_index, buff, (char) NULL);
status = recv_mess(card_index, buff, 1);
retry++;
} while (status <= 0 && retry < 3);
if (status <= 0)
break;
}
brdptr->total_axis = total_axis;
Debug(5, "brdptr->total_axis (number of controllers on chain %d) = %d\n", card_index, brdptr->total_axis);
}
/* Send a message to the board, see if it exists */
for (total_axis = 0; total_axis < MAX_AXIS; total_axis++)
{
int retry = 0;
if (success_rtn == asynSuccess && total_axis > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
/* flush any junk at input port - should not be any data available */
do
recv_mess(card_index, buff, FLUSH);
while (strlen(buff) != 0);
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
do
{
sprintf(buff, "%d ST", (total_axis + 1));
send_mess(card_index, buff, (char) NULL);
status = recv_mess(card_index, buff, 1);
retry++;
} while (status <= 0 && retry < 3);
if (status <= 0)
break;
}
brdptr->total_axis = total_axis;
Debug(5, "brdptr->total_axis (number of controllers on chain %d) = %d\n", card_index, brdptr->total_axis);
}
/* stop and initialize the controller */
sprintf(buff, "%d V 0", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
sprintf(buff, "%d HO", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
sprintf(buff, "%d EN", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
if (success_rtn == asynSuccess && total_axis > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
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;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
/* no encoder support for correct DC controller interaction */
motor_info->encoder_present = NO;
motor_info->status.Bits.EA_PRESENT = 0;
/* stop and initialize the controller */
sprintf(buff, "%d V 0", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
sprintf(buff, "%d HO", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
sprintf(buff, "%d EN", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
/* MVP2001 has PID capabilities */
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
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;
limitStr[0] = '\0';
/* Determine low limit */
sprintf(buff, "%d LL -", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
strncat(limitStr, &buff[5], 8);
motor_info->low_limit = (epicsInt32) strtoul(limitStr, NULL, 16);
/* no encoder support for correct DC controller interaction */
motor_info->encoder_present = NO;
motor_info->status.Bits.EA_PRESENT = 0;
limitStr[0] = '\0';
/* Determine high limit */
sprintf(buff, "%d LL", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
strncat(limitStr, &buff[5], 8);
motor_info->high_limit = (epicsInt32) strtoul(limitStr, NULL, 16);
}
/* MVP2001 has PID capabilities */
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
/*
* Ensure that the position is correctly set to zero so that auto_sr
* loads the saved positions. The task delay is necessary because
* sending the HO command too soon after the EN command results in
* reading back a position within ten encoder pulses away from zero.
*/
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
epicsThreadSleep(0.2);
limitStr[0] = '\0';
/* Determine low limit */
sprintf(buff, "%d LL -", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
strncat(limitStr, &buff[5], 8);
motor_info->low_limit = (epicsInt32) strtoul(limitStr, NULL, 16);
sprintf(buff, "%d HO", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
limitStr[0] = '\0';
/* Determine high limit */
sprintf(buff, "%d LL", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
strncat(limitStr, &buff[5], 8);
motor_info->high_limit = (epicsInt32) strtoul(limitStr, NULL, 16);
}
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
/*
* Ensure that the position is correctly set to zero so that auto_sr
* loads the saved positions. The task delay is necessary because
* sending the HO command too soon after the EN command results in
* reading back a position within ten encoder pulses away from zero.
*/
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
epicsThreadSleep(0.2);
sprintf(buff, "%d HO", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -726,9 +725,9 @@ static int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "MVP2001_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(0);
}
motorApp/NewportSrc/drvESP300.cc
+229 -230
View File
@@ -1,16 +1,16 @@
/*
FILENAME... drvESP300.cc
USAGE... Motor record driver level support for Newport ESP300.
FILENAME... drvESP300.cc
USAGE... Motor record driver level support for Newport ESP300.
Version: $Revision: 1.18 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-03-30 19:10:47 $
Version: $Revision: 1.19 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:52:47 $
*/
/*
* Original Author: Ron Sluiter
* Date: 02/19/03
* Current Author: Ron Sluiter
* Current Author: Ron Sluiter
*
* Experimental Physics and Industrial Control System (EPICS)
*
@@ -22,21 +22,21 @@ Last Modified: $Date: 2005-03-30 19:10:47 $
* 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
* 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
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
* .01 02-19-03 rls copied from drvMM3000
* .02 05-23-03 rls Converted to R3.14.x.
* .02 05-23-03 rls Converted to R3.14.x.
* .03 10-28-03 rls initialize "drive_resolution".
* .04 02-03-04 rls Eliminate erroneous "Motor motion timeout ERROR".
* .05 07/09/04 rls removed unused <driver>Setup() argument.
@@ -44,9 +44,9 @@ Last Modified: $Date: 2005-03-30 19:10:47 $
* .07 09/21/04 rls support for 32axes/controller.
* .08 12/21/04 rls - retry on initial communication.
* - using asyn's set[Out/In]putEos().
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
*/
@@ -60,21 +60,21 @@ Last Modified: $Date: 2005-03-30 19:10:47 $
#include "epicsExport.h"
#define READ_POSITION "%.2dTP"
#define STOP_AXIS "%.2dST"
#define STOP_AXIS "%.2dST"
#define GET_IDENT "VE?"
#define ESP300_NUM_CARDS 4
#define ESP300_NUM_CARDS 4
#define BUFF_SIZE 100 /* Maximum length of string to/from ESP300 */
/* The ESP300 does not respond for 2 to 5 seconds after hitting a travel limit. */
#define SERIAL_TIMEOUT 5.0 /* Command timeout in sec. */
#define SERIAL_TIMEOUT 5.0 /* Command timeout in sec. */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvESP300debug) printf(f,## args); }
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvESP300debug) printf(f,## args); }
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -85,7 +85,7 @@ extern "C" {epicsExportAddress(int, drvESP300debug);}
int ESP300_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
@@ -146,25 +146,25 @@ static long report(int level)
int card;
if (ESP300_num_cards <=0)
printf(" No ESP300 controllers configured.\n");
printf(" No ESP300 controllers configured.\n");
else
{
for (card = 0; card < ESP300_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < ESP300_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" ESP300 controller %d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
if (brdptr == NULL)
printf(" ESP300 controller %d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" ESP300 controller %d port=%s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
}
}
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" ESP300 controller %d port=%s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
}
}
}
return(OK);
}
@@ -172,7 +172,7 @@ static long report(int level)
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
@@ -181,7 +181,7 @@ static long init()
/* Check for setup */
if (ESP300_num_cards <= 0)
{
Debug(1, "init(): ESP300 driver disabled. ESP300Setup() missing from startup script.\n");
Debug(1, "init(): ESP300 driver disabled. ESP300Setup() missing from startup script.\n");
}
return((long) 0);
}
@@ -222,49 +222,49 @@ static int set_status(int card, int signal)
if (charcnt == 1 && (inbuff[0] == '0' || inbuff[0] == '1'))
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
status.Bits.RA_DONE = (inbuff[0] == '1') ? 1 : 0;
/* Get motor position. */
sprintf(outbuff, READ_POSITION, signal + 1);
send_mess(card, outbuff, (char) NULL);
charcnt = recv_mess(card, inbuff, 1);
motorData = atof(inbuff) / cntrl->drive_resolution[signal];
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
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;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
@@ -276,29 +276,29 @@ static int set_status(int card, int signal)
cptr = strchr(inbuff, 'H');
if (cptr == NULL)
{
Debug(2, "set_status(): PH error = %s\n", inbuff);
rtn_state = 1;
goto exit;
Debug(2, "set_status(): PH error = %s\n", inbuff);
rtn_state = 1;
goto exit;
}
mstatus = strtol(inbuff, &cptr, 16);
/* Set Travel limit switch status bits. */
if (((mstatus >> signal) & 0x01) == false)
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_PLUS_LS = 0;
else
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
if (((mstatus >> (signal + 8)) & 0x01) == false)
status.Bits.RA_MINUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
else
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
/* Set home switch status. */
@@ -317,10 +317,10 @@ static int set_status(int card, int signal)
status.Bits.EA_POSITION = (power == true) ? 1 : 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.RA_PROBLEM = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
/* NEEDS WORK */
@@ -328,18 +328,18 @@ static int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(outbuff, nodeptr->postmsgptr);
send_mess(card, outbuff, (char) NULL);
nodeptr->postmsgptr = NULL;
strcpy(outbuff, nodeptr->postmsgptr);
send_mess(card, outbuff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -349,44 +349,43 @@ exit:
/*****************************************************/
/* send a message to the ESP300 board */
/* send_mess() */
/* send a message to the ESP300 board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
struct MMcontroller *cntrl;
int size;
int nwrite;
size_t nwrite;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvESP300:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvESP300:send_mess(); message size violation.\n");
return(ERROR);
}
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvESP300:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvESP300:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
if (name != NULL)
{
errlogPrintf("drvESP300:send_mess() - invalid argument = %s\n", name);
return(ERROR);
errlogPrintf("drvESP300:send_mess() - invalid argument = %s\n", name);
return(ERROR);
}
Debug(2, "send_mess(): message = %s\n", com);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com),
SERIAL_TIMEOUT, &nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com), SERIAL_TIMEOUT, &nwrite);
return(OK);
}
@@ -395,42 +394,42 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
* 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.
* 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...
* Initialize:
* - receive timeout to zero
* - received string length to zero.
* - receive timeout to zero
* - received string length to zero.
*
* IF controller card does not exist.
* ERROR RETURN.
* ERROR RETURN.
* ENDIF
*
* SWITCH on port type.
* CASE port type is GPIB.
* BREAK.
* CASE port type is RS232.
* IF input "flag" indicates NOT flushing the input buffer.
* Set receive timeout nonzero.
* ENDIF
* Call pasynOctetSyncIO->read().
* CASE port type is GPIB.
* BREAK.
* CASE port type is RS232.
* IF input "flag" indicates NOT flushing the input buffer.
* Set receive timeout nonzero.
* ENDIF
* Call pasynOctetSyncIO->read().
*
* NOTE: The ESP300 sometimes responds to an MS command with an error
* message (see ESP300 User's Manual Appendix A). This is an
* unconfirmed ESP300 bug. Retry read if this is a Hard Travel
* limit switch error. This effectively flushes the error message.
* NOTE: The ESP300 sometimes responds to an MS command with an error
* message (see ESP300 User's Manual Appendix A). This is an
* unconfirmed ESP300 bug. Retry read if this is a Hard Travel
* limit switch error. This effectively flushes the error message.
*
* IF input "com" buffer length is > 3 characters, AND, the 1st
* character is an "E" (Maybe this an unsolicited error
* message response?).
* Call pasynOctetSyncIO->read().
* ENDIF
* BREAK
* IF input "com" buffer length is > 3 characters, AND, the 1st
* character is an "E" (Maybe this an unsolicited error
* message response?).
* Call pasynOctetSyncIO->read().
* ENDIF
* BREAK
* ENDSWITCH
*
*
* NORMAL RETURN.
*/
@@ -439,39 +438,39 @@ static int recv_mess(int card, char *com, int flag)
struct MMcontroller *cntrl;
double timeout = 0.;
int flush = 1;
int nread = 0;
size_t nread = 0;
asynStatus status;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
if (flag != FLUSH) {
flush = 0;
timeout = SERIAL_TIMEOUT;
timeout = SERIAL_TIMEOUT;
}
if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
timeout, &nread, &eomReason);
if (nread > 3 && com[0] == 'E')
{
long error;
long error;
error = strtol(&com[1], NULL, 0);
if (error >= 35 && error <= 42) {
error = strtol(&com[1], NULL, 0);
if (error >= 35 && error <= 42) {
if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
timeout, &nread, &eomReason);
}
}
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -484,33 +483,33 @@ static int recv_mess(int card, char *com, int flag)
/* ESP300Setup() */
/*****************************************************/
RTN_STATUS
ESP300Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
ESP300Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > ESP300_NUM_CARDS)
ESP300_num_cards = ESP300_NUM_CARDS;
ESP300_num_cards = ESP300_NUM_CARDS;
else
ESP300_num_cards = num_cards;
ESP300_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before ESP300Config 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(ESP300_num_cards *
sizeof(struct controller *));
sizeof(struct controller *));
for (itera = 0; itera < ESP300_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
@@ -521,7 +520,7 @@ ESP300Setup(int num_cards, /* maximum number of controllers in system. */
/* ESP300Config() */
/*****************************************************/
RTN_STATUS
ESP300Config(int card, /* card being configured */
ESP300Config(int card, /* card being configured */
const char *name, /* asyn port name */
int address) /* asyn address (GPIB) */
{
@@ -542,10 +541,10 @@ ESP300Config(int card, /* card being configured */
/*****************************************************/
/* initialize all software and hardware */
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
@@ -559,100 +558,100 @@ static int motor_init()
static const char output_terminator[] = "\r";
static const char input_terminator[] = "\r\n";
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (ESP300_num_cards <= 0)
return(ERROR);
return(ERROR);
for (card_index = 0; card_index < ESP300_num_cards; card_index++)
{
if (!motor_state[card_index])
continue;
if (!motor_state[card_index])
continue;
brdptr = motor_state[card_index];
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
brdptr = motor_state[card_index];
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
cntrl->asyn_address, &cntrl->pasynUser, NULL);
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
cntrl->asyn_address, &cntrl->pasynUser, NULL);
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
if (success_rtn == asynSuccess)
{
int retry = 0;
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
if (success_rtn == asynSuccess)
{
int retry = 0;
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
do
{
send_mess(card_index, GET_IDENT, (char) NULL);
status = recv_mess(card_index, buff, 1);
retry++;
/* Return value is length of response string */
} while (status == 0 && retry < 3);
}
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
strcpy(brdptr->ident, &buff[1]); /* Skip "\n" */
do
{
send_mess(card_index, GET_IDENT, (char) NULL);
status = recv_mess(card_index, buff, 1);
retry++;
/* Return value is length of response string */
} while (status == 0 && retry < 3);
}
send_mess(card_index, "ZU", (char) NULL);
recv_mess(card_index, buff, 1);
total_axis = buff[0] >> 4;
if (total_axis > 4)
{
Debug(2, "motor_init(): ZU = %s\n", buff);
total_axis = 4;
}
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
strcpy(brdptr->ident, &buff[1]); /* Skip "\n" */
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
sprintf(buff, STOP_AXIS, motor_index + 1); /* Stop motor */
send_mess(card_index, buff, (char) NULL);
/* Initialize. */
brdptr->motor_info[motor_index].motor_motion = NULL;
}
send_mess(card_index, "ZU", (char) NULL);
recv_mess(card_index, buff, 1);
total_axis = buff[0] >> 4;
if (total_axis > 4)
{
Debug(2, "motor_init(): ZU = %s\n", buff);
total_axis = 4;
}
brdptr->total_axis = total_axis;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
sprintf(buff, STOP_AXIS, motor_index + 1); /* Stop motor */
send_mess(card_index, buff, (char) NULL);
/* Initialize. */
brdptr->motor_info[motor_index].motor_motion = NULL;
}
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
brdptr->total_axis = total_axis;
/* Set axis resolution. */
sprintf(buff, "%.2dSU?", motor_index + 1);
send_mess(card_index, buff, 0);
recv_mess(card_index, buff, 1);
cntrl->drive_resolution[motor_index] = atof(&buff[0]);
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
motor_info->encoder_present = YES;
if (motor_info->encoder_present == YES)
{
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
/* Set axis resolution. */
sprintf(buff, "%.2dSU?", motor_index + 1);
send_mess(card_index, buff, 0);
recv_mess(card_index, buff, 1);
cntrl->drive_resolution[motor_index] = atof(&buff[0]);
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
motor_info->encoder_present = YES;
if (motor_info->encoder_present == YES)
{
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -664,8 +663,8 @@ static int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "ESP300_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(OK);
}
motorApp/NewportSrc/drvMM3000.cc
+263 -264
View File
@@ -1,16 +1,16 @@
/*
FILENAME... drvMM3000.cc
USAGE... Motor record driver level support for Newport MM3000.
FILENAME... drvMM3000.cc
USAGE... Motor record driver level support for Newport MM3000.
Version: $Revision: 1.17 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-03-30 19:10:48 $
Version: $Revision: 1.18 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:52:47 $
*/
/*
* Original Author: Mark Rivers
* Date: 10/20/97
* Current Author: Ron Sluiter
* Current Author: Ron Sluiter
*
* Experimental Physics and Industrial Control System (EPICS)
*
@@ -22,16 +22,16 @@ Last Modified: $Date: 2005-03-30 19:10:48 $
* 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
* 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
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
@@ -39,26 +39,26 @@ Last Modified: $Date: 2005-03-30 19:10:48 $
* .02 10-30-97 mlr Replaced driver calls with gpipIO functions
* .03 10-30-98 mlr Minor code cleanup, improved formatting
* .04 02-01-99 mlr Added temporary fix to delay reading motor positions at
* the end of a move.
* the end of a move.
* .05 04-18-00 rls MM3000 takes 2 to 5 seconds to respond to queries after
* hitting a hard travel limit. Adjusted GPIB and SERIAL
* timeouts accordingly. Deleted communication retries.
* Reworked travel limit processing so that direction
* status bit matches limit switch. Copied recv_mess()
* logic from drvMM4000.c. Use TPE command to determine
* if motor has an encoder.
* hitting a hard travel limit. Adjusted GPIB and SERIAL
* timeouts accordingly. Deleted communication retries.
* Reworked travel limit processing so that direction
* status bit matches limit switch. Copied recv_mess()
* logic from drvMM4000.c. Use TPE command to determine
* if motor has an encoder.
* .06 10/02/01 rls - allow one retry after a communication error.
* - use motor status response bit-field.
* .07 05-22-03 rls - Converted to R3.14.2.
* - use motor status response bit-field.
* .07 05-22-03 rls - Converted to R3.14.2.
* .08 10/23/03 rls - recv_mess() eats the controller error response, outputs
* an error message and retries.
* an error message and retries.
* .09 02/03/04 rls - Eliminate erroneous "Motor motion timeout ERROR".
* .10 07/09/04 rls - removed unused <driver>Setup() argument.
* .11 07/28/04 rls - "epicsExport" debug variable.
* .12 09/21/04 rls - support for 32axes/controller.
* .13 12/21/04 rls - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - make debug variables always available.
* extern "C" linkage.
* - make debug variables always available.
*
*/
@@ -89,18 +89,18 @@ Last Modified: $Date: 2005-03-30 19:10:48 $
#define M_MINUS_LIMIT 0x10
#define M_HOME_SIGNAL 0x20
#define MM3000_NUM_CARDS 4
#define MM3000_NUM_CARDS 4
#define BUFF_SIZE 100 /* Maximum length of string to/from MM3000 */
/* The MM3000 does not respond for 2 to 5 seconds after hitting a travel limit. */
#define SERIAL_TIMEOUT 5.0 /* Command timeout in sec. */
#define SERIAL_TIMEOUT 5.0 /* Command timeout in sec. */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvMM3000debug) printf(f,## args); }
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvMM3000debug) printf(f,## args); }
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -112,7 +112,7 @@ extern "C" {epicsExportAddress(int, drvMM3000debug);}
int MM3000_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/*----------------functions-----------------*/
STATIC int recv_mess(int, char *, int);
@@ -168,26 +168,26 @@ static long report(int level)
int card;
if (MM3000_num_cards <=0)
printf(" No MM3000 controllers configured.\n");
printf(" No MM3000 controllers configured.\n");
else
{
for (card = 0; card < MM3000_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < MM3000_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" MM3000 controller %d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
if (brdptr == NULL)
printf(" MM3000 controller %d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" MM3000 controller %d asyn port= %s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
break;
}
}
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" MM3000 controller %d asyn port= %s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
break;
}
}
}
return(OK);
}
@@ -195,7 +195,7 @@ static long report(int level)
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
@@ -204,7 +204,7 @@ static long init()
/* Check for setup */
if (MM3000_num_cards <= 0)
{
Debug(1, "init(): MM3000 driver disabled. MM3000Setup() missing from startup script.\n");
Debug(1, "init(): MM3000 driver disabled. MM3000Setup() missing from startup script.\n");
}
return((long) 0);
}
@@ -243,25 +243,25 @@ STATIC int set_status(int card, int signal)
charcnt = recv_mess(card, inbuff, 1);
if (charcnt > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
mstat.All = inbuff[0];
@@ -277,21 +277,21 @@ STATIC int set_status(int card, int signal)
/* Set Travel limit switch status bits. */
if (mstat.Bits.plustTL == false)
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_PLUS_LS = 0;
else
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
if (mstat.Bits.minusTL == false)
status.Bits.RA_MINUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
else
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
status.Bits.RA_HOME = (mstat.Bits.homels == false) ? 0 : 1;
@@ -299,34 +299,34 @@ STATIC int set_status(int card, int signal)
status.Bits.EA_POSITION = (mstat.Bits.NOT_power == false) ? 1 : 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
sprintf(outbuff, "%dTP", signal + 1);
send_mess(card, outbuff, (char) NULL);
charcnt = recv_mess(card, inbuff, 1);
if (charcnt > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
tok_save = NULL;
@@ -335,18 +335,18 @@ STATIC int set_status(int card, int signal)
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
motor_info->position = (epicsInt32) motorData;
if (motor_state[card]->motor_info[signal].encoder_present == YES)
motor_info->encoder_position = (epicsInt32) motorData;
else
motor_info->encoder_position = 0;
motor_info->position = (epicsInt32) motorData;
if (motor_state[card]->motor_info[signal].encoder_present == YES)
motor_info->encoder_position = (epicsInt32) motorData;
else
motor_info->encoder_position = 0;
motor_info->no_motion_count = 0;
motor_info->no_motion_count = 0;
}
status.Bits.RA_PROBLEM = 0;
@@ -357,18 +357,18 @@ STATIC int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(outbuff, nodeptr->postmsgptr);
send_mess(card, outbuff, (char) NULL);
nodeptr->postmsgptr = NULL;
strcpy(outbuff, nodeptr->postmsgptr);
send_mess(card, outbuff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -378,44 +378,43 @@ exit:
/*****************************************************/
/* send a message to the MM3000 board */
/* send_mess() */
/* send a message to the MM3000 board */
/* send_mess() */
/*****************************************************/
STATIC RTN_STATUS send_mess(int card, char const *com, char *name)
{
struct MMcontroller *cntrl;
int size;
int nwrite;
size_t nwrite;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvMM3000:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvMM3000:send_mess(); message size violation.\n");
return(ERROR);
}
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvMM3000:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvMM3000:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
if (name != NULL)
{
errlogPrintf("drvMM3000:send_mess() - invalid argument = %s\n", name);
return(ERROR);
errlogPrintf("drvMM3000:send_mess() - invalid argument = %s\n", name);
return(ERROR);
}
Debug(2, "send_mess(): message = %s\n", com);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com),
SERIAL_TIMEOUT, &nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com), SERIAL_TIMEOUT, &nwrite);
return(OK);
}
@@ -424,42 +423,42 @@ STATIC RTN_STATUS send_mess(int card, char const *com, char *name)
* 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.
* 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...
* Initialize:
* - receive timeout to zero
* - received string length to zero.
* - receive timeout to zero
* - received string length to zero.
*
* IF controller card does not exist.
* ERROR RETURN.
* ERROR RETURN.
* ENDIF
*
* SWITCH on port type.
* CASE port type is GPIB.
* BREAK.
* CASE port type is RS232.
* IF input "flag" indicates NOT flushing the input buffer.
* Set receive timeout nonzero.
* ENDIF
* Call pasynOctetSyncIO->read().
* CASE port type is GPIB.
* BREAK.
* CASE port type is RS232.
* IF input "flag" indicates NOT flushing the input buffer.
* Set receive timeout nonzero.
* ENDIF
* Call pasynOctetSyncIO->read().
*
* NOTE: The MM3000 sometimes responds to an MS command with an error
* message (see MM3000 User's Manual Appendix A). This is an
* unconfirmed MM3000 bug. Retry read if this is a Hard Travel
* limit switch error. This effectively flushes the error message.
* NOTE: The MM3000 sometimes responds to an MS command with an error
* message (see MM3000 User's Manual Appendix A). This is an
* unconfirmed MM3000 bug. Retry read if this is a Hard Travel
* limit switch error. This effectively flushes the error message.
*
* IF input "com" buffer length is > 3 characters, AND, the 1st
* character is an "E" (Maybe this an unsolicited error
* message response?).
* Call pasynOctetSyncIO->read().
* ENDIF
* BREAK
* IF input "com" buffer length is > 3 characters, AND, the 1st
* character is an "E" (Maybe this an unsolicited error
* message response?).
* Call pasynOctetSyncIO->read().
* ENDIF
* BREAK
* ENDSWITCH
*
*
* NORMAL RETURN.
*/
@@ -468,48 +467,48 @@ STATIC int recv_mess(int card, char *com, int flag)
struct MMcontroller *cntrl;
double timeout = 0.;
int flush = 1;
int nread = 0;
size_t nread = 0;
int eomReason;
asynStatus status;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
if (flag != FLUSH) {
flush = 0;
timeout = SERIAL_TIMEOUT;
timeout = SERIAL_TIMEOUT;
}
if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
timeout, &nread, &eomReason);
if (nread > 3 && com[0] == 'E')
{
long error;
long error;
error = strtol(&com[1], NULL, 0);
if (error >= 35 && error <= 42) {
error = strtol(&com[1], NULL, 0);
if (error >= 35 && error <= 42) {
if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
timeout, &nread, &eomReason);
}
}
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
else
{
/* Test for "system error" response. */
if (com[0] == 'E')
{
errPrintf( -1, __FILE__, __LINE__, "%s\n", com);
return(recv_mess(card, com, flag));
}
/* Test for "system error" response. */
if (com[0] == 'E')
{
errPrintf( -1, __FILE__, __LINE__, "%s\n", com);
return(recv_mess(card, com, flag));
}
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -522,33 +521,33 @@ STATIC int recv_mess(int card, char *com, int flag)
/* MM3000Setup() */
/*****************************************************/
RTN_STATUS
MM3000Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
MM3000Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > MM3000_NUM_CARDS)
MM3000_num_cards = MM3000_NUM_CARDS;
MM3000_num_cards = MM3000_NUM_CARDS;
else
MM3000_num_cards = num_cards;
MM3000_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before MM3000Config 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(MM3000_num_cards *
sizeof(struct controller *));
sizeof(struct controller *));
for (itera = 0; itera < MM3000_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
@@ -559,7 +558,7 @@ MM3000Setup(int num_cards, /* maximum number of controllers in system. */
/* MM3000Config() */
/*****************************************************/
RTN_STATUS
MM3000Config(int card, /* card being configured */
MM3000Config(int card, /* card being configured */
const char *port, /* asyn port name */
int address) /* asyn address (GPIB) */
{
@@ -580,10 +579,10 @@ MM3000Config(int card, /* card being configured */
/*****************************************************/
/* initialize all software and hardware */
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/* motor_init() */
/*****************************************************/
STATIC int motor_init()
{
@@ -597,115 +596,115 @@ STATIC int motor_init()
int status=0;
asynStatus success_rtn;
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (MM3000_num_cards <= 0)
return(ERROR);
return(ERROR);
tok_save = NULL;
for (card_index = 0; card_index < MM3000_num_cards; card_index++)
{
if (!motor_state[card_index])
continue;
if (!motor_state[card_index])
continue;
brdptr = motor_state[card_index];
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
brdptr = motor_state[card_index];
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
cntrl->asyn_address, &cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
{
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
send_mess(card_index, GET_IDENT, (char) NULL);
status = recv_mess(card_index, axis_pos, 1);
/* Return value is length of response string */
}
if (success_rtn == asynSuccess)
{
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
send_mess(card_index, STOP_ALL, (char) NULL); /* Stop all motors */
send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */
recv_mess(card_index, buff, 1);
strncpy(brdptr->ident, &buff[0], MAX_IDENT_LEN); /* Skip "VE" */
send_mess(card_index, GET_IDENT, (char) NULL);
status = recv_mess(card_index, axis_pos, 1);
/* Return value is length of response string */
}
send_mess(card_index, "RC", (char) NULL);
recv_mess(card_index, buff, 1);
bufptr = strtok_r(buff, "=", &tok_save);
bufptr = strtok_r(NULL, " ", &tok_save);
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
send_mess(card_index, STOP_ALL, (char) NULL); /* Stop all motors */
send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */
recv_mess(card_index, buff, 1);
strncpy(brdptr->ident, &buff[0], MAX_IDENT_LEN); /* Skip "VE" */
/* The return string will tell us how many axes this controller has */
for (total_axis = 0; bufptr != NULL; total_axis++)
{
if (strcmp(bufptr, "unused") == 0)
{
cntrl->type[total_axis] = UNUSED;
bufptr = NULL;
}
else
{
if (strcmp(bufptr, "stepper1.5M") == 0)
cntrl->type[total_axis] = STEPPER;
else if (strcmp(bufptr, "dc") == 0)
cntrl->type[total_axis] = DC;
else
errlogPrintf("drvMM3000:motor_init() - invalid RC response = %s\n",
bufptr);
send_mess(card_index, "RC", (char) NULL);
recv_mess(card_index, buff, 1);
bufptr = strtok_r(buff, "=", &tok_save);
bufptr = strtok_r(NULL, " ", &tok_save);
bufptr = strtok_r(NULL, "=", &tok_save);
bufptr = strtok_r(NULL, " ", &tok_save);
}
/* The return string will tell us how many axes this controller has */
for (total_axis = 0; bufptr != NULL; total_axis++)
{
if (strcmp(bufptr, "unused") == 0)
{
cntrl->type[total_axis] = UNUSED;
bufptr = NULL;
}
else
{
if (strcmp(bufptr, "stepper1.5M") == 0)
cntrl->type[total_axis] = STEPPER;
else if (strcmp(bufptr, "dc") == 0)
cntrl->type[total_axis] = DC;
else
errlogPrintf("drvMM3000:motor_init() - invalid RC response = %s\n",
bufptr);
/* Initialize. */
brdptr->motor_info[total_axis].motor_motion = NULL;
}
bufptr = strtok_r(NULL, "=", &tok_save);
bufptr = strtok_r(NULL, " ", &tok_save);
}
brdptr->total_axis = total_axis;
/* Initialize. */
brdptr->motor_info[total_axis].motor_motion = NULL;
}
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
brdptr->total_axis = total_axis;
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
if (cntrl->type[total_axis] == DC)
motor_info->encoder_present = YES;
else
{
sprintf(buff, "%dTPE", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
if (strcmp(buff, "E01") == 0)
motor_info->encoder_present = NO;
else
motor_info->encoder_present = YES;
}
if (motor_info->encoder_present == YES)
{
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
if (cntrl->type[total_axis] == DC)
motor_info->encoder_present = YES;
else
{
sprintf(buff, "%dTPE", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
if (strcmp(buff, "E01") == 0)
motor_info->encoder_present = NO;
else
motor_info->encoder_present = YES;
}
if (motor_info->encoder_present == YES)
{
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -717,8 +716,8 @@ STATIC int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "MM3000_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(OK);
}
motorApp/NewportSrc/drvMM4000.cc
+279 -280
View File
@@ -1,16 +1,16 @@
/*
FILENAME... drvMM4000.cc
USAGE... Motor record driver level support for Newport MM4000.
FILENAME... drvMM4000.cc
USAGE... Motor record driver level support for Newport MM4000.
Version: $Revision: 1.17 $
Modified By: $Author: rivers $
Last Modified: $Date: 2005-03-30 00:48:40 $
Version: $Revision: 1.18 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:52:47 $
*/
/*
* Original Author: Mark Rivers
* Date: 10/20/97
* Current Author: Ron Sluiter
* Current Author: Ron Sluiter
*
* Experimental Physics and Industrial Control System (EPICS)
*
@@ -22,16 +22,16 @@ Last Modified: $Date: 2005-03-30 00:48:40 $
* 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
* 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
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
@@ -39,14 +39,14 @@ Last Modified: $Date: 2005-03-30 00:48:40 $
* .02 10-30-97 mlr Replaced driver calls with gpipIO functions
* .03 10-30-98 mlr Minor code cleanup, improved formatting
* .04 02-01-99 mlr Added temporary fix to delay reading motor positions at
* the end of a move.
* the end of a move.
* .05 10-13-99 rls modified for standardized motor record.
* .06 09-17-01 rls
* - created a bit-field for motor status response.
* - start_status() allows one retry after a communication error.
* - set_status() sets RA_PROBLEM along with CNTRL_COMM_ERR to terminate
* node.
* .07 05-19-03 rls Converted to R3.14.x.
* - created a bit-field for motor status response.
* - start_status() allows one retry after a communication error.
* - set_status() sets RA_PROBLEM along with CNTRL_COMM_ERR to terminate
* node.
* .07 05-19-03 rls Converted to R3.14.x.
* .08 11-04-03 mlr Added a final poll of motor status if drvMM4000ReadbackDelay is
* non-zero. This is required to work around a bug in the firmware
* (versions 2.40 and 2.44) that the motor is reported as done moving
@@ -56,9 +56,9 @@ Last Modified: $Date: 2005-03-30 00:48:40 $
* .11 07/28/04 rls "epicsExport" debug variable.
* .12 09/21/04 rls support for 32axes/controller.
* .13 12/21/04 rls - support for MM4006.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - make debug variables always available.
*/
@@ -87,17 +87,17 @@ Last Modified: $Date: 2005-03-30 00:48:40 $
#define M_MINUS_LIMIT 0x10
#define M_HOME_SIGNAL 0x20
#define MM4000_NUM_CARDS 4
#define MM4000_NUM_CARDS 4
#define BUFF_SIZE 100 /* Maximum length of string to/from MM4000 */
#define TIMEOUT 2.0 /* Command timeout in sec. */
#define TIMEOUT 2.0 /* Command timeout in sec. */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvMM4000debug) printf(f,## args); }
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvMM4000debug) printf(f,## args); }
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -109,7 +109,7 @@ extern "C" {epicsExportAddress(int, drvMM4000debug);}
int MM4000_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/* This is a temporary fix to introduce a delayed reading of the motor
* position after a move completes
@@ -177,25 +177,25 @@ static long report(int level)
int card;
if (MM4000_num_cards <=0)
printf(" No MM4000 controllers configured.\n");
printf(" No MM4000 controllers configured.\n");
else
{
for (card = 0; card < MM4000_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < MM4000_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" MM4000 controller %d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
if (brdptr == NULL)
printf(" MM4000 controller %d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" MM4000 controller %d, port=%s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
}
}
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" MM4000 controller %d, port=%s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
}
}
}
return(OK);
}
@@ -203,7 +203,7 @@ static long report(int level)
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
@@ -212,7 +212,7 @@ static long init()
/* Check for setup */
if (MM4000_num_cards <= 0)
{
Debug(1, "init(): MM4000 driver disabled. MM4000Setup() missing from startup script.\n");
Debug(1, "init(): MM4000 driver disabled. MM4000Setup() missing from startup script.\n");
}
return((long) 0);
}
@@ -235,52 +235,52 @@ static void start_status(int card)
if (card >= 0)
{
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
send_mess(card, READ_STATUS, (char) NULL);
status = recv_mess(card, cntrl->status_string, 1);
if (status > 0)
{
cntrl->status = NORMAL;
send_mess(card, READ_POSITION, (char) NULL);
recv_mess(card, cntrl->position_string, 1);
}
else
{
if (cntrl->status == NORMAL)
cntrl->status = RETRY;
else
cntrl->status = COMM_ERR;
}
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
send_mess(card, READ_STATUS, (char) NULL);
status = recv_mess(card, cntrl->status_string, 1);
if (status > 0)
{
cntrl->status = NORMAL;
send_mess(card, READ_POSITION, (char) NULL);
recv_mess(card, cntrl->position_string, 1);
}
else
{
/*
* For efficiency we send messages to all cards, then read all
* responses. This minimizes the latency due to processing on each card
*/
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
send_mess(itera, READ_STATUS, (char) NULL);
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
{
cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate;
status = recv_mess(itera, cntrl->status_string, 1);
if (status > 0)
cntrl->status = NORMAL;
else
{
if (cntrl->status == NORMAL)
cntrl->status = RETRY;
else
cntrl->status = COMM_ERR;
}
}
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
send_mess(itera, READ_POSITION, (char) NULL);
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
{
cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate;
recv_mess(itera, cntrl->position_string, 1);
}
if (cntrl->status == NORMAL)
cntrl->status = RETRY;
else
cntrl->status = COMM_ERR;
}
}
else
{
/*
* For efficiency we send messages to all cards, then read all
* responses. This minimizes the latency due to processing on each card
*/
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
send_mess(itera, READ_STATUS, (char) NULL);
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
{
cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate;
status = recv_mess(itera, cntrl->status_string, 1);
if (status > 0)
cntrl->status = NORMAL;
else
{
if (cntrl->status == NORMAL)
cntrl->status = RETRY;
else
cntrl->status = COMM_ERR;
}
}
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
send_mess(itera, READ_POSITION, (char) NULL);
for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++)
{
cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate;
recv_mess(itera, cntrl->position_string, 1);
}
}
}
@@ -311,30 +311,30 @@ static int set_status(int card, int signal)
if (cntrl->status != NORMAL)
{
if (cntrl->status == COMM_ERR)
{
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
else
{
rtn_state = 0;
goto exit;
}
if (cntrl->status == COMM_ERR)
{
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
else
status.Bits.CNTRL_COMM_ERR = 0;
{
rtn_state = 0;
goto exit;
}
}
else
status.Bits.CNTRL_COMM_ERR = 0;
nodeptr = motor_info->motor_motion;
/*
/*
* Parse the status string
* Status string format: 1MSx,2MSy,3MSz,... where x, y and z are the status
* bytes for the motors
*/
pos = signal*5 + 3; /* Offset in status string */
pos = signal*5 + 3; /* Offset in status string */
mstat.All = cntrl->status_string[pos];
Debug(5, "set_status(): status byte = %x\n", mstat.All);
@@ -344,7 +344,7 @@ static int set_status(int card, int signal)
if (mstat.Bits.inmotion == false)
{
status.Bits.RA_DONE = 1;
status.Bits.RA_DONE = 1;
/* TEMPORARY FIX, Mark Rivers, 2/1/99. The MM4000 has reported that the
* motor is done moving, which means that the "jerk time" is done. However,
* the axis can still be settling. For now we put in a delay and poll the
@@ -354,52 +354,52 @@ static int set_status(int card, int signal)
* the motor is done moving. However, if the motor power was off, and the firmware
* version is 2.43 or 2.44 then the controller "lies" and says the motor is done
* on the first poll, when it is really moving. We work around this by reading
* the status again after the delay, in case it is really still moving.
* the status again after the delay, in case it is really still moving.
*/
if (motor_info->pid_present == YES && drvMM4000ReadbackDelay != 0.)
{
epicsThreadSleep(drvMM4000ReadbackDelay);
send_mess(card, READ_POSITION, (char) NULL);
recv_mess(card, cntrl->position_string, 1);
pos = signal*5 + 3; /* Offset in status string */
mstat.All = cntrl->status_string[pos];
if (mstat.Bits.inmotion == true)
status.Bits.RA_DONE = 0;
send_mess(card, READ_POSITION, 0);
recv_mess(card, cntrl->position_string, 1);
}
if (motor_info->pid_present == YES && drvMM4000ReadbackDelay != 0.)
{
epicsThreadSleep(drvMM4000ReadbackDelay);
send_mess(card, READ_POSITION, (char) NULL);
recv_mess(card, cntrl->position_string, 1);
pos = signal*5 + 3; /* Offset in status string */
mstat.All = cntrl->status_string[pos];
if (mstat.Bits.inmotion == true)
status.Bits.RA_DONE = 0;
send_mess(card, READ_POSITION, 0);
recv_mess(card, cntrl->position_string, 1);
}
}
else
status.Bits.RA_DONE = 0;
status.Bits.RA_DONE = 0;
/* Set Travel limit switch status bits. */
if (mstat.Bits.plustTL == false)
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_PLUS_LS = 0;
else
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
if (mstat.Bits.minusTL == false)
status.Bits.RA_MINUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
else
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
status.Bits.RA_HOME = (mstat.Bits.homels == false) ? 0 : 1;
status.Bits.EA_POSITION = (mstat.Bits.NOT_power == false) ? 1 : 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
/*
/*
* Parse motor position
* Position string format: 1TP5.012,2TP1.123,3TP-100.567,...
* Skip to substring for this motor, convert to double
@@ -409,24 +409,24 @@ static int set_status(int card, int signal)
tok_save = NULL;
p = strtok_r(buff, ",", &tok_save);
for (itera = 0; itera < signal; itera++)
p = strtok_r(NULL, ",", &tok_save);
p = strtok_r(NULL, ",", &tok_save);
Debug(6, "set_status(): position substring = %s\n", p);
motorData = atof(p+3) / cntrl->drive_resolution[signal];
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
motor_info->position = NINT(motorData);
if (motor_state[card]->motor_info[signal].encoder_present == YES)
motor_info->encoder_position = (epicsInt32) motorData;
else
motor_info->encoder_position = 0;
motor_info->position = NINT(motorData);
if (motor_state[card]->motor_info[signal].encoder_present == YES)
motor_info->encoder_position = (epicsInt32) motorData;
else
motor_info->encoder_position = 0;
motor_info->no_motion_count = 0;
motor_info->no_motion_count = 0;
}
status.Bits.RA_PROBLEM = 0;
@@ -437,18 +437,18 @@ static int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -458,43 +458,42 @@ exit:
/*****************************************************/
/* send a message to the MM4000 board */
/* send_mess() */
/* send a message to the MM4000 board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
struct MMcontroller *cntrl;
int size;
int nwrite;
size_t nwrite;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvMM4000.c:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvMM4000.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvMM4000.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvMM4000.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
if (name != NULL)
{
errlogPrintf("drvMM4000.c:send_mess() - invalid argument = %s\n", name);
return(ERROR);
errlogPrintf("drvMM4000.c:send_mess() - invalid argument = %s\n", name);
return(ERROR);
}
Debug(2, "send_mess(): message = %s\n", com);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com),
TIMEOUT, &nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com), TIMEOUT, &nwrite);
return(OK);
}
@@ -504,14 +503,14 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
* 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.
* 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.
* ERROR RETURN.
* ENDIF
* NORMAL RETURN.
*/
@@ -520,24 +519,24 @@ static int recv_mess(int card, char *com, int flag)
{
struct MMcontroller *cntrl;
double timeout = 0.;
int nread = 0;
size_t nread = 0;
asynStatus status;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
timeout = TIMEOUT;
timeout = TIMEOUT;
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
timeout, &nread, &eomReason);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -550,33 +549,33 @@ static int recv_mess(int card, char *com, int flag)
/* MM4000Setup() */
/*****************************************************/
RTN_STATUS
MM4000Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
MM4000Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > MM4000_NUM_CARDS)
MM4000_num_cards = MM4000_NUM_CARDS;
MM4000_num_cards = MM4000_NUM_CARDS;
else
MM4000_num_cards = num_cards;
MM4000_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before MM4000Config 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(MM4000_num_cards *
sizeof(struct controller *));
sizeof(struct controller *));
for (itera = 0; itera < MM4000_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
@@ -587,7 +586,7 @@ MM4000Setup(int num_cards, /* maximum number of controllers in system. */
/* MM4000Config() */
/*****************************************************/
RTN_STATUS
MM4000Config(int card, /* card being configured */
MM4000Config(int card, /* card being configured */
const char *name, /* asyn port name */
int addr) /* asyn address (GPIB) */
{
@@ -608,10 +607,10 @@ MM4000Config(int card, /* card being configured */
/*****************************************************/
/* initialize all software and hardware */
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
@@ -625,140 +624,140 @@ static int motor_init()
int status, model_num, digits;
asynStatus success_rtn;
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (MM4000_num_cards <= 0)
return(ERROR);
return(ERROR);
for (card_index = 0; card_index < MM4000_num_cards; card_index++)
{
if (!motor_state[card_index])
continue;
if (!motor_state[card_index])
continue;
brdptr = motor_state[card_index];
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
brdptr = motor_state[card_index];
brdptr->cmnd_response = false;
total_cards = card_index + 1;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
cntrl->asyn_address, &cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
{
int retry = 0;
if (success_rtn == asynSuccess)
{
int retry = 0;
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
/* Send a message to the board, see if it exists */
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
do
{
send_mess(card_index, READ_POSITION, (char) NULL);
status = recv_mess(card_index, axis_pos, 1);
retry++;
/* Return value is length of response string */
} while(status == 0 && retry < 3);
}
do
{
send_mess(card_index, READ_POSITION, (char) NULL);
status = recv_mess(card_index, axis_pos, 1);
retry++;
/* Return value is length of response string */
} while(status == 0 && retry < 3);
}
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
send_mess(card_index, STOP_ALL, (char) NULL); /* Stop all motors */
send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */
recv_mess(card_index, buff, 1);
strcpy(brdptr->ident, &buff[2]); /* Skip "VE" */
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
send_mess(card_index, STOP_ALL, (char) NULL); /* Stop all motors */
send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */
recv_mess(card_index, buff, 1);
strcpy(brdptr->ident, &buff[2]); /* Skip "VE" */
/* Set Motion Master model indicator. */
pos_ptr = strstr(brdptr->ident, "MM");
if (pos_ptr == NULL)
{
errlogPrintf("drvMM4000.c:motor_init() - invalid model = %s\n", brdptr->ident);
motor_state[card_index] = (struct controller *) NULL;
continue;
}
model_num = atoi(pos_ptr + 2);
if (model_num == 4000)
cntrl->model = MM4000;
else if (model_num == 4005 || model_num == 4006)
cntrl->model = MM4005;
else
{
errlogPrintf("drvMM4000.c:motor_init() - invalid model = %s\n", brdptr->ident);
motor_state[card_index] = (struct controller *) NULL;
continue;
}
/* Set Motion Master model indicator. */
pos_ptr = strstr(brdptr->ident, "MM");
if (pos_ptr == NULL)
{
errlogPrintf("drvMM4000.c:motor_init() - invalid model = %s\n", brdptr->ident);
motor_state[card_index] = (struct controller *) NULL;
continue;
}
model_num = atoi(pos_ptr + 2);
if (model_num == 4000)
cntrl->model = MM4000;
else if (model_num == 4005 || model_num == 4006)
cntrl->model = MM4005;
else
{
errlogPrintf("drvMM4000.c:motor_init() - invalid model = %s\n", brdptr->ident);
motor_state[card_index] = (struct controller *) NULL;
continue;
}
send_mess(card_index, READ_POSITION, (char) NULL);
recv_mess(card_index, axis_pos, 1);
send_mess(card_index, READ_POSITION, (char) NULL);
recv_mess(card_index, axis_pos, 1);
/* The return string will tell us how many axes this controller has */
for (total_axis = 0, tok_save = NULL, pos_ptr = strtok_r(axis_pos, ",", &tok_save);
pos_ptr != 0; pos_ptr = strtok_r(NULL, ",", &tok_save), total_axis++)
brdptr->motor_info[total_axis].motor_motion = NULL;
/* The return string will tell us how many axes this controller has */
for (total_axis = 0, tok_save = NULL, pos_ptr = strtok_r(axis_pos, ",", &tok_save);
pos_ptr != 0; pos_ptr = strtok_r(NULL, ",", &tok_save), total_axis++)
brdptr->motor_info[total_axis].motor_motion = NULL;
brdptr->total_axis = total_axis;
brdptr->total_axis = total_axis;
start_status(card_index);
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
int loop_state;
start_status(card_index);
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
int loop_state;
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
/* Determine if encoder present based on open/closed loop mode. */
sprintf(buff, "%dTC", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
loop_state = atoi(&buff[3]); /* Skip first 3 characters */
if (loop_state != 0)
{
sprintf(buff, "%dTC", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
loop_state = atoi(&buff[3]); /* Skip first 3 characters */
if (loop_state != 0)
{
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
}
/* Determine drive resolution. */
sprintf(buff, "%dTU", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
cntrl->drive_resolution[motor_index] = atof(&buff[3]);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
cntrl->drive_resolution[motor_index] = atof(&buff[3]);
digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2;
if (digits < 1)
digits = 1;
cntrl->res_decpts[motor_index] = digits;
digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2;
if (digits < 1)
digits = 1;
cntrl->res_decpts[motor_index] = digits;
/* Save home preset position. */
/* Save home preset position. */
sprintf(buff, "%dXH", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
cntrl->home_preset[motor_index] = atof(&buff[3]);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
cntrl->home_preset[motor_index] = atof(&buff[3]);
/* Determine low limit */
sprintf(buff, "%dTL", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
motor_info->low_limit = atof(&buff[3]);
/* Determine high limit */
sprintf(buff, "%dTR", motor_index + 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
motor_info->high_limit = atof(&buff[3]);
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -769,7 +768,7 @@ static int motor_init()
free_list.head = (struct mess_node *) NULL;
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "MM4000_motor",
epicsThreadCreate((char *) "MM4000_motor",
epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
motorApp/NewportSrc/drvPM500.cc
+238 -239
View File
@@ -1,16 +1,16 @@
/*
FILENAME... drvPM500.cc
USAGE... Motor record driver level support for Newport PM500.
FILENAME... drvPM500.cc
USAGE... Motor record driver level support for Newport PM500.
Version: $Revision: 1.16 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-03-30 19:10:48 $
Version: $Revision: 1.17 $
Modified By: $Author: dkline $
Last Modified: $Date: 2005-04-14 18:52:48 $
*/
/* Device Driver Support routines for PM500 motor controller */
/*
* Original Author: Mark Rivers
* Current Author: Ron Sluiter
* Current Author: Ron Sluiter
*
* Experimental Physics and Industrial Control System (EPICS)
*
@@ -22,30 +22,30 @@ Last Modified: $Date: 2005-03-30 19:10:48 $
* 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
* 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
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
* .01 11-19-98 mlr Initial development, based on drvMM4000.c
* .02 06-02-00 rls integrated into standard motor record
* .02 06-02-00 rls integrated into standard motor record
* .03 10/02/01 rls allow one retry after a communication error.
* .04 05-23-03 rls Converted to R3.14.x.
* .04 05-23-03 rls Converted to R3.14.x.
* .05 02/03/04 rls Eliminate erroneous "Motor motion timeout ERROR".
* .06 07/09/04 rls removed unused <driver>Setup() argument.
* .07 07/28/04 rls "epicsExport" debug variable.
* .08 09/21/04 rls support for 32axes/controller.
* .09 12/21/04 rls - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - make debug variables always available.
* extern "C" linkage.
* - make debug variables always available.
*/
@@ -73,18 +73,18 @@ Last Modified: $Date: 2005-03-30 19:10:48 $
#define M_MINUS_LIMIT 0x10
#define M_HOME_SIGNAL 0x20
#define PM500_NUM_CARDS 4
#define PM500_NUM_CARDS 4
#define PM500_NUM_CHANNELS 12
#define BUFF_SIZE 100 /* Maximum length of string to/from PM500 */
#define SERIAL_TIMEOUT 2.0 /* Command timeout in sec. */
#define SERIAL_TIMEOUT 2.0 /* Command timeout in sec. */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvPM500debug) printf(f,## args); }
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvPM500debug) printf(f,## args); }
#else
#define Debug(l, f, args...)
#define Debug(l, f, args...)
#endif
#else
#define Debug()
@@ -95,10 +95,10 @@ extern "C" {epicsExportAddress(int, drvPM500debug);}
/* --- Local data. --- */
int PM500_num_cards = 0;
static char *PM500_axis_names[] = {"X", "Y", "Z", "A", "B", "C", "D", "E", "F",
"G", "H", "I"};
"G", "H", "I"};
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
@@ -159,25 +159,25 @@ static long report(int level)
int card;
if (PM500_num_cards <=0)
printf(" No PM500 controllers configured.\n");
printf(" No PM500 controllers configured.\n");
else
{
for (card = 0; card < PM500_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
for (card = 0; card < PM500_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" PM500 controller #%d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
if (brdptr == NULL)
printf(" PM500 controller #%d connection failed.\n", card);
else
{
struct MMcontroller *cntrl;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" PM500 controller %d port=%s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
}
}
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
printf(" PM500 controller %d port=%s, address=%d, id: %s \n",
card, cntrl->asyn_port, cntrl->asyn_address,
brdptr->ident);
}
}
}
return(OK);
}
@@ -185,7 +185,7 @@ static long report(int level)
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
@@ -194,7 +194,7 @@ static long init()
/* Check for setup */
if (PM500_num_cards <= 0)
{
Debug(1, "init(): PM500 driver disabled. PM500Setup() missing from startup script.\n");
Debug(1, "init(): PM500 driver disabled. PM500Setup() missing from startup script.\n");
}
return((long) 0);
}
@@ -217,7 +217,7 @@ static int set_status(int card, int signal)
register struct mess_info *motor_info;
/* Message parsing variables */
char *axis_name, status_char, dir_char, buff[BUFF_SIZE],
response[BUFF_SIZE];
response[BUFF_SIZE];
int rtnval, rtn_state = 0;
double motorData;
bool ls_active;
@@ -235,59 +235,59 @@ static int set_status(int card, int signal)
rtnval = recv_mess(card, response, 1);
if (rtnval > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
status_char = response[1];
dir_char = response[2];
motorData = atof(&response[2]) / cntrl->drive_resolution[signal];
status.Bits.RA_DONE = (status_char == 'B') ? 0 : 1;
status.Bits.RA_DONE = (status_char == 'B') ? 0 : 1;
status.Bits.RA_PROBLEM = (status_char == 'E') ? 1 : 0;
status.Bits.RA_DIRECTION = (dir_char == '+') ? 1 : 0;
if (status_char == 'L')
{
ls_active = true;
if (dir_char == '+')
status.Bits.RA_PLUS_LS = 1;
else
status.Bits.RA_MINUS_LS = 1;
ls_active = true;
if (dir_char == '+')
status.Bits.RA_PLUS_LS = 1;
else
status.Bits.RA_MINUS_LS = 1;
}
else
{
ls_active = false;
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
ls_active = false;
status.Bits.RA_PLUS_LS = 0;
status.Bits.RA_MINUS_LS = 0;
}
status.Bits.RA_HOME = 0;
/* encoder status */
status.Bits.EA_POSITION = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
status.Bits.EA_POSITION = 0;
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
/*
/*
* Parse motor position
* Position string format: 1TP5.012,2TP1.123,3TP-100.567,...
* Skip to substring for this motor, convert to double
@@ -295,18 +295,18 @@ static int set_status(int card, int signal)
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
motor_info->position = NINT(motorData);
if (motor_state[card]->motor_info[signal].encoder_present == YES)
motor_info->encoder_position = (epicsInt32) motorData;
else
motor_info->encoder_position = 0;
motor_info->position = NINT(motorData);
if (motor_state[card]->motor_info[signal].encoder_present == YES)
motor_info->encoder_position = (epicsInt32) motorData;
else
motor_info->encoder_position = 0;
motor_info->no_motion_count = 0;
motor_info->no_motion_count = 0;
}
status.Bits.RA_PROBLEM = 0;
@@ -317,19 +317,19 @@ static int set_status(int card, int signal)
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
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)
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
strcat(buff, "\r");
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
strcpy(buff, nodeptr->postmsgptr);
strcat(buff, "\r");
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
@@ -339,37 +339,36 @@ exit:
/*****************************************************/
/* send a message to the PM500 board */
/* send_mess() */
/* send a message to the PM500 board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
struct MMcontroller *cntrl;
int size;
int nwrite;
size_t nwrite;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvPM500.c:send_mess(); message size violation.\n");
return(ERROR);
errlogMessage("drvPM500.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvPM500.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
errlogPrintf("drvPM500.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
Debug(2, "send_mess(): message = %s\n", com);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com),
SERIAL_TIMEOUT, &nwrite);
pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com), SERIAL_TIMEOUT, &nwrite);
return(OK);
}
@@ -379,14 +378,14 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
* 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.
* 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.
* ERROR RETURN.
* ENDIF
* NORMAL RETURN.
*/
@@ -396,19 +395,19 @@ static int recv_mess(int card, char *com, int flag)
struct MMcontroller *cntrl;
double timeout = 0.;
int flush=1;
int nread = 0;
size_t nread = 0;
asynStatus status;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
if (flag != FLUSH) {
flush=0;
timeout = SERIAL_TIMEOUT;
timeout = SERIAL_TIMEOUT;
}
if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
@@ -416,14 +415,14 @@ static int recv_mess(int card, char *com, int flag)
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
com[0] = '\0';
nread = 0;
}
else
{
/* Test for "system error" response. */
if (strncmp(com, "SE", 2) == 0)
errlogMessage("recv_mess(): PM500 system error.\n");
/* Test for "system error" response. */
if (strncmp(com, "SE", 2) == 0)
errlogMessage("recv_mess(): PM500 system error.\n");
}
Debug(2, "recv_mess(): message = \"%s\"\n", com);
@@ -436,33 +435,33 @@ static int recv_mess(int card, char *com, int flag)
/* PM500Setup() */
/*****************************************************/
RTN_STATUS
PM500Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
PM500Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
{
int itera;
if (num_cards < 1 || num_cards > PM500_NUM_CARDS)
PM500_num_cards = PM500_NUM_CARDS;
PM500_num_cards = PM500_NUM_CARDS;
else
PM500_num_cards = num_cards;
PM500_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
targs.motor_scan_rate = SCAN_RATE;
/*
/*
* Allocate space for motor_state structures. Note this must be done
* before PM500Config 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(PM500_num_cards *
sizeof(struct controller *));
sizeof(struct controller *));
for (itera = 0; itera < PM500_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
@@ -473,7 +472,7 @@ PM500Setup(int num_cards, /* maximum number of controllers in system. */
/* PM500Config() */
/*****************************************************/
RTN_STATUS
PM500Config(int card, /* card being configured */
PM500Config(int card, /* card being configured */
const char *name, /*asyn port name */
int address) /*asyn address (GPIB) */
{
@@ -494,10 +493,10 @@ PM500Config(int card, /* card being configured */
/*****************************************************/
/* initialize all software and hardware */
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
@@ -509,81 +508,81 @@ static int motor_init()
int status, digits;
asynStatus success_rtn;
initialized = true; /* Indicate that driver is initialized. */
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (PM500_num_cards <= 0)
return(ERROR);
return(ERROR);
for (card_index = 0; card_index < PM500_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 MMcontroller *) brdptr->DevicePrivate;
if (!motor_state[card_index])
continue;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
brdptr = motor_state[card_index];
brdptr->cmnd_response = true;
total_cards = card_index + 1;
cntrl = (struct MMcontroller *) brdptr->DevicePrivate;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port,
cntrl->asyn_address, &cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
{
/* flush any junk at input port - should not be any data available */
if (success_rtn == asynSuccess)
{
/* flush any junk at input port - should not be any data available */
pasynOctetSyncIO->flush(cntrl->pasynUser);
/* Send a SCUM 1 command to put device in this mode. */
send_mess(card_index, "SCUM 1", (char) NULL);
recv_mess(card_index, buff, 1);
/* Set up basic controller parameters
* "ENAINT $AF" means the following:
* Bit 0=1, only affected axis halts on limit
* Bit 1=1, No message when moving axis beyond limit
* Bit 2=1, No query echo, prepends status character to axis.
* Bit 3=1, NO status character inserted in responses.
* Bit 4=0, No acknowledgement when command is received
* Bit 5=1, Disable sign-on message at power-up
* Bit 6=0, No echo
* Bit 7=1, CR terminator only on commands and responses
* Bit 8=0, CR terminator only on commands and responses
* Bit 9=0, No EOI sent
* Bit 10=0, CR terminator only on commands and responses
* Bit 11=0, CR terminator only on commands and responses
* Bit 12=0, Decimal number format
* Bit 13=0, Eearly serial poll mapping
* Bit 14=0, No SRQ assertion
*/
send_mess(card_index, "SENAINT $AF", (char) NULL);
recv_mess(card_index, buff, 1);
/* Send a message and read response from controller to see if
* it exists */
send_mess(card_index, GET_IDENT, (char) NULL);
status = recv_mess(card_index, buff, 1);
/* Return value is length of response string */
}
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */
recv_mess(card_index, buff, 1);
strncpy(brdptr->ident, &buff[2], 50); /* Skip "XD" */
/* Send a SCUM 1 command to put device in this mode. */
send_mess(card_index, "SCUM 1", (char) NULL);
recv_mess(card_index, buff, 1);
/* Set up basic controller parameters
* "ENAINT $AF" means the following:
* Bit 0=1, only affected axis halts on limit
* Bit 1=1, No message when moving axis beyond limit
* Bit 2=1, No query echo, prepends status character to axis.
* Bit 3=1, NO status character inserted in responses.
* Bit 4=0, No acknowledgement when command is received
* Bit 5=1, Disable sign-on message at power-up
* Bit 6=0, No echo
* Bit 7=1, CR terminator only on commands and responses
* Bit 8=0, CR terminator only on commands and responses
* Bit 9=0, No EOI sent
* Bit 10=0, CR terminator only on commands and responses
* Bit 11=0, CR terminator only on commands and responses
* Bit 12=0, Decimal number format
* Bit 13=0, Eearly serial poll mapping
* Bit 14=0, No SRQ assertion
*/
send_mess(card_index, "SENAINT $AF", (char) NULL);
recv_mess(card_index, buff, 1);
/* Send a message and read response from controller to see if
* it exists */
send_mess(card_index, GET_IDENT, (char) NULL);
status = recv_mess(card_index, buff, 1);
/* Return value is length of response string */
}
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */
recv_mess(card_index, buff, 1);
strncpy(brdptr->ident, &buff[2], 50); /* Skip "XD" */
/* Figure out how many axes this controller has.
* Do this by querying status of each axis in order */
for (total_axis = 0; total_axis < PM500_NUM_CHANNELS; total_axis++)
{
brdptr->motor_info[total_axis].motor_motion = NULL;
sprintf(buff, "%cSTAT?", PM500_axis_names[total_axis]);
{
brdptr->motor_info[total_axis].motor_motion = NULL;
sprintf(buff, "%cSTAT?", PM500_axis_names[total_axis]);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
if (buff[1] == 'E')
break;
break;
/* Determine axis type and resolution.
* This is a real pain, since the only way to get this
* information is to ask for the axis firmware and use a
@@ -596,72 +595,72 @@ static int motor_init()
*/
}
brdptr->total_axis = total_axis;
brdptr->total_axis = total_axis;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
char *firmware, *axis_name = PM500_axis_names[motor_index];
double res = 0.0;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
char *firmware, *axis_name = PM500_axis_names[motor_index];
double res = 0.0;
sprintf(buff, "%sCONFIG?", axis_name);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
send_mess(card_index, buff, (char) NULL);
recv_mess(card_index, buff, 1);
firmware = &buff[8];
Debug(3, "motor_init: firmware = %s\n", firmware);
if (!strcmp(firmware, "302"))
{
/* 50 nm translator */
res = .01;
Debug(3, "motor_init: axis %d is a 50 nm translator\n", motor_index);
}
else if (!strcmp(firmware, "309"))
{
/* 25 nm translator */
res = .01;
Debug(3, "motor_init: axis %d is a 25 nm translator\n", motor_index);
}
else if (!strcmp(firmware, "300"))
{
/* ????? translator ?????? */
res = .01;
Debug(3, "motor_init: axis %d is a ?????? translator\n", motor_index);
}
else if (!strcmp(firmware, "XXX"))
{
/* Rotator */
res = .01;
}
Debug(3, "motor_init: firmware = %s\n", firmware);
if (!strcmp(firmware, "302"))
{
/* 50 nm translator */
res = .01;
Debug(3, "motor_init: axis %d is a 50 nm translator\n", motor_index);
}
else if (!strcmp(firmware, "309"))
{
/* 25 nm translator */
res = .01;
Debug(3, "motor_init: axis %d is a 25 nm translator\n", motor_index);
}
else if (!strcmp(firmware, "300"))
{
/* ????? translator ?????? */
res = .01;
Debug(3, "motor_init: axis %d is a ?????? translator\n", motor_index);
}
else if (!strcmp(firmware, "XXX"))
{
/* Rotator */
res = .01;
}
/* Set drive resolution. */
cntrl->drive_resolution[motor_index] = res;
cntrl->drive_resolution[motor_index] = res;
digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2;
if (digits < 1)
digits = 1;
cntrl->res_decpts[motor_index] = digits;
digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2;
if (digits < 1)
digits = 1;
cntrl->res_decpts[motor_index] = digits;
/* PM500 only supports DC motors. */
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
/* PM500 only supports DC motors. */
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
motor_info->encoder_present = NO;
motor_info->pid_present = NO;
motor_info->encoder_present = NO;
motor_info->pid_present = NO;
cntrl->home_preset[motor_index] = 0;
cntrl->home_preset[motor_index] = 0;
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -673,8 +672,8 @@ static int motor_init()
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "PM500_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(OK);
}