From 9329ad6a7c7281debef3aa12c00a4e3ae30b8e45 Mon Sep 17 00:00:00 2001 From: jsullivan-anl Date: Thu, 2 Nov 2006 21:05:56 +0000 Subject: [PATCH] fix various problems related to position readback and multi-axis motion. Inmotion position readback was eliminated. --- motorApp/OrielSrc/README | 12 +++++ motorApp/OrielSrc/devEMC18011.cc | 82 ++++++++++++++++++------------ motorApp/OrielSrc/drvEMC18011.cc | 87 ++++++++++++++++++++------------ motorApp/OrielSrc/drvEMC18011.h | 6 ++- 4 files changed, 121 insertions(+), 66 deletions(-) diff --git a/motorApp/OrielSrc/README b/motorApp/OrielSrc/README index 04ed9162..64addeb6 100755 --- a/motorApp/OrielSrc/README +++ b/motorApp/OrielSrc/README @@ -119,6 +119,18 @@ iocBoot/iocLinux Set the DTYPE column to "EMC18011" +=================== Motor Record Setup ================================ + +EGU = microns +VELO = [0.5 to 200] Maximum and Base velocity not used + +BDST = 0 Controller does its own backlash + +MRES = .010 (set to velocity resolution, position resolution is .10 microns) +RDBD = .05 (position resolution is only 0.1 - will be rounded up) + + + diff --git a/motorApp/OrielSrc/devEMC18011.cc b/motorApp/OrielSrc/devEMC18011.cc index 8726bc2c..16c91ecb 100755 --- a/motorApp/OrielSrc/devEMC18011.cc +++ b/motorApp/OrielSrc/devEMC18011.cc @@ -2,9 +2,9 @@ FILENAME... devEMC18011.cc USAGE... Motor record device level support for Parker Compumotor drivers -Version: $Revision: 1.2 $ +Version: $Revision: 1.3 $ Modified By: $Author: sullivan $ -Last Modified: $Date: 2006-09-07 21:19:43 $ +Last Modified: $Date: 2006-11-02 21:05:56 $ */ /* @@ -46,6 +46,7 @@ Last Modified: $Date: 2006-09-07 21:19:43 $ #include "drvEMC18011.h" #include "epicsExport.h" + #define STATIC static extern struct driver_table EMC18011_access; @@ -160,11 +161,13 @@ STATIC RTN_STATUS EMC18011_build_trans(motor_cmnd command, double *parms, struct double dval, cntrl_units; unsigned int size; bool sendMsg; + bool switchMotor; RTN_STATUS rtnval; rtnval = OK; buff[0] = '\0'; sendMsg = true; + switchMotor = false; /* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */ intval = (parms == NULL) ? 0 : NINT(parms[0]); @@ -180,10 +183,11 @@ STATIC RTN_STATUS EMC18011_build_trans(motor_cmnd command, double *parms, struct motor_call->type = EMC18011_table[command]; brdptr = (*trans->tabptr->card_array)[card]; - if (brdptr == NULL) - return(rtnval = ERROR); + if (brdptr == NULL) + return(rtnval = ERROR); cntrl = (struct EMC18011Controller *) brdptr->DevicePrivate; + /* 6K Controllers expect Velocity and Acceleration settings in Revs/sec/sec */ cntrl_units = dval * cntrl->drive_resolution; @@ -194,47 +198,63 @@ STATIC RTN_STATUS EMC18011_build_trans(motor_cmnd command, double *parms, struct if (trans->state != BUILD_STATE) return(rtnval = ERROR); - if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0) - { - strcat(motor_call->message, mr->init); - strcat(motor_call->message, EMC18011_OUT_EOS); - } + /* SPECIAL: This controller can only address one motor at a time + * Other motion requests have to wait until the current motion + * is complete. Depend on motorRecord retries to complete pending + * requests. Allow record to stop current motion. + * Switch message type to INFO to get done flag + */ switch (command) { case JOG: - case SET_VELOCITY: - /* Before a motion is started - assure that the proper motor - * is selected. First Stop current motion - if any */ - if (cntrl->motorSelect != signal) + case LOAD_POS: + case SET_VELOCITY: + case GET_INFO: + + /* Try to get motorLock then switch active motor */ + if (cntrl->motorLock->tryWait()) { - strcat(motor_call->message, "S"); - rtnval = motor_end_trans_com(mr, drvtabptr); - rtnval = (RTN_STATUS) motor_start_trans_com(mr, EMC18011_cards); - motor_call->type = EMC18011_table[command]; + if (cntrl->motorSelect != signal) + { + cntrl->motorSelect = signal; - sprintf(buff, "M%d", axis); - strcat(motor_call->message, buff); - rtnval = motor_end_trans_com(mr, drvtabptr); - rtnval = (RTN_STATUS) motor_start_trans_com(mr, EMC18011_cards); - motor_call->type = EMC18011_table[command]; - - /* Keep track of currently selected motor */ - cntrl->motorSelect = signal; + sprintf(buff, "M%d", axis); + strcat(motor_call->message, buff); + motor_call->type = IMMEDIATE; /* Assure message gets sent */ + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, EMC18011_cards); + motor_call->type = EMC18011_table[command]; + } } break; default: break; } + + /* Only communicate to selected motor */ + if (cntrl->motorSelect != signal) + { + /* Assure continuous retries until other motion complete */ + motor_call->type = INFO; + if (mr->rcnt > 0) + mr->rcnt--; + return(rtnval = OK); + } + if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0) + { + strcat(motor_call->message, mr->init); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, EMC18011_cards); + motor_call->type = EMC18011_table[command]; + } switch (command) { case MOVE_ABS: case MOVE_REL: - case HOME_FOR: - case HOME_REV: case JOG: if (strlen(mr->prem) != 0) { @@ -257,17 +277,16 @@ STATIC RTN_STATUS EMC18011_build_trans(motor_cmnd command, double *parms, struct case MOVE_ABS: case MOVE_REL: { - char *xstart = &buff[1]; - buff[0] = (command == MOVE_ABS) ? 'G' : 'T'; - + buff[0] = (command == MOVE_ABS) ? 'G' : 'T'; + /* Maximum length for distance is 7 characters. */ sprintf(xstart, "%.1f", cntrl_units); if (strlen(xstart) > 7) /* Sacrifice precision for quantity */ sprintf(xstart, "%.0f", cntrl_units); - + if (strlen(xstart) > 7) { /* Put out maximum distance string */ @@ -277,7 +296,6 @@ STATIC RTN_STATUS EMC18011_build_trans(motor_cmnd command, double *parms, struct strcpy(xstart, "9999999"); } } - break; case HOME_FOR: diff --git a/motorApp/OrielSrc/drvEMC18011.cc b/motorApp/OrielSrc/drvEMC18011.cc index 3812c058..559d28d0 100755 --- a/motorApp/OrielSrc/drvEMC18011.cc +++ b/motorApp/OrielSrc/drvEMC18011.cc @@ -3,9 +3,9 @@ FILENAME... drvEMC18011.cc USAGE... Motor record driver level support for Spectra-Physics Encoder Mike Controller (Model: 18011) -Version: $Revision: 1.2 $ +Version: $Revision: 1.3 $ Modified By: $Author: sullivan $ -Last Modified: $Date: 2006-09-07 21:19:43 $ +Last Modified: $Date: 2006-11-02 21:05:56 $ */ @@ -61,6 +61,8 @@ Last Modified: $Date: 2006-09-07 21:19:43 $ #define CMD_SELECT "M" #define RTN_REMOTE "ON LINE" +#define RTN_READY "RE" // READY +#define RTN_OVERLOAD "OV" // OVERLOAD #define EMC18011_NUM_CARDS 16 @@ -224,12 +226,14 @@ static int set_status(int card, int signal) struct mess_node *nodeptr; register struct mess_info *motor_info; char send_buff[80]; - char Zstatus[2]; - char *startptr, *endptr; + char Zstatus; + char *recvStr; + char *brkptr, *endptr; int rtn_state; int recvCnt; int motorData; int motor; + double datad; bool recvRetry; bool plusdir, ls_active = false; msta_field status; @@ -242,35 +246,45 @@ static int set_status(int card, int signal) recvRetry = true; + /* Initialize motorData in-order to detect motor not moving */ + motorData = motor_info->position; + /* Status updates only available on selected motor */ if (signal == cntrl->motorSelect) { - startptr = cntrl->recv_string[0]; - send_mess(card, CMD_POS, NULL); - if ((recvCnt = recv_mess(card, startptr, 0))) + recvStr = cntrl->recv_string[0]; + /* Get Zstatus (one character motion indicator) */ + recvCnt = send_recv_mess(card, CMD_STATUS, recvStr, 1); + if (recvCnt == 1) { - double datad; + /* Test for valid reply */ + Zstatus = *recvStr; + if (Zstatus >= Z_STOPPED && Zstatus <= Z_LSUP) + recvRetry = false; + else + Zstatus = Z_RUNUP; - /* Convert position and check for error */ - datad = strtod(startptr, &endptr); - if (startptr != endptr) + /* Update position after motion has stopped */ + /* NOTE: This controller does not provide reliable position + * feedback during motion (BUG?) */ + if (Zstatus != Z_RUNUP && Zstatus != Z_RUNDOWN) { - motorData = NINT(datad / cntrl->drive_resolution); - recvRetry = false; - if (motorData == motor_info->position && motor_info->no_motion_count) + recvCnt = send_recv_mess(card, CMD_POS, recvStr); + if (recvCnt > 0) { - Zstatus[0] = Zstatus[1] = 0x0; // Null terminate - send_mess(card, CMD_STATUS, NULL); - recvCnt = recv_mess(card, Zstatus, 0, 1); - /* Check for valid response */ - if (recvCnt && *Zstatus >= Z_STOPPED && *Zstatus <= Z_LSUP) - recvRetry = false; + datad = strtod(recvStr, &endptr); + if (brkptr != endptr) + { + motorData = NINT(datad / cntrl->drive_resolution); + /* Release motor */ + cntrl->motorSelect = -1; + cntrl->motorLock->signal(); + recvRetry = false; + } else - /* Invalid response - assume the motor is still moving */ - *Zstatus = Z_RUNUP; + /* Don't indicate done until we get a valid position */ + Zstatus = Z_RUNUP; } - else - *Zstatus = Z_RUNUP; } } } @@ -278,10 +292,12 @@ static int set_status(int card, int signal) { /* Not the selected motor - no new information */ recvRetry = false; - motorData = motor_info->position; - Zstatus[0] = Z_STOPPED; + Zstatus = Z_STOPPED; + epicsThreadSleep(0.1); /* Pretend we did something - in-case of record retry loop */ } + + /* Check for normal look termination - all queries successful */ if (recvRetry == false) cntrl->status = NORMAL; @@ -320,19 +336,19 @@ static int set_status(int card, int signal) * Parse the status/fault string */ - Debug(5, "set_status(): status = %s\n", Zstatus); + Debug(5, "set_status(): status = %c\n", Zstatus); - plusdir = (*Zstatus == Z_STOPPED || *Zstatus == Z_RUNUP || *Zstatus == Z_LSUP) ? true : false; + plusdir = (Zstatus == Z_STOPPED || Zstatus == Z_RUNUP || Zstatus == Z_LSUP) ? true : false; status.Bits.RA_DIRECTION = plusdir ? 1 : 0; status.Bits.RA_HOME = 0; - status.Bits.RA_DONE = (*Zstatus == Z_STOPPED) ? 1 : 0; + status.Bits.RA_DONE = (Zstatus == Z_STOPPED) ? 1 : 0; /* Set Travel limit switch status bits. */ - status.Bits.RA_PLUS_LS = (*Zstatus == Z_LSUP) ? 1 : 0; - status.Bits.RA_MINUS_LS = (*Zstatus == Z_LSDOWN) ? 1 : 0; + status.Bits.RA_PLUS_LS = (Zstatus == Z_LSUP) ? 1 : 0; + status.Bits.RA_MINUS_LS = (Zstatus == Z_LSDOWN) ? 1 : 0; ls_active = (status.Bits.RA_PLUS_LS || status.Bits.RA_MINUS_LS) ? true : false; @@ -343,7 +359,9 @@ static int set_status(int card, int signal) status.Bits.EA_SLIP_STALL = 0; status.Bits.EA_HOME = 0; - if (motorData == motor_info->position) + /* Disable no motion test - this driver does not update position */ + // if (motorData == motor_info->position) + if (false) { if (nodeptr != 0) /* Increment counter only if motor is moving. */ motor_info->no_motion_count++; @@ -643,6 +661,8 @@ static int motor_init() total_cards = card_index + 1; cntrl = (struct EMC18011Controller *) brdptr->DevicePrivate; + cntrl->motorLock = new epicsEvent; + /* Initialize communications channel */ success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, cntrl->asyn_address, &cntrl->pasynUser, NULL); @@ -693,6 +713,9 @@ static int motor_init() { struct mess_info *motor_info = &brdptr->motor_info[motor_index]; + brdptr->motor_info[motor_index].motor_motion = NULL; + + motor_info->status.All = 0; motor_info->no_motion_count = 0; motor_info->encoder_position = 0; diff --git a/motorApp/OrielSrc/drvEMC18011.h b/motorApp/OrielSrc/drvEMC18011.h index 116b7acc..17b77005 100755 --- a/motorApp/OrielSrc/drvEMC18011.h +++ b/motorApp/OrielSrc/drvEMC18011.h @@ -3,9 +3,9 @@ FILENAME... drvEMC18011.h USAGE... This file contains Parker Compumotor driver "include" information that is specific to the 6K series serial controller -Version: $Revision: 1.2 $ +Version: $Revision: 1.3 $ Modified By: $Author: sullivan $ -Last Modified: $Date: 2006-09-07 21:19:43 $ +Last Modified: $Date: 2006-11-02 21:05:56 $ */ /* @@ -46,6 +46,7 @@ Last Modified: $Date: 2006-09-07 21:19:43 $ #include "motordrvCom.h" #include "asynDriver.h" #include "asynOctetSyncIO.h" +#include "epicsEvent.h" #define EMC18011_MAX_MOTORS 3 #define EMC18011_MSG_SIZE 120 @@ -77,6 +78,7 @@ struct EMC18011Controller int motorSelect; /* Keep track of currently selected motor - this */ /* is the only one that can get status information */ double drive_resolution; /* This controller has a fixed drive resolution */ + epicsEvent *motorLock; /* Only one axis can move at a time */ CommStatus status; /* Controller communication status. */ };