diff --git a/motorApp/MicroMoSrc/devMVP2001.cc b/motorApp/MicroMoSrc/devMVP2001.cc index 1b44106b..68d64ffc 100644 --- a/motorApp/MicroMoSrc/devMVP2001.cc +++ b/motorApp/MicroMoSrc/devMVP2001.cc @@ -1,16 +1,16 @@ /* FILENAME... devMVP2001.cc USAGE... Motor record device level support for MicroMo - MVP 2001 B02 (Linear, RS-485). + MVP 2001 B02 (Linear, RS-485). -Version: $Revision: 1.3 $ +Version: $Revision: 1.4 $ Modified By: $Author: sluiter $ -Last Modified: $Date: 2008-03-14 20:13:20 $ +Last Modified: $Date: 2008-06-06 17:16:51 $ */ /* - * Original Author: Kevin Peterson - * Date: 08/27/2002 + * Original Author: Kevin Peterson + * Date: 08/27/2002 * * * Illinois Open Source License @@ -48,7 +48,7 @@ Last Modified: $Date: 2008-03-14 20:13:20 $ * 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 @@ -76,13 +76,14 @@ Last Modified: $Date: 2008-03-14 20:13:20 $ * .04 08/30/02 kmp fixed problem with HOMF & HOMR. The MVP2001 does not * home commands. The correct way to disable the HOMF & * HOMR is to simply break from the switch statement. - * Doing anything else (i.e. "send = OFF;" or + * Doing anything else (i.e. "send = OFF;" or * "trans->state = IDLE_STATE;") causes the MEDM window * to get stuck in the "Moving" state even though the motor * is not moving. Use a current version of MEDM. * .05 10/02/02 kmp changed default current limit (in case where desired * current limit is out of range) from 500mA to 100mA. * .06 02/13/04 rls port to R3.14.x + * .07 06/06/08 rls Bug fix initializing driver twice. * */ @@ -99,6 +100,7 @@ extern struct driver_table MVP2001_access; #define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */ + /* ----------------Create the dsets for devMVP2001----------------- */ static struct driver_table *drvtabptr; static long MVP2001_init(void *); @@ -116,7 +118,7 @@ struct motor_dset devMVP2001 = MVP2001_end_trans }; -extern "C" {epicsExportAddress(dset,devMVP2001);} +extern "C"{epicsExportAddress(dset, devMVP2001);} /* --------------------------- program data --------------------- */ @@ -124,28 +126,28 @@ extern "C" {epicsExportAddress(dset,devMVP2001);} /* WARNING! this must match "motor_cmnd" in motor.h */ static msg_types MVP2001_table[] = { - MOTION, /* MOVE_ABS */ - MOTION, /* MOVE_REL */ - MOTION, /* HOME_FOR */ - MOTION, /* HOME_REV */ - IMMEDIATE, /* LOAD_POS */ - IMMEDIATE, /* SET_VEL_BASE */ - IMMEDIATE, /* SET_VELOCITY */ - IMMEDIATE, /* SET_ACCEL */ - IMMEDIATE, /* GO */ - IMMEDIATE, /* SET_ENC_RATIO */ - INFO, /* GET_INFO */ - MOVE_TERM, /* STOP_AXIS */ - VELOCITY, /* JOG */ - IMMEDIATE, /* SET_PGAIN */ - IMMEDIATE, /* SET_IGAIN */ - IMMEDIATE, /* SET_DGAIN */ - IMMEDIATE, /* ENABLE_TORQUE */ - IMMEDIATE, /* DISABL_TORQUE */ - IMMEDIATE, /* PRIMITIVE */ - IMMEDIATE, /* SET_HIGH_LIMIT */ - IMMEDIATE, /* SET_LOW_LIMIT */ - VELOCITY /* JOG_VELOCITY */ + MOTION, /* MOVE_ABS */ + MOTION, /* MOVE_REL */ + MOTION, /* HOME_FOR */ + MOTION, /* HOME_REV */ + IMMEDIATE, /* LOAD_POS */ + IMMEDIATE, /* SET_VEL_BASE */ + IMMEDIATE, /* SET_VELOCITY */ + IMMEDIATE, /* SET_ACCEL */ + IMMEDIATE, /* GO */ + IMMEDIATE, /* SET_ENC_RATIO */ + INFO, /* GET_INFO */ + MOVE_TERM, /* STOP_AXIS */ + VELOCITY, /* JOG */ + IMMEDIATE, /* SET_PGAIN */ + IMMEDIATE, /* SET_IGAIN */ + IMMEDIATE, /* SET_DGAIN */ + IMMEDIATE, /* ENABLE_TORQUE */ + IMMEDIATE, /* DISABL_TORQUE */ + IMMEDIATE, /* PRIMITIVE */ + IMMEDIATE, /* SET_HIGH_LIMIT */ + IMMEDIATE, /* SET_LOW_LIMIT */ + VELOCITY /* JOG_VELOCITY */ }; @@ -160,12 +162,7 @@ static long MVP2001_init(void *arg) long rtnval; int after = (arg == 0) ? 0 : 1; - if (after == 0) - { - drvtabptr = &MVP2001_access; - (drvtabptr->init)(); - } - + drvtabptr = &MVP2001_access; rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &MVP2001_cards); return(rtnval); } @@ -205,40 +202,40 @@ static RTN_STATUS MVP2001_build_trans(motor_cmnd command, double *parms, struct char prem[3] = {'A', 'N', 'O'}; int card, axis; unsigned int size; - int sp, ac, i, j, ano; + int sp, ac, i, j, ano; RTN_STATUS rtnval; epicsInt32 cntrl_units = 0; double dval; bool send; - send = true; /* Default to send motor command. */ + send = true; /* Default to send motor command. */ rtnval = OK; buff[0] = '\0'; dval = *parms; motor_start_trans_com(mr, MVP2001_cards); - + motor_call = &(trans->motor_call); card = motor_call->card; axis = motor_call->signal + 1; brdptr = (*trans->tabptr->card_array)[card]; if (brdptr == NULL) - return(rtnval = ERROR); + return(rtnval = ERROR); cntrl = (struct MVPcontroller *) brdptr->DevicePrivate; cntrl_units = (epicsInt32) NINT(dval); - + if (MVP2001_table[command] > motor_call->type) - motor_call->type = MVP2001_table[command]; + motor_call->type = MVP2001_table[command]; if (trans->state != BUILD_STATE) - return(rtnval = ERROR); + return(rtnval = ERROR); if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0) { - sprintf(buff, "%d %s", axis, mr->init); + sprintf(buff, "%d %s", axis, mr->init); strcpy(motor_call->message, buff); - buff[0] = '\0'; + buff[0] = '\0'; rtnval = motor_end_trans_com(mr, drvtabptr); rtnval = (RTN_STATUS) motor_start_trans_com(mr, MVP2001_cards); motor_call->type = MVP2001_table[command]; @@ -246,227 +243,232 @@ static RTN_STATUS MVP2001_build_trans(motor_cmnd command, double *parms, struct switch (command) { - case MOVE_ABS: - case MOVE_REL: - case JOG: - if (strlen(mr->prem) != 0) - { - if (strncmp(mr->prem, prem, 3) == 0) - { - /* - * If the current limit has not already been calculated, the - * calculation is performed using the current limit specified - * in the parm string of the out field (in mA). - * The parm string has the form: "ENC_CPR,MAX_CURRENT_in_mA" - */ - if (cntrl->maxCurrent[axis - 1] == NULL) - { - i = 0; - strcpy(parm, mr->out.value.vmeio.parm); + case MOVE_ABS: + case MOVE_REL: + case JOG: + if (strlen(mr->prem) != 0) + { + if (strncmp(mr->prem, prem, 3) == 0) + { + /* + * If the current limit has not already been calculated, the + * calculation is performed using the current limit specified + * in the parm string of the out field (in mA). The parm string + * has the form: "ENC_CPR,MAX_CURRENT_in_mA" + */ + if (cntrl->maxCurrent[axis - 1] == 0) + { + i = 0; + strcpy(parm, mr->out.value.vmeio.parm); - /* get the max current from the parm string */ - while (parm[i] != ',') - { - i++; - } - i++; - j = i; - while (isdigit(parm[i])) - { - max_mA[i-j] = parm[i]; - i++; - } - max_mA[i-j] = '\0'; - cntrl->maxCurrent[axis - 1] = atoi(max_mA); - } + /* get the max current from the parm string */ + while (parm[i] != ',') + { + i++; + } + i++; + j = i; + while (isdigit(parm[i])) + { + max_mA[i - j] = parm[i]; + i++; + } + max_mA[i - j] = '\0'; + cntrl->maxCurrent[axis - 1] = atoi(max_mA); + } - /* The MVP2001 manual implies that the range 0.1-2.3 Amps is ok */ - if (cntrl->maxCurrent[axis - 1] < 100 || cntrl->maxCurrent[axis - 1] > 2300) - cntrl->maxCurrent[axis - 1] = 100; - - /* The values in the ano calc are determined from data in the MVP manual */ - ano = NINT(cntrl->maxCurrent[axis - 1] * 0.865909 + 2103.431); - sprintf(buff, "%d ANO %d", axis, ano); - } - else - { - sprintf(buff, "%d %s", axis, mr->prem); - } + /* The MVP2001 manual implies that the range 0.1-2.3 Amps is ok */ + if (cntrl->maxCurrent[axis - 1] < 100 || cntrl->maxCurrent[axis - 1] > 2300) + cntrl->maxCurrent[axis - 1] = 100; - strcpy(motor_call->message, buff); - buff[0] = '\0'; - /* end ANO command trans because MVP can't handle multiple commands */ - rtnval = motor_end_trans_com(mr, drvtabptr); - /* begin the original transaction again (command) */ - rtnval = (RTN_STATUS) motor_start_trans_com(mr, MVP2001_cards); - motor_call->type = MVP2001_table[command]; - } - /* - * The following probably will not work for the MVP2001 as the - * mr->post field is not ready to be sent in it's primitive form - */ - if (strlen(mr->post) != 0) - motor_call->postmsgptr = (char *) &mr->post; - break; + /* + * The values in the ano calc are determined from data in the + * MVP manual + */ + ano = NINT(cntrl->maxCurrent[axis - 1] * 0.865909 + 2103.431); + sprintf(buff, "%d ANO %d", axis, ano); + } + else + { + sprintf(buff, "%d %s", axis, mr->prem); + } - case HOME_FOR: - case HOME_REV: - default: - break; + strcpy(motor_call->message, buff); + buff[0] = '\0'; + /* end ANO command trans because MVP can't handle multiple commands */ + rtnval = motor_end_trans_com(mr, drvtabptr); + /* begin the original transaction again (command) */ + rtnval = (RTN_STATUS) motor_start_trans_com(mr, MVP2001_cards); + motor_call->type = MVP2001_table[command]; + } + /* + * The following probably will not work for the MVP2001 as the mr->post + * field is not ready to be sent in it's primitive form + */ + if (strlen(mr->post) != 0) + motor_call->postmsgptr = (char *) &mr->post; + break; + + case HOME_FOR: + case HOME_REV: + default: + break; } switch (command) { - case MOVE_ABS: - sprintf(buff, "%d LA %d", axis, cntrl_units); - break; - - case MOVE_REL: - sprintf(buff, "%d LR %d", axis, cntrl_units); - break; - - case HOME_FOR: - case HOME_REV: - break; - - case LOAD_POS: - sprintf(buff, "%d HO %d", axis, cntrl_units); - break; - - case SET_VEL_BASE: - send = false; - break; - - case SET_VELOCITY: - cntrl_units = NINT(dval * 0.03); - sprintf(buff, "%d SP %d", axis, cntrl_units); - break; - - /* - * The calculation of the acceleration requires the number of - * encoder counts per revolution. This value is passed in - * through the parm string of the out field. the parm string - * has the form: "ENC_CPR,MAX_CURRENT_in_mA" - */ - case SET_ACCEL: - /* - * If the Encoder counts per revolution has already been calculated - * and stored into the device private structure, then it will not - * be recalculated. - */ - if (cntrl->encoderCpr[axis - 1] == NULL) - { - i = 0; - strcpy(parm, mr->out.value.vmeio.parm); + case MOVE_ABS: + sprintf(buff, "%d LA %d", axis, cntrl_units); + break; - /* get the encoder cpr from the parm string */ - while(isdigit(parm[i])) - { - enc_cpr[i] = parm[i]; - i++; - } - enc_cpr[i] = '\0'; - cntrl->encoderCpr[axis - 1] = atoi(enc_cpr); - } - - sp = NINT(mr->velo / fabs(mr->mres) * 0.03); - ac = NINT(dval * 0.03 * cntrl->encoderCpr[axis - 1] * 0.0000625); - if (ac < sp) - { - if (ac > 0) - cntrl_units = ac; - else - cntrl_units = 1; - } - else - { - cntrl_units = sp; - } - sprintf(buff, "%d AC %d", axis, cntrl_units); - break; - - case GO: - sprintf(buff, "%d M", axis); - break; - - case PRIMITIVE: - case GET_INFO: - /* These commands are not actually done by sending a message, but - rather they will indirectly cause the driver to read the status - of all motors */ - break; - - case STOP_AXIS: - sprintf(buff, "%d AB", axis); - break; - - case JOG_VELOCITY: - case JOG: - cntrl_units = NINT(dval * 0.03); - sprintf(buff, "%d V %d", axis, cntrl_units); - break; - - case SET_PGAIN: - sprintf(buff, "%d POR %ld", axis, (NINT(dval * 28000 + 4000))); - break; - case SET_IGAIN: - sprintf(buff, "%d I %ld", axis, (NINT(dval * 31999 + 1))); - break; - case SET_DGAIN: - sprintf(buff, "%d DER %ld", axis, (NINT(dval * 31000 + 1000))); - break; - - case ENABLE_TORQUE: - sprintf(buff, "%d EN", axis); - break; - - case DISABL_TORQUE: - sprintf(buff, "%d DI", axis); - break; - - case SET_HIGH_LIMIT: - motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1]; - trans->state = IDLE_STATE; /* No command sent to the controller. */ + case MOVE_REL: + sprintf(buff, "%d LR %d", axis, cntrl_units); + break; - if (cntrl_units > motor_info->high_limit) - { - mr->dhlm = motor_info->high_limit * fabs(mr->mres); - rtnval = ERROR; - } - break; + case HOME_FOR: + case HOME_REV: + break; - case SET_LOW_LIMIT: - motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1]; - trans->state = IDLE_STATE; /* No command sent to the controller. */ + case LOAD_POS: + sprintf(buff, "%d HO %d", axis, cntrl_units); + break; - if (cntrl_units < motor_info->low_limit) - { - mr->dllm = motor_info->low_limit * fabs(mr->mres); - rtnval = ERROR; - } - break; + case SET_VEL_BASE: + send = false; + break; - case SET_ENC_RATIO: - trans->state = IDLE_STATE; /* No command sent to the controller. */ - send = false; - break; - - default: - send = false; - rtnval = ERROR; + case SET_VELOCITY: + cntrl_units = NINT(dval * 0.03); + sprintf(buff, "%d SP %d", axis, cntrl_units); + break; + + /* + * The calculation of the acceleration requires the number of encoder + * counts per revolution. This value is passed in through the parm + * string of the out field. the parm string has the form: + * "ENC_CPR,MAX_CURRENT_in_mA" + */ + case SET_ACCEL: + /* + * If the Encoder counts per revolution has already been calculated and + * stored into the device private structure, then it will not be + * recalculated. + */ + if (cntrl->encoderCpr[axis - 1] == 0) + { + i = 0; + strcpy(parm, mr->out.value.vmeio.parm); + + /* get the encoder cpr from the parm string */ + while (isdigit(parm[i])) + { + enc_cpr[i] = parm[i]; + i++; + } + enc_cpr[i] = '\0'; + cntrl->encoderCpr[axis - 1] = atoi(enc_cpr); + } + + sp = NINT(mr->velo / fabs(mr->mres) * 0.03); + ac = NINT(dval * 0.03 * cntrl->encoderCpr[axis - 1] * 0.0000625); + if (ac < sp) + { + if (ac > 0) + cntrl_units = ac; + else + cntrl_units = 1; + } + else + { + cntrl_units = sp; + } + sprintf(buff, "%d AC %d", axis, cntrl_units); + break; + + case GO: + sprintf(buff, "%d M", axis); + break; + + case PRIMITIVE: + case GET_INFO: + /* + * These commands are not actually done by sending a message, but + * rather they will indirectly cause the driver to read the status of + * all motors + */ + break; + + case STOP_AXIS: + sprintf(buff, "%d AB", axis); + break; + + case JOG_VELOCITY: + case JOG: + cntrl_units = NINT(dval * 0.03); + sprintf(buff, "%d V %d", axis, cntrl_units); + break; + + case SET_PGAIN: + sprintf(buff, "%d POR %ld", axis, (NINT(dval * 28000 + 4000))); + break; + case SET_IGAIN: + sprintf(buff, "%d I %ld", axis, (NINT(dval * 31999 + 1))); + break; + case SET_DGAIN: + sprintf(buff, "%d DER %ld", axis, (NINT(dval * 31000 + 1000))); + break; + + case ENABLE_TORQUE: + sprintf(buff, "%d EN", axis); + break; + + case DISABL_TORQUE: + sprintf(buff, "%d DI", axis); + break; + + case SET_HIGH_LIMIT: + motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1]; + trans->state = IDLE_STATE; /* No command sent to the controller. */ + + if (cntrl_units > motor_info->high_limit) + { + mr->dhlm = motor_info->high_limit * fabs(mr->mres); + rtnval = ERROR; + } + break; + + case SET_LOW_LIMIT: + motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1]; + trans->state = IDLE_STATE; /* No command sent to the controller. */ + + if (cntrl_units < motor_info->low_limit) + { + mr->dllm = motor_info->low_limit * fabs(mr->mres); + rtnval = ERROR; + } + break; + + case SET_ENC_RATIO: + trans->state = IDLE_STATE; /* No command sent to the controller. */ + send = false; + break; + + default: + send = false; + rtnval = ERROR; } size = strlen(buff); if (send == false) - return(rtnval); + return(rtnval); else if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE) - errlogMessage("MVP2001_build_trans(): buffer overflow.\n"); + errlogMessage("MVP2001_build_trans(): buffer overflow.\n"); else { - strcat(motor_call->message, buff); - motor_end_trans_com(mr, drvtabptr); + strcat(motor_call->message, buff); + motor_end_trans_com(mr, drvtabptr); } - + return(rtnval); }