From 9b249ebe2a55b3336014864c9086222d3412eb4d Mon Sep 17 00:00:00 2001 From: Ron Sluiter Date: Fri, 6 Jun 2008 17:23:42 +0000 Subject: [PATCH] Bug fix setting RA_DONE based on inMotion. --- motorApp/MicroMoSrc/drvMVP2001.cc | 482 +++++++++++++++--------------- 1 file changed, 243 insertions(+), 239 deletions(-) diff --git a/motorApp/MicroMoSrc/drvMVP2001.cc b/motorApp/MicroMoSrc/drvMVP2001.cc index 68e14843..42b8917f 100644 --- a/motorApp/MicroMoSrc/drvMVP2001.cc +++ b/motorApp/MicroMoSrc/drvMVP2001.cc @@ -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); } -