lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity
Files
7cd511c1942cc8afd96de8fa907501b7cbeed67f
motorBase/motorApp/MicroMoSrc/devMVP2001.cc
T
kmpeters 3ef4b54215 Multiple fixes for MVP2001 driver
2011-03-18 19:39:24 +00:00

477 lines
14 KiB
C++
Raw Blame History

/*
FILENAME... devMVP2001.cc
USAGE... Motor record device level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
Version: $Revision: 1.4 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2008-06-06 17:16:51 $
*/
/*
* Original Author: Kevin Peterson
* Date: 08/27/2002
*
*
* Illinois Open Source License
* University of Illinois
* Open Source License
*
*
* Copyright (c) 2004, UNICAT. All rights reserved.
*
*
* Developed by:
*
* UNICAT, Advanced Photon Source, Argonne National Laboratory
*
* Frederick Seitz Materials Research Laboratory,
* University of Illinois at Urbana-Champaign
*
* http://www.uni.aps.anl.gov
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the
* "Software"), to deal with the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
*
* Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimers.
*
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimers in the
* documentation and/or other materials provided with the distribution.
*
*
* 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
* permission.
*
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR
* ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE SOFTWARE.
*
*
* Modification Log:
* -----------------
* .01 08/27/02 kmp copied from devIM483PL.c (rev 1.9, mod .04) and
* customized for the MVP2001.
* .02 08/27/02 kmp changed message construction to allow for addresses
* larger than 9.
* .03 08/29/02 kmp fixed the sending of the ANO command. Previously had
* attempted to send it during "case GO:" but now properly
* handled using the mr->prem. Also implemented JOG.
* .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
* "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.
* .08 03/18/11 kmp Corrected the MVP2001_table structure; only the "GO"
* command causes the motor to move.
*
*/
#include <string.h>
#include <math.h>
#include <ctype.h>
#include "motorRecord.h"
#include "motor.h"
#include "motordevCom.h"
#include "drvMVP2001.h"
#include "epicsExport.h"
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 *);
static long MVP2001_init_record(void *);
static long MVP2001_start_trans(struct motorRecord *);
static RTN_STATUS MVP2001_build_trans(motor_cmnd, double *, struct motorRecord *);
static RTN_STATUS MVP2001_end_trans(struct motorRecord *);
struct motor_dset devMVP2001 =
{
{8, NULL, (DEVSUPFUN) MVP2001_init, (DEVSUPFUN) MVP2001_init_record, NULL},
motor_update_values,
MVP2001_start_trans,
MVP2001_build_trans,
MVP2001_end_trans
};
extern "C"{epicsExportAddress(dset, devMVP2001);}
/* --------------------------- program data --------------------- */
/* This table is used to define the command types */
/* WARNING! this must match "motor_cmnd" in motor.h */
static msg_types MVP2001_table[] = {
IMMEDIATE, /* MOVE_ABS */
IMMEDIATE, /* MOVE_REL */
IMMEDIATE, /* HOME_FOR */
IMMEDIATE, /* HOME_REV */
IMMEDIATE, /* LOAD_POS */
IMMEDIATE, /* SET_VEL_BASE */
IMMEDIATE, /* SET_VELOCITY */
IMMEDIATE, /* SET_ACCEL */
MOTION, /* 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 */
};
static struct board_stat **MVP2001_cards;
/* --------------------------- program data --------------------- */
/* initialize device support for MVP2001 DC motor */
static long MVP2001_init(void *arg)
{
long rtnval;
int after = (arg == 0) ? 0 : 1;
drvtabptr = &MVP2001_access;
rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &MVP2001_cards);
return(rtnval);
}
/* initialize a record instance */
static long MVP2001_init_record(void *arg)
{
struct motorRecord *mr = (struct motorRecord *) arg;
return(motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, drvtabptr, MVP2001_cards));
}
/* start building a transaction */
static long MVP2001_start_trans(struct motorRecord *mr)
{
return(OK);
}
/* end building a transaction */
static RTN_STATUS MVP2001_end_trans(struct motorRecord *mr)
{
return(OK);
}
/* add a part to the transaction */
static RTN_STATUS MVP2001_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr)
{
struct motor_trans *trans = (struct motor_trans *) mr->dpvt;
struct mess_node *motor_call;
struct controller *brdptr;
struct mess_info *motor_info;
struct MVPcontroller *cntrl;
char buff[BUFF_SIZE], enc_cpr[10], max_mA[6], parm[31];
char prem[3] = {'A', 'N', 'O'};
int card, axis;
unsigned int size;
int sp, ac, i, j, ano;
RTN_STATUS rtnval;
epicsInt32 cntrl_units = 0;
double dval;
bool send;
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);
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
cntrl_units = (epicsInt32) NINT(dval);
if (MVP2001_table[command] > motor_call->type)
motor_call->type = MVP2001_table[command];
if (trans->state != BUILD_STATE)
return(rtnval = ERROR);
if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0)
{
sprintf(buff, "%d %s", axis, mr->init);
strcpy(motor_call->message, buff);
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];
}
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] == 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);
}
/* 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);
}
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] == 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);
else if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE)
errlogMessage("MVP2001_build_trans(): buffer overflow.\n");
else
{
strcat(motor_call->message, buff);
motor_end_trans_com(mr, drvtabptr);
}
return(rtnval);
}
View Git Blame Copy Permalink