lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity
Files
2f75044d817ef4e9a44c2995e128200ecfd775f2
motorBase/motorApp/V544Src/devV544.c
T
Ron Sluiter 77b3a342cc Restored SET_ENC_RATIO command.
2002-07-05 17:03:39 +00:00

689 lines
19 KiB
C
Raw Blame History

/* File: devV544.c */
/* Version: 1.2a */
/* Date Last Modified: 2/19/97 */
/* Device Support Routines for motor */
/*
* Original Author: Jim Kowalkowski
* Current Author: Joe Sullivan
* Date: 11/14/94
*
* Experimental Physics and Industrial Control System (EPICS)
*
* Copyright 1991, the Regents of the University of California,
* and the University of Chicago Board of Governors.
*
* This software was produced under U.S. Government contracts:
* (W-7405-ENG-36) at the Los Alamos National Laboratory,
* and (W-31-109-ENG-38) at Argonne National Laboratory.
*
* Initial development by:
* The Controls and Automation Group (AT-8)
* Ground Test Accelerator
* Accelerator Technology Division
* Los Alamos National Laboratory
*
* Co-developed with
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
* .01 01-18-93 jbk initialized
* .02 11-14-94 jps copy devOms.c and modify to point to vme58 driver
* .03 ??-??-95 jps copy devOms58.c and modify to point to Highland driver
* .04 06-20-96 jps allow for bumpless-reboot on position
* .04a 02-19-97 tmm fixed for EPICS 3.13
*/
#define VERSION 1.3
#include <vxWorks.h>
#include <stdioLib.h>
#include <string.h>
#include <math.h>
#include <semLib.h> /* jps: include for init_record wait */
#include <alarm.h>
#ifdef __cplusplus
extern "C" {
#include <callback.h>
#include <dbAccess.h>
#include <recSup.h>
}
#else
#include <callback.h>
#include <dbAccess.h>
#include <recSup.h>
#endif
#include <dbDefs.h>
#include <dbCommon.h>
#include <fast_lock.h>
#include <devSup.h>
#include <drvSup.h>
#include "motorRecord.h"
#include "motor.h"
#include "drvV544.h"
#define PRIVATE_FUNCTIONS 0 /* normal:1, debug:0 */
#define STATIC
#ifdef NODEBUG
#define Debug(L,FMT,V) ;
#else
#define Debug(L,FMT,V) { if(L <= devV544debug) \
{ printf("%s(%d):",__FILE__,__LINE__); \
printf(FMT,V); } }
#endif
#define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) /* tmm */
/* ----------------Create the dsets for devV544----------------- */
static long report();
STATIC long v544_init(int after);
STATIC long v544_init_record(struct motorRecord *);
STATIC long get_ioint_info();
STATIC long v544_start_trans(struct motorRecord *);
STATIC long v544_update_values(struct motorRecord *);
STATIC long v544_build_trans(motor_cmnd, double *, struct motorRecord *);
STATIC long v544_end_trans(struct motorRecord *);
STATIC void v544_callback(MOTOR_RETURN *);
STATIC void v544_init_callback(MOTOR_RETURN *);
struct motor_dset devV544 =
{
{8, NULL, v544_init, v544_init_record, NULL},
v544_update_values,
v544_start_trans,
v544_build_trans,
v544_end_trans
};
/* xxDSET devXxxx={ 5,report,init,init_rec,get_ioint_info,read_write}; */
/* --------------------------- program data --------------------- */
volatile int devV544debug = 0;
struct v544_motor_table
{
int type;
uint16_t command;
int num_parms;
};
/* WARNING! this must match "motor_cmnd" in motor.h */
static struct v544_motor_table v544_table[] = {
/* type cmd num_parms */
{MOTION, CS_MOVE_ABS, 1}, /* MOVE_ABS */
{MOTION, CS_MOVE_INC, 1}, /* MOVE_REL */
{MOTION, CS_SEEK_INDX, 1}, /* HOME_FOR */
{MOTION, CS_SEEK_INDX, 1}, /* HOME_REV */
{IMMEDIATE, CS_SET_POSITION, 1}, /* LOAD_POS */
{IMMEDIATE, -1, 1}, /* SET_VELO_BASE - Not supported */
{IMMEDIATE, -1, 1}, /* SET_VELO */
{IMMEDIATE, -1, 1}, /* SET_ACCEL */
{IMMEDIATE, CS_GO, 0}, /* GO */
{IMMEDIATE, -1, 2}, /* SET_ENC_RATIO - Not supported */
{INFO, -1, 0}, /* GET_INFO */
{MOVE_TERM, CS_IDLE, 0}, /* STOP_AXIS */
{VELOCITY, -1, 1}, /* JOG */
{UNDEFINED, 0, 0}, /* SET_PGAIN */
{UNDEFINED, 0, 0}, /* SET_IGAIN */
{UNDEFINED, 0, 0}, /* SET_DGAIN */
{UNDEFINED, 0, 0}, /* ENABLE_TORQUE */
{UNDEFINED, 0, 0}, /* DISABL_TORQUE */
{UNDEFINED, 0, 0}, /* PRIMITIVE */
{UNDEFINED, 0, 0}, /* SET_HIGH_LIMIT */
{UNDEFINED, 0, 0}, /* SET_LOW_LIMIT */
{VELOCITY, 0, 0} /* JOG_VELOCITY */
};
/* status of an axis of a board, one present for each axis of each board*/
struct a_axis
{
/*??? int encoder_present; */
int in_use;
};
/* the device private data structure of the record */
struct v544_trans
{
int state;
FAST_LOCK lock;
MOTOR_CALL motor_call;
int callback_changed;
int32_t motor_pos;
int32_t encoder_pos;
int32_t vel;
unsigned long status;
SEM_ID initSem;
};
/* the board status structure */
struct v544_stat
{
int exists;
int total_axis;
struct a_axis *v544_axis;
};
STATIC struct v544_stat *v544_cards;
extern struct v544_support v544_access;
#define IDLE_STATE 0
#define BUILD_STATE 1
#define YES 1
#define NO 0
#define SEM_TIMEOUT 50
/* --------------------------- program data --------------------- */
/* initialize device support for V544 stepper motor */
STATIC long v544_init(int after)
{
MOTOR_CARD_QUERY card_query;
int i, j;
Debug(1, "v544_init: entry...\n", 0);
if (after)
return (0);
/* allocate space for total cards in system */
v544_cards = (struct v544_stat *) malloc(V544_NUM_CARDS *
sizeof(struct v544_stat));
/* for each card ask driver for num of axis supported and name */
for (i = 0; i < V544_NUM_CARDS; i++)
{
(*v544_access.get_card_info) (i, &card_query);
if (card_query.total_axis == 0)
v544_cards[i].exists = NO;
else
{
v544_cards[i].exists = YES;
v544_cards[i].total_axis = card_query.total_axis;
Debug(5, "v544_init: calling malloc(card_query...)\n total axis =%d\n",
v544_cards[i].total_axis);
v544_cards[i].v544_axis = (struct a_axis *) malloc(
card_query.total_axis * sizeof(struct a_axis));
for (j = 0; j < card_query.total_axis; j++)
v544_cards[i].v544_axis[j].in_use = 0;
}
}
Debug(1, "v544_init: exit...\n", 0);
return (0);
}
/* initialize a record instance */
STATIC long v544_init_record(struct motorRecord * mr)
{
int card, signal;
int initEncoder, initPos;
struct v544_trans *ptrans;
MOTOR_AXIS_QUERY axis_query;
MOTOR_CALL *motor_call;
double ep_mp[2]; /* encoder pulses, motor pulses */
int rtnStat;
Debug(1, "v544_init_record: entry...\n", 0);
/* allocate space for private field - an v544_trans structure */
Debug(5, "v544_init_record: calling malloc(...v544_trans...)\n", 0);
mr->dpvt = (struct v544_trans *) malloc(sizeof(struct v544_trans));
ptrans = mr->dpvt;
ptrans->state = IDLE_STATE;
ptrans->callback_changed = NO;
FASTLOCKINIT(&ptrans->lock);
motor_call = &(ptrans->motor_call);
callbackSetCallback((void (*)(struct callbackPvt *)) v544_callback,
&(motor_call->callback));
callbackSetPriority(priorityMedium, &(motor_call->callback));
/* check if axis already in use, then mark card, axis as in use */
/* out must be an VME_IO */
switch (mr->out.type)
{
case (VME_IO):
break;
default:
recGblRecordError(S_dev_badBus, (void *) mr,
(char *) "devV544 (init_record) Illegal OUT Bus Type");
return (S_dev_badBus);
}
card = mr->out.value.vmeio.card;
Debug(5, "v544_init_record: card %d\n", card);
signal = mr->out.value.vmeio.signal;
Debug(5, "v544_init_record: signal %d\n", signal);
rtnStat = 0;
if (card < 0 || card >= V544_NUM_CARDS || v544_cards[card].exists == NO)
{
recGblRecordError(S_db_badField, (void *) mr,
(char *) "devV544 (init_record) card does not exist!");
rtnStat = S_db_badField;
}
else if (signal < 0 || signal >= v544_cards[card].total_axis)
{
recGblRecordError(S_db_badField, (void *) mr,
(char *) "devV544 (init_record) signal does not exist!");
rtnStat = S_db_badField;
}
else if (v544_cards[card].v544_axis[signal].in_use == YES)
{
recGblRecordError(S_db_badField, (void *) mr,
"(char *) devV544 (init_record) motor already in use!");
rtnStat = S_db_badField;
}
if (rtnStat)
{
/* Initialize readback fields for simulation */
mr->msta = RA_PROBLEM;
mr->rmp = 0; /* raw motor pulse count */
mr->rep = 0; /* raw encoder pulse count */
return (rtnStat);
}
else
/* query motor for all info to fill into record */
(*v544_access.get_axis_info) (card, signal, &axis_query);
mr->msta = axis_query.status; /* status info */
v544_cards[card].v544_axis[signal].in_use = YES;
/*jps: setting the encoder ratio was moved from init_record() remove callbacks during iocInit */
/*
* Set the encoder ratio. Note this is blatantly device dependent.
* Determine the number of encoder pulses and the number of motor pulses
* per engineering unit (EGU). Send an array containing this information
* to device support.
*/
if ((initEncoder = (mr->msta & EA_PRESENT)) && mr->ueip)
{
if (fabs(mr->mres) < 1.e-9)
mr->mres = 1.;
if (fabs(mr->eres) < 1.e-9)
mr->eres = mr->mres;
{
int m;
for (m=10000000; (m > 1) &&
(fabs(m/mr->eres) > 1.e6 || fabs(m/mr->mres) > 1.e6);
m /= 10)
;
Debug(5, "oms58_init_record: ER mult = %d\n", m);
ep_mp[0] = m / mr->eres; /* encoder pulses per ...*/
ep_mp[1] = m / mr->mres; /* motor pulses */
}
}
else
{
ep_mp[0] = 1.;
ep_mp[1] = 1.;
}
/* Program the device if an initial position is programmed or
* an encoder is present. */
if ((initPos = (mr->dval && mr->mres)) || initEncoder)
{
/*
* Semaphore used to hold initialization until device is programmed -
* cleared by callback
*/
ptrans->initSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
/*
* Switch to special init callback so that record will not be
* processed during iocInit
*/
callbackSetCallback((void (*)(struct callbackPvt *)) v544_init_callback,
&(motor_call->callback));
if (initEncoder)
{
Debug(7, "v544_init_record: encoder pulses per EGU = %f\n", ep_mp[0]);
Debug(7, "v544_init_record: motor pulses per EGU = %f\n", ep_mp[1]);
v544_start_trans(mr);
v544_build_trans(SET_ENC_RATIO, ep_mp, mr);
v544_end_trans(mr);
}
if (initPos)
{
double setPos = mr->dval / mr->mres;
Debug(7, "v544_init_record: initial raw position = %f\n", setPos);
v544_start_trans(mr);
v544_build_trans(LOAD_POS, &setPos, mr);
v544_end_trans(mr);
}
/* Changing positions or encoder ratio may have
* changed the readback value. */
v544_start_trans(mr);
v544_build_trans(GET_INFO, NULL, mr);
v544_end_trans(mr);
/* Wait for callback w/timeout */
if ((rtnStat = semTake(ptrans->initSem, SEM_TIMEOUT)) == ERROR)
{
recGblRecordError(S_dev_NoInit, (void *) mr,
(char *) "dev_NoInit (init_record: callback2 timeout");
}
semDelete(ptrans->initSem);
/* Restore regular record callback */
callbackSetCallback((void (*)(struct callbackPvt *)) v544_callback,
&(motor_call->callback));
}
/* query motor for all info to fill into record */
(*v544_access.get_axis_info) (card, signal, &axis_query);
mr->rmp = axis_query.position; /* raw motor pulse count */
mr->rep = axis_query.encoder_position; /* raw encoder pulse count */
mr->msta = axis_query.status; /* status info */
Debug(7, "v544_init_record: rmp = %f\n", mr->rmp);
Debug(7, "v544_init_record: rep = %f\n", mr->rep);
Debug(7, "v544_init_record: msta = %d\n", mr->msta);
Debug(1, "v544_init_record: exit...\n", 0);
return (0);
}
STATIC long v544_update_values(struct motorRecord * mr)
{
struct v544_trans *ptrans;
long rc;
rc = NOTHING_DONE;
ptrans = (struct v544_trans *) mr->dpvt;
FASTLOCK(&ptrans->lock);
/* raw motor pulse count */
if (ptrans->callback_changed == YES)
{
mr->rmp = ptrans->motor_pos;
mr->rep = ptrans->encoder_pos;
mr->rvel = ptrans->vel;
mr->msta = ptrans->status;
ptrans->callback_changed = NO;
rc = CALLBACK_DATA;
}
FASTUNLOCK(&ptrans->lock);
return (rc);
}
/* start building a transaction */
STATIC long v544_start_trans(struct motorRecord * mr)
{
int card = mr->out.value.vmeio.card;
int axis = mr->out.value.vmeio.signal;
struct v544_trans *trans = (struct v544_trans *) mr->dpvt;
MOTOR_CALL *motor_call;
CMND_DEF *motor_cmnd;
if (!mr->dpvt)
return (S_dev_NoInit);
motor_call = &(trans->motor_call);
/*
* initialize area to device private field to store command that is to be
* built and mark as command build in progress
*/
trans->state = BUILD_STATE;
motor_call->card = card;
motor_call->signal = axis;
motor_call->type = UNDEFINED;
motor_call->precord = (struct dbCommon *) mr;
motor_call->cmndCnt = 0;
motor_call->cmndIndx = 0;
/* Zero command define structure */
bzero((void *)motor_call->cmndList, sizeof(CMND_DEF)*MAX_LIST_SIZE);
Debug(3, "v544_start_trans: motor %d command started\n", (card * V544_NUM_CHANNELS) + axis);
return (0);
}
/* end building a transaction */
STATIC long v544_end_trans(struct motorRecord * mr)
{
struct v544_trans *trans = (struct v544_trans *) mr->dpvt;
MOTOR_CALL *motor_call;
long rc;
int card = mr->out.value.vmeio.card;
int axis = mr->out.value.vmeio.signal;
rc = 0;
motor_call = &(trans->motor_call);
switch (trans->state)
{
case BUILD_STATE:
/* shut off command build in process thing */
trans->state = IDLE_STATE;
Debug(3, "v544_end_trans: motor %d command send\n", (card * V544_NUM_CHANNELS) + axis);
rc = (*v544_access.send) (motor_call);
break;
case IDLE_STATE:
default:
rc = ERROR;
}
return (rc);
}
/* add a part to the transaction */
STATIC long v544_build_trans(motor_cmnd command, double *parms, struct motorRecord * mr)
{
struct v544_trans *trans = (struct v544_trans *) mr->dpvt;
MOTOR_CALL *motor_call;
char buffer[20];
int parmInt;
int first_one, i;
motor_call = &(trans->motor_call);
if (v544_table[command].type == UNDEFINED)
return(OK);
if (v544_table[command].type > motor_call->type)
motor_call->type = v544_table[command].type;
if (devV544debug >= 4)
printf("build_trans: v544 cmndCode %d, cmndParm %12.4f\n", command, *parms);
/* concatenate onto the dpvt message field */
switch (trans->state)
{
case BUILD_STATE:
/* put in command */
switch (command)
{
case MOVE_ABS:
case MOVE_REL:
case HOME_FOR:
case HOME_REV:
motor_call->cmndList[motor_call->cmndCnt].cmnd |= v544_table[command].command;
motor_call->cmndList[motor_call->cmndCnt].pos = NINT(*parms);
break;
case LOAD_POS:
motor_call->cmndList[motor_call->cmndCnt].cmnd |= v544_table[command].command;
motor_call->cmndList[motor_call->cmndCnt].pos = NINT(*parms);
motor_call->cmndCnt++;
break;
case SET_VEL_BASE:
break;
case SET_VELOCITY:
parmInt = NINT(*parms/(double)VEL_SCALE);
motor_call->cmndList[motor_call->cmndCnt].vel = parmInt;
break;
case SET_ACCEL:
parmInt = NINT(*parms/(double)(ACC_SCALE*ACC_SCALE));
if (parmInt == 0)
parmInt = 1;
motor_call->cmndList[motor_call->cmndCnt].accel = parmInt;
break;
case GO:
motor_call->cmndList[motor_call->cmndCnt].cmnd |= v544_table[command].command;
motor_call->cmndCnt++;
break;
case SET_ENC_RATIO:
case GET_INFO:
motor_call->cmndCnt++;
break;
case STOP_AXIS:
motor_call->cmndList[motor_call->cmndCnt].cmnd |= CS_IDLE;
motor_call->cmndList[motor_call->cmndCnt].vel = 0;
motor_call->cmndCnt++;
break;
case JOG:
if (*parms >= 0)
{
motor_call->cmndList[motor_call->cmndCnt].cmnd |= CS_RUN_CW;
parmInt = NINT(*parms/(double)VEL_SCALE);
motor_call->cmndList[motor_call->cmndCnt].vel = parmInt;
}
else
{
motor_call->cmndList[motor_call->cmndCnt].cmnd |= CS_RUN_CCW;
parmInt = -1*NINT(*parms/(double)VEL_SCALE);
motor_call->cmndList[motor_call->cmndCnt].vel = parmInt;
}
motor_call->cmndCnt++;
break;
default:
break;
}
break;
case IDLE_STATE:
default:
return ERROR;
}
return (0);
}
/* callback from V544 driver for motor in motion */
STATIC void v544_callback(MOTOR_RETURN * motor_return)
{
struct motorRecord *mr = (struct motorRecord *) motor_return->precord;
struct rset *prset = (struct rset *) (motor_return->precord->rset);
struct v544_trans *ptrans;
Debug(5, "v544_callback: entry\n", 0);
ptrans = (struct v544_trans *) mr->dpvt;
Debug(6, "v544_callback: FASTLOCK\n", 0);
FASTLOCK(&ptrans->lock);
/* raw motor pulse count */
Debug(6, "v544_callback: update ptrans\n", 0);
ptrans->callback_changed = YES;
ptrans->motor_pos = motor_return->position;
/* raw encoder pulse count */
ptrans->encoder_pos = motor_return->encoder_position;
/* raw motor velocity */
ptrans->vel = motor_return->velocity*VEL_SCALE;
ptrans->status = motor_return->status; /* status */
if (devV544debug >= 4)
printf("v544 callback: pos %d, enc %d, vel %d, status =0x%04x\n",
ptrans->motor_pos, ptrans->encoder_pos,ptrans->vel, ptrans->status);
Debug(6, "v544_callback: FASTUNLOCK\n", 0);
FASTUNLOCK(&ptrans->lock);
/* free the return data buffer */
Debug(6, "v544_callback: free MOTOR_RETURN\n", 0);
(*v544_access.free) (motor_return);
Debug(6, "v544_callback: dbScanLock\n", 0);
dbScanLock((struct dbCommon *) mr);
Debug(6, "v544_callback: calling process\n", 0);
(*prset->process) ((struct dbCommon *) mr);
Debug(6, "v544_callback: dbScanUnlock\n", 0);
dbScanUnlock((struct dbCommon *) mr);
Debug(5, "v544_callback: exit..\n", 0);
return;
}
/* callback from V544 driver for init_record - duplicate of
* the v544_callback without the call to process the record */
STATIC void v544_init_callback(MOTOR_RETURN * motor_return)
{
struct motorRecord *mr = (struct motorRecord *) motor_return->precord;
struct rset *prset = (struct rset *) (motor_return->precord->rset);
struct v544_trans *ptrans;
Debug(5, "v544_init_callback: entry\n", 0);
ptrans = (struct v544_trans *) mr->dpvt;
Debug(6, "v544_init_callback: FASTLOCK\n", 0);
FASTLOCK(&ptrans->lock);
/* raw motor pulse count */
Debug(6, "v544_init_callback: update ptrans\n", 0);
ptrans->callback_changed = YES;
ptrans->motor_pos = motor_return->position;
/* raw encoder pulse count */
ptrans->encoder_pos = motor_return->encoder_position;
/* raw motor velocity */
ptrans->vel = motor_return->velocity;
ptrans->status = motor_return->status; /* status */
/* free the return data buffer */
Debug(6, "v544_init_callback: free MOTOR_RETURN\n", 0);
(*v544_access.free) (motor_return);
/* Continue record init. */
Debug(6, "v544_init_callback: Continue sig.\n", 0);
semGive(ptrans->initSem);
Debug(6, "v544_init_callback: FASTUNLOCK\n", 0);
FASTUNLOCK(&ptrans->lock);
Debug(5, "v544_init_callback: exit\n", 0);
return;
}
View Git Blame Copy Permalink