From 295f5ea9172fea12fe73dc9189f50aabeb4dc747 Mon Sep 17 00:00:00 2001 From: Ron Sluiter Date: Wed, 17 Feb 2010 21:42:49 +0000 Subject: [PATCH] Removing support for Highland V544. --- motorApp/V544Src/Makefile | 3 - motorApp/V544Src/devHighland.dbd | 4 - motorApp/V544Src/devV544.c | 688 ---------------- motorApp/V544Src/drvV544.c | 1273 ------------------------------ motorApp/V544Src/drvV544.h | 266 ------- 5 files changed, 2234 deletions(-) delete mode 100644 motorApp/V544Src/Makefile delete mode 100644 motorApp/V544Src/devHighland.dbd delete mode 100644 motorApp/V544Src/devV544.c delete mode 100644 motorApp/V544Src/drvV544.c delete mode 100644 motorApp/V544Src/drvV544.h diff --git a/motorApp/V544Src/Makefile b/motorApp/V544Src/Makefile deleted file mode 100644 index 77a421ba..00000000 --- a/motorApp/V544Src/Makefile +++ /dev/null @@ -1,3 +0,0 @@ -TOP=../.. -include $(TOP)/configure/CONFIG_APP -include $(TOP)/configure/RULES_ARCHS diff --git a/motorApp/V544Src/devHighland.dbd b/motorApp/V544Src/devHighland.dbd deleted file mode 100644 index edda7dcf..00000000 --- a/motorApp/V544Src/devHighland.dbd +++ /dev/null @@ -1,4 +0,0 @@ -# Highland V544 driver support. -device(motor,VME_IO,devV544,"Highland V544") -driver(drvV544) - diff --git a/motorApp/V544Src/devV544.c b/motorApp/V544Src/devV544.c deleted file mode 100644 index 836d58df..00000000 --- a/motorApp/V544Src/devV544.c +++ /dev/null @@ -1,688 +0,0 @@ -/* 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 -#include -#include -#include -#include /* jps: include for init_record wait */ -#include -#ifdef __cplusplus -extern "C" { -#include -#include -#include -} -#else -#include -#include -#include -#endif -#include -#include -#include -#include -#include - -#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; -} diff --git a/motorApp/V544Src/drvV544.c b/motorApp/V544Src/drvV544.c deleted file mode 100644 index cd34e591..00000000 --- a/motorApp/V544Src/drvV544.c +++ /dev/null @@ -1,1273 +0,0 @@ -/* File: drvV544.c */ -/* Version: 1.3a */ -/* Date Last Modified: 2/19/97 */ - - -/* Device Driver 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 (drvOms.c) - * .02 11-14-94 jps copy drvOms.c and modify to point to vme58 driver - * .03 ??-??-95 jps copy drvOms58.c and modify to point to Highland driver - * .03a 02-19-97 tmm fixed for EPICS 3.13 -*/ - -#include -#include -#include -#include -#include -#ifdef __cplusplus -extern "C" { -#include -#include -#include -} -#else -#include -#include -#include -#endif -#include -#include - -#include -/* #include tmm: EPICS 3.13 */ -#include -#include -#include -#include -#include -#include -#include - -#include "motor.h" -#include "drvV544.h" - -#define PRIVATE_FUNCTIONS 1 /* normal:1, debug:0 */ - -#define STATIC static - -/* Define for return test on locationProbe() */ -#define PROBE_SUCCESS(STATUS) ((STATUS)==S_dev_addressOverlap) - -#define SET_MM_ON(v,a) v|=(1< motor_scan_rate) - ? NO_WAIT : (motor_scan_rate - tick_curr); - wait_time = any_motor_in_motion ? (int) tick_count : WAIT_FOREVER; - - sem_ret = semTake(motor_sem, wait_time); - - if (sem_ret == ERROR) - tick_used = tickGet(); - - - if (any_motor_in_motion) - { - for (i = 0; i < v544_num_cards; i++) - { - if (motor_state[i] && - motor_state[i]->motor_in_motion) - query_axis(i); - } - } - - if (sem_ret != ERROR) - process_messages(); - } -} - - - -/*****************************************************/ -/* send query to get all axis in motions position */ -/* query_axis() */ -/*****************************************************/ -STATIC query_axis(int card) -{ - volatile struct vmex_motor *pmotor; - register struct mess_node *motor_motion; - register struct mess_info *motor_info; - int i; - - - pmotor = (struct vmex_motor *) motor_state[card]->localaddr; - - /* get the encoder positions for axis in motion */ - for (i = 0; i < motor_state[card]->total_axis; i++) - { - motor_info = &(motor_state[card]->motor_info[i]); - motor_motion = motor_info->motor_motion; - - /* - * Check for commanded motion and actual motion or done before - * issuing a callback. - */ - if (motor_motion && set_status(card, i)) - { - MOTOR_RETURN *mess_ret; - BOOLEAN ls_active; - - if (drvV544debug >= 4) - printf("query_axis: motor %d, pos=%d, enc=%d, vel=%d, status=0x%04x\n", - (card * V544_NUM_CHANNELS) + i, - motor_info->position, - motor_info->encoder_position, - motor_info->velocity, - motor_info->status); - - motor_motion->position = motor_info->position; - motor_motion->encoder_position = motor_info->encoder_position; - motor_motion->velocity = motor_info->velocity; - motor_motion->status = motor_info->status; - - mess_ret = (MOTOR_RETURN *) motor_malloc(); - mess_ret->callback = motor_motion->callback; - mess_ret->precord = motor_motion->precord; - mess_ret->position = motor_motion->position; - mess_ret->encoder_position = motor_motion->encoder_position; - mess_ret->velocity = motor_motion->velocity; - mess_ret->status = motor_motion->status; - mess_ret->type = motor_motion->type; - - if (motor_motion->status & RA_DIRECTION) - { - if (motor_motion->status & RA_PLUS_LS) - ls_active = ON; - else - ls_active = OFF; - } - else - { - if (motor_motion->status & RA_MINUS_LS) - ls_active = ON; - else - ls_active = OFF; - } - - if (ls_active == ON || motor_motion->status & RA_PROBLEM) - { - /* Assure motion is stopped */ - send_cmnd(card, i, 0, 0, 0, CS_IDLE); - Debug(1, "query_axis: motion aborted - status=0x%4x\n", - motor_info->status); - - motor_state[card]->motor_in_motion--; - - motor_free(motor_motion); - motor_motion = (struct mess_node *) NULL; - motor_info->motor_motion = (struct mess_node *) NULL; - } - else if (motor_motion->status & RA_DONE) - { - /* Check for additional motion in command list */ - if (motor_motion->cmndIndx < motor_motion->cmndCnt) - { - CMND_DEF *pcmnd = &motor_motion->cmndList[motor_motion->cmndIndx++]; - - send_cmnd(card, i, pcmnd->pos, pcmnd->vel, - pcmnd->accel, pcmnd->cmnd); - - /* Not done yet. */ - motor_motion->status &= ~RA_DONE; - mess_ret->status = motor_motion->status; - - } - else - { - motor_state[card]->motor_in_motion--; - - motor_free(motor_motion); - motor_motion = (struct mess_node *) NULL; - motor_info->motor_motion = (struct mess_node *) NULL; - } - } - - callbackRequest((CALLBACK *) mess_ret); - - if (motor_state[card]->motor_in_motion == 0) - { - SET_MM_OFF(any_motor_in_motion, card); - } - } - } - - return (0); -} - - -/************************************************************** - * Read motor status from card - * set_status() - ************************************************************/ - -STATIC set_status(int card, int signal) -{ - register struct mess_info *motor_info; - struct vmex_motor *pmotor; - MOTOR_REG *pmotorReg; - POS_REG *pmotorPos; - POS_REG motorPosReg; - int32_t motorPos; - uint16_t motorVel; - uint16_t motorStatusReg; - uint16_t motorCmndReg; - int rtn_state = 0; - BOOLEAN plusdir, ls_active = OFF; - - motor_info = &(motor_state[card]->motor_info[signal]); - pmotor = (struct vmex_motor *) motor_state[card]->localaddr; - - pmotorReg = &pmotor->motorReg[signal]; - motorStatusReg = pmotorReg->status; - motorCmndReg = pmotorReg->cmnd; - - Debug(5, "set_status: cmndReg=0x%04x\n", motorCmndReg); - - - /* Test for command-in-progress */ - if (motorCmndReg & (CS_BUSY | CS_DONE)) - { - pmotorPos = &pmotor->posReg[signal]; - - /* Get motor position, test if read during update */ - do - { - motorPosReg.coarse = pmotorPos->coarse; - motorPosReg.fine = pmotorPos->fine; - } while (motorPosReg.coarse != pmotorPos->coarse); - - motorVel = pmotor->velReg[signal]; - motorPos = (motorPosReg.coarse << 16) | motorPosReg.fine; - - /* Set direction base on position history */ - if (motorPos == motor_info->position) - motor_info->no_motion_count++; - else - { - epicsInt32 newposition; - - newposition = NINT(motorPos); - if (newposition >= motor_info->position) - motor_info->status |= RA_DIRECTION; - else - motor_info->status &= ~RA_DIRECTION; - motor_info->position = newposition; - motor_info->no_motion_count = 0; - } - - plusdir = (motor_info->status & RA_DIRECTION) ? ON : OFF; - - if (motorStatusReg & STAT_CWL) - { - motor_info->status |= RA_PLUS_LS; - if (plusdir == ON) - ls_active = ON; - } - else - motor_info->status &= ~RA_PLUS_LS; - - - if (motorStatusReg & STAT_CCWL) - { - motor_info->status |= RA_MINUS_LS; - if (plusdir == OFF) - ls_active = ON; - } - else - motor_info->status &= ~RA_MINUS_LS; - - - if (motor_info->no_motion_count > motionTO) - { - motor_info->status |= RA_PROBLEM; - motor_info->no_motion_count = 0; - } - else - motor_info->status &= ~RA_PROBLEM; - - motor_info->encoder_position = 0; - motor_info->position = motorPos; - motor_info->velocity = motorVel; - - if (motorCmndReg & CS_DONE) - motor_info->status |= RA_DONE; - else - motor_info->status &= ~RA_DONE; - - if (motorStatusReg & STAT_INDX) - motor_info->status |= RA_HOME; - else - motor_info->status &= ~RA_HOME; - - rtn_state = (!motor_info->no_motion_count || ls_active == ON || - (motor_info->status & (RA_DONE | RA_PROBLEM))) ? 1 : 0; - } - else if (motor_info->no_motion_count++ > motionTO) - { - /* Command was never acknowledged */ - motor_info->no_motion_count = 0; - - Debug(1, "set_status: Ack Timeout - cmndReg=0x%04x\n", motorCmndReg); - - motor_info->status |= RA_PROBLEM; - rtn_state = 1; - } - - return (rtn_state); -} - - -/*****************************************************/ -/* Process messages */ -/* process_messages() */ -/*****************************************************/ -STATIC process_messages() -{ - register struct mess_node *motor_motion; - struct mess_node *node; - CMND_DEF *pcmnd; - int16_t cmndbuf; - char qa_buf[15]; - - while (node = get_head_node()) - { - Debug(6, "Got node type (%d)\n", node->type); - - /* Check that card and signal exists - check tightened 6/11/96 (jps) */ - if ((node->card >= 0 && node->card < total_cards) && - motor_state[node->card] && - (node->signal >= 0 && node->signal < motor_state[node->card]->total_axis)) - { - switch (node->type) - { - case QUERY: - /* Not needed */ - callbackRequest((CALLBACK *) node); - break; - case VELOCITY: - /* send a message to V544 */ - motor_motion = - motor_state[node->card]->motor_info[node->signal].motor_motion; - - pcmnd = &node->cmndList[node->cmndIndx++]; - send_cmnd(node->card, node->signal, pcmnd->pos, pcmnd->vel, pcmnd->accel, - pcmnd->cmnd); - - /* - * this is tricky - another motion is here there is a very - * large assumption being made here: that the person who sent - * the previous motion is the same one that is sending this - * one, if he weren't, the guy that sent the original would - * never get notified of finish motion. This makes sense in - * record processing since only one record can be assigned to - * an axis and sent commands to it. An improvement would be - * to check and see if the record pointers were the same, if - * they were not, then send a finish message to the previous - * registered motion guy. - */ - - if (!motor_motion) /* if NULL */ - motor_state[node->card]->motor_in_motion++; - else - motor_free(motor_motion); - - SET_MM_ON(any_motor_in_motion, node->card); - motor_state[node->card]->motor_info[node->signal].motor_motion = node; - break; - case MOTION: - /* send a message to V544 */ - motor_motion = - motor_state[node->card]->motor_info[node->signal].motor_motion; - - pcmnd = &node->cmndList[node->cmndIndx++]; - send_cmnd(node->card, node->signal, pcmnd->pos, pcmnd->vel, pcmnd->accel, - pcmnd->cmnd); - - /* this is tricky - see velocity comment */ - if (!motor_motion) /* if NULL */ - motor_state[node->card]->motor_in_motion++; - else - motor_free(motor_motion); - - SET_MM_ON(any_motor_in_motion, node->card); - motor_state[node->card]->motor_info[node->signal].no_motion_count = 0; - motor_state[node->card]->motor_info[node->signal].motor_motion = node; - break; - case INFO: - set_status(node->card, node->signal); - node->position = - motor_state[node->card]->motor_info[node->signal].position; - node->encoder_position = - motor_state[node->card]->motor_info[node->signal].encoder_position; - node->status = - motor_state[node->card]->motor_info[node->signal].status; - - /* - * =========================================================== - * node->status & RA_DONE is not a reliable indicator of - * anything, in this case, since set_status() didn't ask the - * V544 controller to set the "Done" flag on command - * completion. Nevertheless, recMotor:process() needs to know - * whether the motor has stopped, and this we can tell by - * looking for a struct motor_motion. - * =========================================================== - * */ - motor_motion = - motor_state[node->card]->motor_info[node->signal].motor_motion; - if (motor_motion) - node->status &= ~RA_DONE; - else - node->status |= RA_DONE; - - callbackRequest((CALLBACK *) node); - break; - - case MOVE_TERM: - pcmnd = &node->cmndList[node->cmndIndx++]; - send_cmnd(node->card, node->signal, pcmnd->pos, pcmnd->vel, pcmnd->accel, - pcmnd->cmnd); - motor_motion = - motor_state[node->card]->motor_info[node->signal].motor_motion; - /* if motion-in-progress - cancel buffered commands */ - if (motor_motion) - motor_motion->cmndIndx = motor_motion->cmndCnt; - break; - default: - /* send a message to V544 */ - pcmnd = &node->cmndList[node->cmndIndx++]; - send_cmnd(node->card, node->signal, pcmnd->pos, pcmnd->vel, - pcmnd->accel, pcmnd->cmnd); - taskDelay(1); /* Give command time to finish */ - set_status(node->card, node->signal); - if (!motor_state[node->card]->motor_info[node->signal].status & CS_DONE) - Debug(1, "process(v455): motor %d Immediate command NOT-done yet.\n", - (node->card * V544_NUM_CHANNELS) + node->signal); - motor_free(node); /* free message buffer */ - break; - } - } - else - { - Debug(1, "send_mess - invalid card #%d\n", node->card); - node->position = 0; - node->encoder_position = 0; - node->velocity = 0; - node->status = RA_PROBLEM; - callbackRequest((CALLBACK *) node); - } - } - return (0); -} - - - - -/*****************************************************/ -/* Get a message off the queue */ -/* get_head_node() */ -/*****************************************************/ -STATIC struct mess_node *get_head_node() -{ - struct mess_node *node, *save_node; - - Debug(7, "get_head_node: entry\n", 0); - /* get a message from queue */ - FASTLOCK(&motor_queue); - - node = mess_queue.head; - save_node = (struct mess_node *) NULL; - Debug(7, "get_head_node: node = %x\n", node); - - /* delete node from list */ - if (node) - { /* tmm added */ - if (node == mess_queue.head) - { - Debug(7, "get_head_node: setting mess_queue.head=%x\n", node->next); - mess_queue.head = node->next; - } - if (node == mess_queue.tail) - { - Debug(7, "get_head_node: setting mess_queue.tail=%x\n", save_node); - mess_queue.tail = save_node; - } - if (save_node) - { - Debug(7, "get_head_node: setting save_node->next=%x\n", node->next); - save_node->next = node->next; - } - } - FASTUNLOCK(&motor_queue); - - Debug(7, "get_head_node: returning %x\n", node); - return (node); -} - - - - -/*****************************************************/ -/* send command to the V544 board control register */ -/* send_cmnd() */ -/* units = micro steps (256 ustep per step) */ -/*****************************************************/ -STATIC int send_cmnd(int card, /* board number */ - int motor,/* motor number */ - int32_t pos, /* request position */ - uint16_t vel, /* request velocity */ - uint16_t accel, /* request acceleration */ - uint16_t cmnd) /* motor command */ -{ - volatile struct vmex_motor *pmotor; /* board base address */ - volatile MOTOR_REG *pmotorReg; - int return_code, i, trys; - int abortTO; - uint16_t cmndSave; - - pmotor = (struct vmex_motor *) motor_state[card]->localaddr; - pmotorReg = &pmotor->motorReg[motor]; - return_code = 0; - - cmndSave = pmotorReg->cmnd; - - if (vel != 0 && !(cmndSave & CS_DONE)) - { - Debug(1, "send_cmnd: motor %d busy\n", (card * V544_NUM_CHANNELS) + motor); - return_code = -1; - } - - if (cmndSave & CS_ERROR) - { - Debug(1, "send_cmnd: command error - CMND=0x%04x\n", pmotorReg->cmnd); - return_code = -1; - } - - if ((pmotorReg->status) & STAT_MOTR) - { - Debug(1, "send_cmnd: motor %d sense error\n", (card * 4) + motor); - return_code = -1; - } - - pmotorReg->pos.coarse = pos >> 16; /* High word */ - pmotorReg->pos.fine = pos & 0xffff; /* Low word */ - pmotorReg->velocity = vel; - pmotorReg->accel = pmotorReg->lsDecel = accel; - - if (cmndSave & CS_DONE) - { - /* - * The v544 sometimes indicates done after 180us without starting the - * command. That time is well short of the normal latency so it is - * save to test for it. - */ - trys = 0; - do - { - pmotorReg->cmnd = (cmnd | CS_GO | v544InterruptMask | v544ParkSelect); - abortTO = v544CmndAbortTO; - while (abortTO--); /* dwell about 200us */ - if ((cmndSave = (pmotorReg->cmnd & CS_DONE))) - Debug(2, "send_cmnd: motor %d command aborted - retrying.\n", - (card * 4) + motor); - } while (cmndSave && trys++ < 3); - - if (drvV544sendTest) - { - POS_REG *posReg = &pmotor->posReg[motor]; - int posMoving = 0; - - pos = posReg->fine; - - printf("sendTest: Waiting for BUSY/DONE."); - while (!((cmndSave = pmotorReg->cmnd) & (CS_BUSY | CS_DONE))) - { - if (pos != posReg->fine && !posMoving) - { - posMoving = 1; - printf("\nMotor Moving"); - } - printf("."); - } - posMoving = 0; - if (cmndSave & CS_BUSY) - { - printf("\nsendTest: Waiting for DONE."); - while (!(pmotorReg->cmnd & CS_DONE)) - { - if (pos == posReg->fine && !posMoving) - { - posMoving = 1; - printf("\nMotor Stopped"); - } - pos = posReg->fine; - printf("."); - } - } - printf("\nsendTest: **DONE**\n"); - } - - if (drvV544debug >= 4) - printf("send_cmnd: motor %d, pos=%ld, vel=%d, accel=%d, cmnd=0x%04x\n", - (card * V544_NUM_CHANNELS) + motor, pos, vel, accel, - (cmnd | CS_GO | v544InterruptMask | v544ParkSelect)); - - - } - - return (return_code); -} - - - -/*****************************************************/ -/* Configuration function for module_types data */ -/* areas. v544Setup() */ -/*****************************************************/ -v544Setup(int num_cards, /* maximum number of cards in rack */ - int num_channels, /* Channels per card (4 - default) */ - void *addrs, /* Base Address(0xdd00 - default) */ - unsigned enableInt, /* 0=interrupts disabled, 1=VXI standard - * vector */ - int int_level, /* interrupt level (5 - default) */ - int scan_rate) /* polling rate - 1/60 sec units (6 - - * default) */ -{ - int vector; - - if (num_cards < 1 || num_cards > V544_NUM_CARDS) - v544_num_cards = V544_NUM_CARDS; - else - v544_num_cards = num_cards; - - if (num_channels < 1 || num_channels > V544_NUM_CHANNELS) - v544_num_channels = V544_NUM_CHANNELS; - else - v544_num_channels = num_channels; - - v544_addrs = addrs; - - if ((vector = enableInt) != 0) - { - vector = VXI_VECTOR((int) v544_addrs); - if (vector < 64 || vector > 255) - { - Debug(0, "v544Setup: invalid interrupt vector %d\n", vector); - Debug(0, " generated from VXI address 0x%x\n", addrs); - vector = 0; - } - } - - v544InterruptVector = vector; - - - if (int_level < 1 || int_level > 6) - { - Debug(0, "v544Setup: invalid interrupt level %d\n", int_level); - v544InterruptLevel = V544_INT_LEVEL; - } - else - v544InterruptLevel = int_level; - - /* Set motor polling task rate */ - if (scan_rate >= 1 && scan_rate <= sysClkRateGet()) - motor_scan_rate = sysClkRateGet() / scan_rate; - else - motor_scan_rate = SCAN_RATE; -} - - - -/*****************************************************/ -/* Interrupt service routine. */ -/* motorIsr() */ -/*****************************************************/ -STATIC void motorIsr(int card) -{ - volatile struct mot_state *pmotorState; - struct vmex_motor *pmotor; - - - if (card >= total_cards || (pmotorState = motor_state[card]) == NULL) - { - logMsg("Invalid entry-card #%d\n", card, 0, 0, 0, 0, 0); - return; - } - - pmotor = (struct vmex_motor *) (pmotorState->localaddr); - - if (drvV544debug >= 4) - logMsg("V544 card %d,vscr=0x%X\n", card, pmotor->vcsr, 0, 0, 0, 0); - - /* Let polling task check for motor done */ - semGive(motor_sem); -} - -STATIC int motorIsrSetup(unsigned card) -{ - struct vmex_motor *pmotor; - long status; - - Debug(6, "motorIsrSetup: Entry card#%d\n", card); - - pmotor = (struct vmex_motor *) (motor_state[card]->localaddr); - - status = devConnectInterrupt(V544_INTERRUPT_TYPE, - v544InterruptVector + card, - motorIsr, (void *) card); - if (!RTN_SUCCESS(status)) - { - errPrintf(status, __FILE__, __LINE__, "Can't connect to vector %d\n", - v544InterruptVector + card); - v544InterruptVector = 0;/* Disable interrupts */ - v544InterruptMask = CS_IEN_OFF; - return (ERROR); - } - - status = devEnableInterruptLevel(V544_INTERRUPT_TYPE, - v544InterruptLevel); - if (!RTN_SUCCESS(status)) - { - errPrintf(status, __FILE__, __LINE__, - "Can't enable enterrupt level %d\n", - v544InterruptLevel); - v544InterruptVector = 0;/* Disable interrupts */ - v544InterruptMask = CS_IEN_OFF; - return (ERROR); - } - - /* Setup mask for interrupt-on-done */ -/* v544InterruptMask = CS_IEN_ON; */ - return (OK); -} - -/*****************************************************/ -/* initialize all software and hardware */ -/* motor_init() */ -/*****************************************************/ -STATIC motor_init() -{ - volatile struct vmex_motor *pmotor; - long status; - int i, j, k; - char axis_pos[50]; - char encoder_pos[50]; - int total_encoders = 0; - int total_axis = 0; - void *localaddr; - void *probeAddr; - - /* Check for setup */ - if (v544_num_cards <= 0) - { - Debug(1, "motor_init: *V544 driver disabled* \n v544Setup() is missing from startup script.\n", 0); - return (ERROR); - } - - /* allocate space for total number of motors */ - (void *) motor_state = malloc(v544_num_cards * sizeof(struct mot_state *)); - - /* allocate structure space for each motor present */ - total_cards = 0; - - if (rebootHookAdd((FUNCPTR) v544_reset) == ERROR) - Debug(1, "vme58 motor_init: v544_reset disabled\n", 0); - - for (i = 0; i < v544_num_cards; i++) - { - Debug(2, "motor_init: v544 card %d\n", i); - - probeAddr = v544_addrs + (i * V544_BRD_SIZE); - Debug(9, "motor_init: locationProbe() on addr 0x%x\n", probeAddr); - status = locationProbe(V544_ADDRS_TYPE, probeAddr); - - if (PROBE_SUCCESS(status)) - { - status = devRegisterAddress(__FILE__, - V544_ADDRS_TYPE, - probeAddr, - V544_BRD_SIZE, - &localaddr); - Debug(9, "motor_init: v544 devRegisterAddress() status = %d\n", status); - if (!RTN_SUCCESS(status)) - { - errPrintf(status, __FILE__, __LINE__, - "Can't register V544 address 0x%x\n", probeAddr); - return (ERROR); - } - - Debug(9, "motor_init: V544 localaddr = %x\n", localaddr); - pmotor = (struct vmex_motor *) localaddr; - - total_cards++; - - Debug(9, "motor_init: V544 malloc'ing motor_state\n", 0); - motor_state[i] = (struct mot_state *) malloc( - sizeof(struct mot_state)); - motor_state[i]->localaddr = localaddr; - motor_state[i]->motor_in_motion = 0; - - /* Check controller ID */ - sprintf(motor_state[i]->ident, "%s rev. %d-%c, sn. %d", V544_LABEL, - pmotor->pgid, (char) pmotor->prev, pmotor->vsn); - Debug(3, "motor_init: V544 card ID = %s \n", motor_state[i]->ident); - - /* Issue warnings if self-test failed or VXI ID is not correct */ - if (!(pmotor->vcsr & (VCSR_SFT | VCSR_RDY))) - Debug(0, "motor_init: V544 card %d SELFTEST FAILED\n", i); - if (pmotor->vxid != V544_VXID) - Debug(0, "motor_init: V544 card %d - WRONG ID\n", i); - - total_axis = v544_num_channels; - - /* Test for optional encoders */ - if (pmotor->opts & OPTS_ENC) - total_encoders = total_axis; - else - total_encoders = 0; - - /* Initilize per-axis controller and internal data fields */ - for (j = 0; j < total_axis; j++) - { - send_cmnd(i, j, 0, 0, 0, CS_IDLE); - motor_state[i]->motor_info[j].motor_motion = NULL; - motor_state[i]->motor_info[j].status = 0; - if (total_encoders) - motor_state[i]->motor_info[j].encoder_present = YES; - else - motor_state[i]->motor_info[j].encoder_present = NO; - } - - Debug(3, "motor_init: Total axis = %d\n", total_axis); - - motor_state[i]->total_axis = total_axis; - - /* Enable interrupt-when-done if selected */ - if (v544InterruptVector) - { - if (motorIsrSetup(i) == ERROR) - errPrintf(0, __FILE__, __LINE__, "Interrupts Disabled!\n"); - } - - for (j = 0; j < total_axis; j++) - { - motor_state[i]->motor_info[j].status = 0; - motor_state[i]->motor_info[j].no_motion_count = 0; - motor_state[i]->motor_info[j].encoder_position = 0; - motor_state[i]->motor_info[j].position = 0; - - if (motor_state[i]->motor_info[j].encoder_present == YES) - motor_state[i]->motor_info[j].status |= EA_PRESENT; - - set_status(i, j); - } - - Debug(2, "motor_init: Init Address=0x%08.8x\n", localaddr); - Debug(3, "motor_init: Total encoders = %d\n\n", total_encoders); - } - else - { - Debug(3, "motor_init: Card NOT found!\n", 0); - motor_state[i] = (struct mot_state *) NULL; - } - } - - - motor_sem = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY); - any_motor_in_motion = 0; - - - FASTLOCKINIT(&motor_queue); - FASTLOCK(&motor_queue); - mess_queue.head = (struct mess_node *) NULL; - mess_queue.tail = (struct mess_node *) NULL; - FASTUNLOCK(&motor_queue); - - FASTLOCKINIT(&motor_freelist); - FASTLOCK(&motor_freelist); - free_list.head = (struct mess_node *) NULL; - free_list.tail = (struct mess_node *) NULL; - FASTUNLOCK(&motor_freelist); - - Debug(3, "Motors initialized\n", 0); - - taskSpawn("tmotor", 64, VX_FP_TASK | VX_STDIO, 5000, motor_task, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); - - Debug(3, "Started motor_task\n", 0); - - return (0); -} - - -/*****************************************************/ -/* send a command to the stepper motor task queue */ -/* motor_send() */ -/*****************************************************/ -STATIC int motor_send(MOTOR_CALL * u_msg) -{ - struct mess_node *new_message; - int index; - - Debug(9, "motor_send invoked for v544\n", 0); - - new_message = motor_malloc(); - new_message->callback = u_msg->callback; - new_message->next = (struct mess_node *) NULL; - new_message->type = u_msg->type; - new_message->signal = u_msg->signal; - new_message->card = u_msg->card; - new_message->precord = u_msg->precord; - new_message->status = 0; - new_message->cmndCnt = u_msg->cmndCnt; - new_message->cmndIndx = 0; - - bcopy((void *) u_msg->cmndList, (void *) new_message->cmndList, sizeof(CMND_DEF) * u_msg->cmndCnt); - - switch (new_message->type) - { - case MOTION: - break; - case VELOCITY: - break; - case MOVE_TERM: - break; - case IMMEDIATE: - break; - case QUERY: - case INFO: - break; - default: - return (-1); - break; - } - - FASTLOCK(&motor_queue); - - if (mess_queue.tail) - { - mess_queue.tail->next = new_message; - mess_queue.tail = new_message; - } - else - { - mess_queue.tail = new_message; - mess_queue.head = new_message; - } - - FASTUNLOCK(&motor_queue); - - semGive(motor_sem); - - return (0); -} - -STATIC struct mess_node *motor_malloc() -{ - struct mess_node *node; - - FASTLOCK(&motor_freelist); - - if (!free_list.head) - node = (struct mess_node *) malloc(sizeof(struct mess_node)); - else - { - node = free_list.head; - free_list.head = node->next; - if (!free_list.head) - free_list.tail = (struct mess_node *) NULL; - } - - FASTUNLOCK(&motor_freelist); - return (node); -} - -STATIC int motor_free(struct mess_node * node) -{ - FASTLOCK(&motor_freelist); - - node->next = (struct mess_node *) NULL; - - if (free_list.tail) - { - free_list.tail->next = node; - free_list.tail = node; - } - else - { - free_list.head = node; - free_list.tail = node; - } - - FASTUNLOCK(&motor_freelist); - return (0); -} - -/*---------------------------------------------------------------------*/ -/* - * both of these routines are only to be used at initialization time - before - * queuing transactions starts for an axis. These routines do no locking, - * since transactions will not enter the queue for an axis until after they - * have run. - */ - -/* return the card information to caller */ -STATIC int motor_card_info(int card, MOTOR_CARD_QUERY * cq) -{ - if (card >= 0 && - card < total_cards && - motor_state[card]) - { - cq->total_axis = motor_state[card]->total_axis; - cq->card_name = motor_state[card]->ident; - } - else - cq->total_axis = 0; - - return (0); -} - -/* return information for an axis to the caller */ -STATIC int motor_axis_info(int card, int signal, MOTOR_AXIS_QUERY * aq) -{ - if (card >= 0 && - card < total_cards && - motor_state[card] && - signal >= 0 && - signal < motor_state[card]->total_axis) - { - aq->position = motor_state[card]->motor_info[signal].position; - aq->encoder_position = - motor_state[card]->motor_info[signal].encoder_position; - aq->status = motor_state[card]->motor_info[signal].status; - } - else - { - aq->position = aq->encoder_position = 0; - aq->status = RA_PROBLEM; - } - - return (0); -} - -/* - * - * Disables interrupts. Called on CTL X reboot. - * - */ - -STATIC void v544_reset() -{ - short card; - short axis; - struct vmex_motor *pmotor; - short status; - - for (card = 0; card < total_cards; card++) - { - (void *) pmotor = motor_state[card]->localaddr; - if (vxMemProbe((char *) pmotor, READ, sizeof(short), (char *) &status) == OK) - { - v544InterruptMask = CS_IEN_OFF; - for (axis = 0; axis < v544_num_channels; axis++) - send_cmnd(card, axis, 0, 0, 0, CS_IDLE); - } - } -} - - -/*---------------------------------------------------------------------*/ diff --git a/motorApp/V544Src/drvV544.h b/motorApp/V544Src/drvV544.h deleted file mode 100644 index fa67ed95..00000000 --- a/motorApp/V544Src/drvV544.h +++ /dev/null @@ -1,266 +0,0 @@ -/* File: drvV544.h */ -/* Version: 1.2 */ -/* Date Last Modified: 8/6/96 */ - - -/* Device Support Definitions for Highland motor controller */ -/* - * 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 drvOms.h and modify to point to vme58 driver - * .03 ??-??-95 jps copy drvOms58.h and modify to point to Highland driver - * ... - */ - - - - -/* - * valid command types for the driver, the order is importance, 0 is of - * lowest importance. Device support will choose the greatest one to use as - * the driver transaction type. - */ - -#define UNDEFINED (unsigned char)0 /* garbage type */ -#define IMMEDIATE (unsigned char)1 /* 'i' an execute immediate, no reply */ -#define MOVE_TERM (unsigned char)2 /* 't' terminate a previous active - * motion */ -#define MOTION (unsigned char)3 /* 'm' will produce motion updates */ -#define VELOCITY (unsigned char)4 /* 'v' make motion updates till - * MOVE_TERM */ -#define INFO (unsigned char)5 /* 'f' get curr motor/encoder pos and - * stat */ -#define QUERY (unsigned char)6 /* 'q' specialty type, not needed */ - -/* Set list size to accomidate the 2 move commands - * when all parameters are being changed */ -#define MAX_LIST_SIZE 10 - -typedef struct CmndDef -{ - int32_t pos; - uint16_t vel; - uint16_t accel; - uint16_t standby; /* Drive current standby percent */ - uint16_t cmnd; /* Motion command (ABS,INC, HOME,...) */ -} CMND_DEF; - -/* message queue management - device and driver support only */ -struct mess_node -{ - CALLBACK callback; - int signal; - int card; - unsigned char type; - int cmndCnt; /* Number of commands in list */ - int cmndIndx; /* Current command index */ - CMND_DEF cmndList[MAX_LIST_SIZE]; - long position; - long encoder_position; - long velocity; - unsigned long status; - struct dbCommon *precord; - struct mess_node *next; -}; -typedef struct mess_node MOTOR_CALL; -typedef struct mess_node MOTOR_RETURN; - -typedef struct mess_card_query -{ - char *card_name; - int total_axis; -} MOTOR_CARD_QUERY; - -typedef struct mess_axis_query -{ - long position; - long encoder_position; - unsigned long status; -} MOTOR_AXIS_QUERY; - -struct v544_support -{ - int (*send) (MOTOR_CALL *); - int (*free) (struct mess_node *); - int (*get_card_info) (int, MOTOR_CARD_QUERY *); - int (*get_axis_info) (int, int signal, MOTOR_AXIS_QUERY *); -}; - -/* Board control register structures */ - - -/* - * V544 default profile - */ - -#define V544_NUM_CARDS 8 -#define V544_NUM_CHANNELS 4 -#define V544_INTERRUPT_TYPE intVME -#define V544_ADDRS_TYPE atVMEA16 -#define V544_NUM_ADDRS 0xDD00 -#define V544_INT_LEVEL 5 /* default interrupt level (1-6) */ -#define V544_BRD_SIZE 0x80 /* card address boundary */ - -/* Board type info */ -#define V544_LABEL "V544" -#define V544_VXID 0xFEEE /* Manufacturer registered VXI ID */ - -#define BRD_OFFSET 0x80 -#define VXI_BASE 0xC000 -#define VXI_VECTOR(v) ((v-VXI_BASE)/0x40) - -/* V544 Parameter Scaling Constants */ -/* (assuming half step scaling by the record */ -#define STEP_SCALE 256 /* Position scaling per step */ -#define VEL_SCALE 16 /* Velocity scaling per step/sec */ -#define ACC_SCALE 256 /* Acceleration scaling per step/sec/sec */ - -/* V544 Parameter Limits */ -#define MAX_VEL 4096 -#define MAX_ACCEL 65535 -#define MAX_POS (2**31) - -/* V544 Control Register Definitions */ - -/* VCSR register status bits (board level status) */ -#define VCSR_RES 0x1 /* Soft Reset */ -#define VCSR_SFT 0x4 /* Self-test passed */ -#define VCSR_RDY 0x8 /* Ready to operate */ -#define VCSR_MOD 0x4000 /* Module ID 'off' */ - -/* VCSR register commands (board level commands) */ -#define VCSR_FRZ 0x8000 /* Board level freeze on motor positions updates */ - -/* OPTS (options) register status masks */ -#define OPTS_ENC 0x1 /* Encoder option enabled */ -#define OPTS_ACT 0x2 /* Encoder FPGA installed */ -#define OPTS_SER 0x4 /* Serial port enabled */ - -/* Per-axis Command/Status register definitions */ -/* status bits */ -#define CS_BUSY_TO 2 /* Maximum command ACK lantency in polling cycles */ -#define CS_ABORT_TO 1900 /* Dwell loop count for send_cmnd() check of abort bug */ -#define CS_ERROR 0x8000 /* Command error flag */ -#define CS_DONE 0x80 /* Command completed flag */ -#define CS_BUSY 0x40 /* Command accepted */ - -/* CMD codes (bits 0-4)*/ -#define CS_IDLE 0x0 /* Soft-reset - update status/position regs */ -#define CS_RUN_CW 0x1 /* Constant velocity - CW */ -#define CS_RUN_CCW 0x2 /* Constant velocity - CCW */ -#define CS_MOVE_ABS 0x3 /* Move to absolute motor position */ -#define CS_MOVE_INC 0x4 /* Move to relative motor position */ -#define CS_SEEK_INDX 0x5 /* Stop ABS move at index switch */ -#define CS_BACKOFF_CW 0x6 /* Stop CW ABS move when off limit switch */ -#define CS_BACKOFF_CCW 0x7 /* Stop CCW ABS move when off limit switch */ -#define CS_SET_ZERO 0x8 /* Set absolute zero to motor position */ -#define CS_SET_POSITION 0x9 /* Set logical position to motor position */ -#define CS_READ_THETA 0xA /* Read motor quadrant/microstep position */ - -/* Operations */ -#define CS_GO 0x20 /* Initiate command execution */ - -/* Interrupt control */ -#define CS_IEN_ON 0x4000 /* Bit 14 - Enable interrupt on "DONE" */ -#define CS_IEN_OFF 0x0 - -/* Motor PARKing currents (bits 9,10) */ -#define CS_PARK_100 0x000 /* Part motor at 100% current */ -#define CS_PARK_75 0x200 /* 75% */ -#define CS_PARK_50 0x400 /* 50% */ -#define CS_PARK_0 0x600 /* 0% */ - - -/* V544 STATus Register Bit Mapping */ -#define STAT_BADC 0x8000 /* Illegal command detected */ -#define STAT_SPRA 0x20 /* Spare limit switch status */ -#define STAT_MOTR 0x8 /* Motor driver error */ -#define STAT_CCWL 0x4 /* CCW limit switch status */ -#define STAT_CWL 0x2 /* CW limit switch status */ -#define STAT_INDX 0x1 /* INDEX switch status */ - - - - -/* V544 DUAL-PORT MEMORY MAP */ -/* Define byte offsets into axis control registers */ -#define COARSE_POS_ID 0x0 -#define FINE_POS_ID 0x2 -#define VELOCITY_ID 0x4 -#define ACCEL_ID 0x6 -#define LS_DECEL_ID 0x8 -#define AUX_REG_ID 0xa -#define STAT_REG_ID 0xc -#define CMND_REG_ID 0xe - -/* - * union posReg{ - * int32_t all; - * struct { - * int16_t high; - * int16_t low; - * } split; - * }; -*/ - -typedef struct pos_reg_str { - int16_t coarse; - uint16_t fine; - } POS_REG; - -/* Define register structure */ -typedef struct motor_reg_str { - POS_REG pos; - uint16_t velocity; - uint16_t accel; - uint16_t lsDecel; - uint16_t aux; - uint16_t status; - uint16_t cmnd; - } MOTOR_REG; - - -struct vmex_motor { - uint16_t vxid; - uint16_t vtype; - uint16_t vcsr; - uint16_t pgid; - uint16_t prev; - uint16_t vsn; - uint16_t opts; - uint16_t scan; - - /* Programming/Status Registers */ - MOTOR_REG motorReg[V544_NUM_CHANNELS]; - - /* Readback Registers */ - POS_REG posReg[V544_NUM_CHANNELS]; /* Actual positions */ - uint16_t velReg[V544_NUM_CHANNELS]; /* Actual velocity */ -}; -