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

Bug fix setting RA_DONE based on inMotion.

Browse Source
This commit is contained in:
Ron Sluiter
2008-06-06 17:23:42 +00:00
parent 5f6e872008
commit 9b249ebe2a
1 changed files with 243 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/MicroMoSrc/drvMVP2001.cc
+243 -239
View File
@@ -1,16 +1,16 @@
/*
FILENAME... drvMVP2001.cc
USAGE... Motor record driver level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
MVP 2001 B02 (Linear, RS-485).
Version: $Revision: 1.10 $
Version: $Revision: 1.11 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2006-10-02 15:49:08 $
Last Modified: $Date: 2008-06-06 17:23:42 $
*/
/*
* Original Author: Kevin Peterson
* Date: 08/27/2002
* Original Author: Kevin Peterson
* Date: 08/27/2002
*
*
* Illinois Open Source License
@@ -83,6 +83,7 @@ Last Modified: $Date: 2006-10-02 15:49:08 $
* extern "C" linkage.
* - retry on initial communication.
* .08 10/02/06 rls - Bug in recv_mess(); always returned nread=0.
* .09 06/06/08 rls - Bug fix setting RA_DONE based on inMotion.
*/
/*
@@ -128,18 +129,19 @@ MORE DESIGN LIMITATIONS
#include "epicsExport.h"
#define MVP2001_NUM_CARDS 8
#define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */
#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); }
#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()
#endif
volatile int drvMVP2001debug = 0;
extern "C" {epicsExportAddress(int, drvMVP2001debug);}
@@ -149,6 +151,7 @@ int MVP2001_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 *);
@@ -202,26 +205,26 @@ static long report(int level)
{
int card;
if (MVP2001_num_cards <=0)
printf(" No MVP2001 CHAINS configured.\n");
if (MVP2001_num_cards <= 0)
printf(" No MVP2001 CHAINS configured.\n");
else
{
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
for (card = 0; card < MVP2001_num_cards; card++)
{
struct MVPcontroller *cntrl;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
printf(" MVP2001 controller chain #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
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);
}
}
}
}
return (0);
return(0);
}
@@ -233,11 +236,9 @@ static long 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);
return((long) 0);
}
@@ -247,32 +248,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 *
* *
* *
********************************************************************************/
/*
@@ -306,111 +307,109 @@ 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;
}
}
/*
* Parse status string
* Status string format: 0001 FFFF
* Skip to status substring for this motor, convert from hex to int
* Parse status string Status string format: 0001 FFFF Skip to status
* substring for this motor, convert from hex to int
*/
strncat(statusStr, &buff[5], 4);
mstat.All = strtoul(statusStr, NULL, 16);
buff[0] = '\0';
status.Bits.RA_DONE = mstat.Bits.inMotion;
status.Bits.RA_DONE = !mstat.Bits.inMotion;
sprintf(buff, "%d POS", (signal + 1));
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
* 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';
/*
* Set direction by comparing positions since the MVP2001
* does not have a direction bit.
* 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 */
status.Bits.RA_HOME = 0;
/* !!! Assume no closed-looped control!!!*/
/* !!! 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
{
/*
* 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 +420,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:
@@ -455,16 +454,16 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
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);
@@ -487,31 +486,30 @@ static int recv_mess(int card, char *com, int flag)
size_t nread = 0, lenTemp = 0;
asynStatus status = asynError;
int timeout;
int flush = 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;
status = pasynOctetSyncIO->read(cntrl->pasynUser, temp, BUFF_SIZE,
COMM_TIMEOUT, &lenTemp, &eomReason);
COMM_TIMEOUT, &lenTemp, &eomReason);
status = 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);
@@ -525,27 +523,27 @@ static int recv_mess(int card, char *com, int flag)
/*****************************************************/
RTN_STATUS
MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
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
*/
/*
* 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);
}
@@ -560,13 +558,13 @@ MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
* MVP2001Config() *
********************************************************/
RTN_STATUS
MVP2001Config(int card, /* CHAIN being configured */
MVP2001Config(int card, /* CHAIN being configured */
const char *name) /* asyn server task name */
{
struct MVPcontroller *cntrl;
if (card < 0 || card >= MVP2001_num_cards)
return (ERROR);
return(ERROR);
motor_state[card] = (struct controller *) calloc(1, sizeof(struct controller));
motor_state[card]->DevicePrivate = calloc(1, sizeof(struct MVPcontroller));
@@ -591,128 +589,135 @@ static int motor_init()
int status;
asynStatus success_rtn;
static const char output_terminator[] = "\r";
static const char input_terminator[] = "\n";
static const char input_terminator[] = "\n";
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;
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;
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;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
/* 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 */
for (total_axis = 0; total_axis < MAX_AXIS; total_axis++)
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));
/* flush any junk at input port - should not be any data available */
do
recv_mess(card_index, buff, FLUSH);
while (strlen(buff) != 0);
/* Send a message to the board, see if it exists */
for (total_axis = 0; total_axis < MAX_AXIS; total_axis++)
{
int retry = 0;
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);
}
/*
* 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 && total_axis > 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];
/* 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);
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;
/* no encoder support for correct DC controller interaction */
motor_info->encoder_present = NO;
motor_info->status.Bits.EA_PRESENT = 0;
/* MVP2001 has PID capabilities */
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
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);
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);
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);
}
/*
* 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++)
if (success_rtn == asynSuccess && total_axis > 0)
{
epicsThreadSleep(0.2);
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
sprintf(buff, "%d HO", (motor_index + 1));
send_mess(card_index, buff, (char) NULL);
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 */
/* 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);
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;
/* no encoder support for correct DC controller interaction */
motor_info->encoder_present = NO;
motor_info->status.Bits.EA_PRESENT = 0;
/* MVP2001 has PID capabilities */
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
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);
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);
}
/*
* 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;
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
@@ -724,9 +729,8 @@ 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);
}