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

Bug fix initializing driver twice.

Browse Source
This commit is contained in:
Ron Sluiter
2008-06-06 17:16:51 +00:00
parent 781dc22517
commit 5f6e872008
1 changed files with 247 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/MicroMoSrc/devMVP2001.cc
+247 -245
View File
@@ -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);
}