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

Restore format.

Browse Source
This commit is contained in:
Ron Sluiter
2006-04-24 19:52:36 +00:00
parent 1defd6dc3d
commit 31cb3b1d8c
1 changed files with 185 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/NewportSrc/drvESP300.cc
+185 -180
View File
@@ -2,15 +2,15 @@
FILENAME... drvESP300.cc
USAGE... Motor record driver level support for Newport ESP300/100.
Version: $Revision: 1.21 $
Version: $Revision: 1.22 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2005-10-20 19:43:37 $
Last Modified: $Date: 2006-04-24 19:52:36 $
*/
/*
* Original Author: Ron Sluiter
* Date: 02/19/03
* Current Author: Ron Sluiter
* Current Author: Ron Sluiter
*
* Experimental Physics and Industrial Control System (EPICS)
*
@@ -75,9 +75,9 @@ Last Modified: $Date: 2005-10-20 19:43:37 $
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) { if(l<=drvESP300debug) printf(f,## args); }
#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()
@@ -90,6 +90,7 @@ int ESP300_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
static RTN_STATUS send_mess(int, char const *, char *);
@@ -149,42 +150,42 @@ 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];
if (brdptr == NULL)
printf(" ESP300 controller %d connection failed.\n", card);
else
for (card = 0; card < ESP300_num_cards; card++)
{
struct MMcontroller *cntrl;
struct controller *brdptr = motor_state[card];
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);
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);
}
}
}
}
return(OK);
}
static long init()
{
/*
* We cannot call motor_init() here, because that function can do GPIB I/O,
* and hence requires that the drvGPIB have already been initialized.
* That cannot be guaranteed, so we need to call motor_init from device
* support
*/
/*
* We cannot call motor_init() here, because that function can do GPIB I/O,
* and hence requires that the drvGPIB have already been initialized.
* That cannot be guaranteed, so we need to call motor_init from device
* support
*/
/* Check for setup */
if (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);
}
@@ -225,25 +226,25 @@ 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;
@@ -257,17 +258,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;
@@ -279,29 +280,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. */
@@ -331,23 +332,23 @@ 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:
motor_info->status.All = status.All;
return (rtn_state);
return(rtn_state);
}
@@ -365,22 +366,22 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
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);
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);
@@ -447,33 +448,37 @@ static int recv_mess(int card, char *com, int flag)
/* Check that card exists */
if (!motor_state[card])
return(ERROR);
return(ERROR);
cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate;
if (flag != FLUSH) {
if (flag != FLUSH)
{
flush = 0;
timeout = SERIAL_TIMEOUT;
timeout = SERIAL_TIMEOUT;
}
if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser);
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) {
if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
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,
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);
@@ -487,32 +492,32 @@ static int recv_mess(int card, char *com, int flag)
/*****************************************************/
RTN_STATUS
ESP300Setup(int num_cards, /* maximum number of controllers in system. */
int scan_rate) /* polling rate - 1/60 sec units. */
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
*/
/*
* 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);
}
@@ -524,13 +529,13 @@ ESP300Setup(int num_cards, /* maximum number of controllers in system. */
/*****************************************************/
RTN_STATUS
ESP300Config(int card, /* card being configured */
const char *name, /* asyn port name */
int address) /* asyn address (GPIB) */
const char *name, /* asyn port name */
int address) /* asyn address (GPIB) */
{
struct MMcontroller *cntrl;
if (card < 0 || card >= ESP300_num_cards)
return (ERROR);
return(ERROR);
motor_state[card] = (struct controller *) malloc(sizeof(struct controller));
motor_state[card]->DevicePrivate = malloc(sizeof(struct MMcontroller));
@@ -565,96 +570,96 @@ static int motor_init()
/* 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;
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, 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" */
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;
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);
}
for (motor_index = 0; motor_index < total_axis; motor_index++)
if (success_rtn == asynSuccess && status > 0)
{
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;
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
strcpy(brdptr->ident, &buff[1]); /* Skip "\n" */
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;
}
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;
}
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];
/* 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 */
}
}
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];
/* 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;
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -666,8 +671,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);
}