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

Start

Browse Source
This commit is contained in:
Ron Sluiter
2004-03-03 20:02:58 +00:00
parent 7dc4614c43
commit 9847893e42
6 changed files with 1433 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/MicroMoSrc/Makefile
+20
View File
@@ -0,0 +1,20 @@
# Makefile
TOP = ../..
include $(TOP)/configure/CONFIG
# The following are used for debugging messages.
#USR_CXXFLAGS += -DDEBUG
DBD += devMicroMo.dbd
LIBRARY_IOC = MicroMo
# Intelligent Motion Systems driver support.
SRCS += MicroMoRegister.cc
SRCS += devMVP2001.cc drvMVP2001.cc
MicroMo_LIBS += motor motorCOM_mpf
MicroMo_LIBS += $(EPICS_BASE_IOC_LIBS)
include $(TOP)/configure/RULES
motorApp/MicroMoSrc/MicroMoRegister.cc
+71
View File
@@ -0,0 +1,71 @@
/*
FILENAME... MicroMoRegister.cc
USAGE... Register MicroMo MVP 2001 B02 motor controller device driver shell
commands.
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-03-03 20:02:56 $
*/
/*****************************************************************
COPYRIGHT NOTIFICATION
*****************************************************************
(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO
This software was developed under a United States Government license
described on the COPYRIGHT_UniversityOfChicago file included as part
of this distribution.
**********************************************************************/
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <iocsh.h>
#include "motor.h"
#include "drvMVP2001.h"
#include "epicsExport.h"
extern "C"
{
// MicroMo Setup arguments
static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt};
static const iocshArg setupArg1 = {"Max. motor count", iocshArgInt};
static const iocshArg setupArg2 = {"Polling rate", iocshArgInt};
// MicroMo Config arguments
static const iocshArg configArg0 = {"Card being configured", iocshArgInt};
static const iocshArg configArg1 = {"MPF server location", iocshArgInt};
static const iocshArg configArg2 = {"MPF server task name", iocshArgString};
static const iocshArg * const MicroMoSetupArgs[3] = {&setupArg0, &setupArg1,
&setupArg2};
static const iocshArg * const MicroMoConfigArgs[3] = {&configArg0, &configArg1,
&configArg2};
static const iocshFuncDef setupMicroMo = {"MicroMoSetup", 3, MicroMoSetupArgs};
static const iocshFuncDef configMicroMo = {"MicroMoConfig", 3, MicroMoConfigArgs};
static void setupMicroMoCallFunc(const iocshArgBuf *args)
{
MVP2001Setup(args[0].ival, args[1].ival, args[2].ival);
}
static void configMicroMoCallFunc(const iocshArgBuf *args)
{
MVP2001Config(args[0].ival, args[1].ival, args[2].sval);
}
static void MicroMomotorRegister(void)
{
iocshRegister(&setupMicroMo, setupMicroMoCallFunc);
}
epicsExportRegistrar(MicroMomotorRegister);
} // extern "C"
motorApp/MicroMoSrc/devMVP2001.cc
+472
View File
@@ -0,0 +1,472 @@
/*
FILENAME... devMVP2001.cc
USAGE... Motor record device level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-03-03 20:02:57 $
*/
/*
* 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
*
*/
#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
};
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[] = {
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 */
};
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 = (int) arg;
if (after == 0)
{
drvtabptr = &MVP2001_access;
(drvtabptr->init)();
}
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] == NULL)
{
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] == NULL)
{
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);
}
motorApp/MicroMoSrc/devMicroMo.dbd
+4
View File
@@ -0,0 +1,4 @@
# Micro Mo MVP 2001 Driver support.
device(motor,VME_IO,devMVP2001,"MVP2001")
driver(drvMVP2001)
motorApp/MicroMoSrc/drvMVP2001.cc
+741
View File
@@ -0,0 +1,741 @@
/*
FILENAME... drvMVP2001.cc
USAGE... Motor record driver level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-03-03 20:02:57 $
*/
/*
* 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 drvIM483PL.c (rev 1.7, mod .03) and
* customized for the MVP2001.
* .02 08/27/02 kmp changed message construction to allow for addresses
* larger than 9.
* .03 09/06/02 kmp added an extra loop to motor_init() that sends the HO
* command a second time to ensure the position is set to
* zero. Previously, saved positions would not be loaded
* if the controller was power-cycled.
* .04 02/06/04 rls Eliminate erroneous "Motor motion timeout ERROR".
* .05 02/13/04 rls port to R3.14.x
*
*/
/*
DESIGN LIMITATIONS...
1 - Like all controllers, the MVP2001 must be powered-on when EPICS is first
booted up.
2 - The MVP2001 cannot be power cycled while EPICS is up and running. The
consequences are permanent communication loss with the MVP2001 until
EPICS is rebooted.
3 - Translation between the MVP2001 and the ACCL/BACC fields is not obvious.
*/
/*
MORE DESIGN LIMITATIONS
1 - For the most part the standard terminology (card-signal-axis) has been
used here so that this code resembles other drivers. Unfortunately,
the terminology is not the best for this controller. The MVP2001 is a
single-axis, RS-485-daisy-chainable, DC controller. The following
equations succinctly illustrate the relationship between the physical
setup and the standard terminology:
card = chain of MVP2001 controllers
signal = axis = one of the MVP2001 controllers on a chain
2 - Strtol and strtoul have been switched in the vxWorks that KMP used
at the time that this was being written. If your vxWorks functions
behave correctly, then they will have to be switched in the code.
3 - Factors that currently limit the number of controllers on one chain:
A. MVP2001 addresses
The MVP2001 can have an address of 1-64 for serial communication.
B. RS-485 communication degradation
There is a practical limit to how many controllers can be on one chain
C. The motor_info array of the controller structure in motordrvCom.h
For a chain to work correctly, there needs to be one element in the
motor_info array for every controller on the chain. The limit is
set by the constant MAX_AXIS, which is defined in motor.h. The end
result is that the number of controllers is limited by the motor
record. The current maximum number of controllers is 10.
*/
#include <string.h>
#include <epicsThread.h>
#include <drvSup.h>
#include "motor.h"
#include "drvMVP2001.h"
#include "serialIO.h"
#include "epicsExport.h"
#define MVP2001_NUM_CARDS 8
#define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */
/*----------------debugging-----------------*/
#ifdef __GNUG__
#ifdef DEBUG
volatile int drvMVP2001debug = 0;
#define Debug(l, f, args...) { if(l<=drvMVP2001debug) printf(f,## args); }
#else
#define Debug(l, f, args...)
#endif
#else
#define Debug()
#endif
/* --- Local data. --- */
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 card, char const *com, char c);
static int set_status(int card, int signal);
static long report(int level);
static long init();
static int motor_init();
static void query_done(int, int, struct mess_node *);
/*----------------functions-----------------*/
struct driver_table MVP2001_access =
{
motor_init,
motor_send,
motor_free,
motor_card_info,
motor_axis_info,
&mess_queue,
&queue_lock,
&free_list,
&freelist_lock,
&motor_sem,
&motor_state,
&total_cards,
&any_motor_in_motion,
send_mess,
recv_mess,
set_status,
query_done,
NULL,
&initialized,
NULL
};
struct
{
long number;
long (*report) (int);
long (*init) (void);
} drvMVP2001 = {2, report, init};
epicsExportAddress(drvet, drvMVP2001);
static struct thread_args targs = {SCAN_RATE, &MVP2001_access};
/*********************************************************
* Print out driver status report
*********************************************************/
static long report(int level)
{
int card;
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
{
struct MVPcontroller *cntrl;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
switch (cntrl->port_type)
{
case RS232_PORT:
printf(" MVP2001 controller chain %d port type = RS-232, id: %s \n",
card,
brdptr->ident);
break;
default:
printf(" MVP2001 controller chain %d port type = Unknown, id: %s \n",
card,
brdptr->ident);
break;
}
}
}
}
return (0);
}
static long init()
{
/* initialize all hardware and software */
motor_init();
/* Check for setup */
if (MVP2001_num_cards <= 0)
{
Debug(1, "init(): MVP2001 driver disabled. MVP2001Setup() missing \
from startup script.\n");
}
return ((long) 0);
}
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 *
* *
* *
********************************************************************************/
/*
* When the motor record calls set_status it passes it card and signal values
* that are found in the mvpMotors template (or the OUT field of the M.R.)
*/
static int set_status(int card, int signal)
{
struct MVPcontroller *cntrl;
struct mess_node *nodeptr;
register struct mess_info *motor_info;
/* Message parsing variables */
char buff[BUFF_SIZE];
char statusStr[BUFF_SIZE], positionStr[BUFF_SIZE];
int rtn_state;
epicsInt32 motorData;
MOTOR_STATUS mstat;
bool plusdir, ls_active = false;
msta_field status;
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
motor_info = &(motor_state[card]->motor_info[signal]);
nodeptr = motor_info->motor_motion;
status.All = motor_info->status.All;
statusStr[0] = positionStr[0] = buff[0] = '\0';
sprintf(buff, "%d ST", (signal + 1));
send_mess(card, buff, (char) NULL);
rtn_state = recv_mess(card, buff, 1);
if (rtn_state > 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;
}
}
/*
* 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;
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
*/
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.
*/
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
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;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
/* Set limit switch error indicators. */
if (mstat.Bits.plusLS == false)
status.Bits.RA_PLUS_LS = 0;
else
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
if (mstat.Bits.minusLS == false)
status.Bits.RA_MINUS_LS = 0;
else
{
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!!!*/
status.Bits.EA_POSITION = 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
if (motor_state[card]->motor_info[signal].encoder_present == NO)
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;
}
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
/* NEEDS WORK */
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
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)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
motor_info->status.All = status.All;
return(rtn_state);
}
/*****************************************************/
/* send a message to the MVP2001 board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char inchar)
{
char local_buff[MAX_MSG_SIZE];
struct MVPcontroller *cntrl;
int size;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvMVP2001.c:send_mess(); message size violation.\n");
return(ERROR);
}
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);
}
/* Make a local copy of the string and add the command line terminator. */
strcpy(local_buff, com);
strcat(local_buff, "\r");
if (inchar != (char) NULL)
local_buff[0] = inchar; /* put in axis */
Debug(2, "send_mess(): message = %s\n", local_buff);
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
cntrl->serialInfo->serialIOSend(local_buff, strlen(local_buff), SERIAL_TIMEOUT);
return(OK);
}
/*****************************************************/
/* receive a message from the MVP2001 board */
/* recv_mess() */
/*****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct MVPcontroller *cntrl;
char temp[BUFF_SIZE];
int timeout;
int len=0, lenTemp=0;
/* Check that card exists */
if (!motor_state[card])
return (-1);
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
if (flag == FLUSH)
timeout = 0;
else
timeout = SERIAL_TIMEOUT;
lenTemp = cntrl->serialInfo->serialIORecv(temp, BUFF_SIZE, (char *) "\n", timeout);
len = cntrl->serialInfo->serialIORecv(com, BUFF_SIZE, (char *) "\n", timeout);
Debug(5, "bytes: 1st call: %d\t2nd call: %d\n", lenTemp, len);
if (len == 0)
com[0] = '\0';
else
com[len - 1] = '\0';
Debug(2, "recv_mess(): message = \"%s\"\n", com);
return (len);
}
/*****************************************************/
/* Setup system configuration */
/* MVP2001Setup() */
/*****************************************************/
RTN_STATUS
MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
int num_channels, /* NOT Used. */
int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
{
if (num_cards < 1 || num_cards > MVP2001_NUM_CARDS)
MVP2001_num_cards = MVP2001_NUM_CARDS;
else
MVP2001_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
else
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
*/
motor_state = (struct controller **) calloc(MVP2001_num_cards,
sizeof(struct controller *));
return(OK);
}
/*******************************************************
* Configure a CHAIN of controllers *
* *
* Note: Addresses of controllers on a chain must *
* begin at 1 and follow sequentially. *
* *
* MVP2001Config() *
********************************************************/
RTN_STATUS
MVP2001Config(int card, /* CHAIN being configured */
int port_type, /* 1:RS232_PORT */
int location, /* MPF server location */
const char *name) /* MPF server task name */
{
struct MVPcontroller *cntrl;
if (card < 0 || card >= MVP2001_num_cards)
return (ERROR);
motor_state[card] = (struct controller *) calloc(1, sizeof(struct controller));
motor_state[card]->DevicePrivate = calloc(1, sizeof(struct MVPcontroller));
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
switch (port_type)
{
case GPIB_PORT:
/* GPIB not possible with MVP2001 */
break;
case RS232_PORT:
cntrl->port_type = port_type;
cntrl->serial_card = location;
strcpy(cntrl->serial_task, name);
break;
/* DeviceNet not yet implemented */
default:
return (ERROR);
}
return(OK);
}
/*****************************************************/
/* initialize all software and hardware */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
struct controller *brdptr;
struct MVPcontroller *cntrl;
int card_index, motor_index;
char buff[BUFF_SIZE], limitStr[BUFF_SIZE];
int total_axis = 0;
int status;
bool success_rtn;
buff[0] = limitStr[0] = '\0';
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (MVP2001_num_cards <= 0)
return (ERROR);
for (card_index = 0; card_index < MVP2001_num_cards; card_index++)
{
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;
/* Initialize communications channel */
success_rtn = false;
cntrl->serialInfo = new serialIO(cntrl->serial_card,
cntrl->serial_task, &success_rtn);
if (success_rtn == false)
{
/* Send a message to the board, see if it exists */
for (total_axis = 0; total_axis < MAX_AXIS; 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);
sprintf(buff, "%d ST", (total_axis + 1));
send_mess(card_index, buff, (char) NULL);
status = recv_mess(card_index, buff, 1);
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);
}
if (success_rtn == false && 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);
}
/*
* 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;
}
any_motor_in_motion = 0;
mess_queue.head = (struct mess_node *) NULL;
mess_queue.tail = (struct mess_node *) NULL;
free_list.head = (struct mess_node *) NULL;
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "MVP2001_motor", 64, 5000, (EPICSTHREADFUNC) motor_task, (void *) &targs);
return(0);
}
motorApp/MicroMoSrc/drvMVP2001.h
+125
View File
@@ -0,0 +1,125 @@
/*
FILENAME... drvMVP2001.h
USAGE... This file contains driver "include" information that is specific to
the MicroMo MVP 2001 B02 (Linear, RS-485).
Version: $Revision: 1.2 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-03-03 20:02:58 $
*/
/*
* 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/2002 kmp copied from drvIM483.h (rev 1.1, mod .01) and
* customized for the MVP2001.
*
*/
#ifndef INCdrvMVP2001h
#define INCdrvMVP2001h 1
#include "motor.h"
#include "motordrvCom.h"
#include "serialIO.h"
#define SERIAL_TIMEOUT 2000 /* Command timeout in msec */
/* MVP2001 (chain) specific data is stored in this structure. */
struct MVPcontroller
{
int port_type; /* GPIB_PORT or RS232_PORT */
serialIO *serialInfo; /* For RS-232 */
int serial_card; /* Card on which Hideos is running */
char serial_task[20]; /* Hideos task name for serial port */
int maxCurrent[MAX_AXIS]; /* Maximum current (in mA) */
int encoderCpr[MAX_AXIS]; /* Encoder counts per revolution */
CommStatus status; /* Controller communication status. */
};
/* Motor status response for MVP2001. */
typedef union
{
epicsUInt16 All;
struct
{
bool minusLS :1; /* negative limit switch hit */
bool extEvent2 :1; /* external event #2 */
bool plusLS :1; /* positive limit switch hit */
bool extEvent1 :1; /* external event #1 */
bool emergencyStop :1; /* emergency stop flag is active */
bool localMode :1; /* 1:local mode - 0:remote mode */
bool softLimit :1; /* soft-limit has been reached */
bool NOT_power :1; /* motor power 0 - ON; 1 - OFF. */
bool offTraj :1; /* off traj (more than FD com. value) */
bool devicenetErr :1; /* error in devicenet message packets */
bool devicenet :1; /* devicenet connection active */
bool trajComplete :1; /* 1=traj (set by T com.) is complete */
bool aiState :1; /* ANM mode not used--always 1 */
bool operatingMode :1; /* 1:Velocity - 0:Position */
bool inPosition :1; /* in-position indicator */
bool inMotion :1; /* in-motion indicator */
} Bits;
} MOTOR_STATUS;
/* Function prototypes. */
extern RTN_STATUS MVP2001Setup(int, int, int);
extern RTN_STATUS MVP2001Config(int, int, const char *);
#endif /* INCdrvMVP2001h */