diff --git a/src/rec/recSteppermotor.c b/src/rec/recSteppermotor.c new file mode 100644 index 000000000..5f55b5fa8 --- /dev/null +++ b/src/rec/recSteppermotor.c @@ -0,0 +1,832 @@ +/* recSm.c */ +/* share/src/rec $Id$ */ + +/* recSm.c - Record Support Routines for Steppermotor records + * + * Author: Bob Dalesio + * Date: 12-11-89 + * + * Control System Software for the GTA Project + * + * Copyright 1988, 1989, the Regents of the University of California. + * + * This software was produced under a U.S. Government contract + * (W-7405-ENG-36) at the Los Alamos National Laboratory, which is + * operated by the University of California for the U.S. Department + * of Energy. + * + * Developed by the Controls and Automation Group (AT-8) + * Accelerator Technology Division + * Los Alamos National Laboratory + * + * Direct inqueries to: + * Bob Dalesio, AT-8, Mail Stop H820 + * Los Alamos National Laboratory + * Los Alamos, New Mexico 87545 + * Phone: (505) 667-3414 + * E-mail: dalesio@luke.lanl.gov + * + * Modification Log: + * ----------------- + * .01 02-07-90 lrd fix initial fetch from within the motor record + * .02 02-07-90 lrd add a motor command for reading current status + * .03 04-11-90 lrd fixed acceleration for velocity mode motor + * .04 04-13-90 lrd make second argument for move = 0 + * .05 04-19-90 lrd keep first error + * add retry deadband + * make the retry count a database field + * .06 04-20-90 lrd make readback occur before setting MOVN to 0 + * .07 07-02-90 lrd make conversion compute in floating point + * .08 10-01-90 lrd modify readbacks to be throttled by delta + * .09 10-15-90 mrk extensible record and device support + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Create RSET - Record Support Entry Table*/ +#define report NULL +#define initialize NULL +long init_record(); +long process(); +#define special NULL +long get_value(); +#define cvt_dbaddr NULL +#define get_array_info NULL +#define put_array_info NULL +long get_units(); +long get_precision(); +#define get_enum_str NULL +#define get_enum_strs NULL +#define put_enum_str NULL +long get_graphic_double(); +long get_control_double(); +long get_alarm_double(); + +struct rset smRSET={ + RSETNUMBER, + report, + initialize, + init_record, + process, + special, + get_value, + cvt_dbaddr, + get_array_info, + put_array_info, + get_units, + get_precision, + get_enum_str, + get_enum_strs, + put_enum_str, + get_graphic_double, + get_control_double, + get_alarm_double }; + +/* because the driver does all the work just declare device support here*/ +struct dset devSmCompumotor1830={4,NULL,NULL,NULL,NULL}; +struct dset devSmOms6Axis={4,NULL,NULL,NULL,NULL}; + +#define VELOCITY 0 +#define POSITION 1 +#define POSITIVE_LIMIT 1 +#define NEGATIVE_LIMIT 2 + +void alarm(); +void monitor(); +void smcb_callback(); +void init_sm(); +void convert_sm(); +void positional_sm(); +void velocity_sm(); +void sm_get_position(); + + +static long init_record(psm) + struct smRecord *psm; +{ + + init_sm(psm); + return(0); +} + + +static long process(paddr) + struct dbAddr *paddr; +{ + struct smRecord *psm=(struct smRecord *)(paddr->precord); + + /* intialize the stepper motor record when the init bit is 0 */ + /* the init is set when the readback returns */ + if (psm->init == 0){ + init_sm(psm); + monitor(psm); + return(0); + } + + psm->pact = TRUE; + if(psm->cmod == POSITION) + positional_sm(psm); + else + velocity_sm(psm); + + + /* check event list */ + monitor(psm); + + /* process the forward scan link record */ + if (psm->flnk.type==DB_LINK) dbScanPassive(psm->flnk.value.db_link.pdbAddr); + + psm->pact=FALSE; + return(0); +} + +static long get_value(psm,pvdes) + struct smRecord *psm; + struct valueDes *pvdes; +{ + pvdes->field_type = DBF_FLOAT; + pvdes->no_elements=1; + (float *)(pvdes->pvalue) = &psm->val; + return(0); +} + +static long get_units(paddr,units) + struct dbAddr *paddr; + char *units; +{ + struct smRecord *psm=(struct smRecord *)paddr->precord; + + strncpy(units,psm->egu,sizeof(psm->egu)); + return(0); +} + +static long get_precision(paddr,precision) + struct dbAddr *paddr; + long *precision; +{ + struct smRecord *psm=(struct smRecord *)paddr->precord; + + *precision = psm->prec; + return(0); +} + +static long get_graphic_double(paddr,pgd) + struct dbAddr *paddr; + struct dbr_grDouble *pgd; +{ + struct smRecord *psm=(struct smRecord *)paddr->precord; + + pgd->upper_disp_limit = psm->hopr; + pgd->lower_disp_limit = psm->lopr; + return(0); +} + +static long get_control_double(paddr,pcd) + struct dbAddr *paddr; + struct dbr_ctrlDouble *pcd; +{ + struct smRecord *psm=(struct smRecord *)paddr->precord; + + pcd->upper_ctrl_limit = psm->hopr; + pcd->lower_ctrl_limit = psm->lopr; + return(0); +} + +static long get_alarm_double(paddr,pad) + struct dbAddr *paddr; + struct dbr_alDouble *pad; +{ + struct smRecord *psm=(struct smRecord *)paddr->precord; + + pad->upper_alarm_limit = psm->hihi; + pad->upper_warning_limit = psm->high; + pad->lower_warning_limit = psm->low; + pad->lower_alarm_limit = psm->lolo; + return(0); +} + + +static void alarm(psm) + struct smRecord *psm; +{ + float deviation; + + deviation = psm->val - psm->rbv; + + /* alarm condition hihi */ + if (psm->nsevhhsv){ + if (deviation > psm->hihi){ + psm->nsta = HIHI_ALARM; + psm->nsev = psm->hhsv; + return; + } + } + + /* alarm condition lolo */ + if (psm->nsevllsv){ + if (deviation < psm->lolo){ + psm->nsta = LOLO_ALARM; + psm->nsev = psm->llsv; + return; + } + } + + /* alarm condition high */ + if (psm->nsevhsv){ + if (deviation > psm->high){ + psm->nsta = HIGH_ALARM; + psm->nsev =psm->hsv; + return; + } + } + + /* alarm condition lolo */ + if (psm->nsevlsv){ + if (deviation < psm->low){ + psm->nsta = LOW_ALARM; + psm->nsev = psm->lsv; + return; + } + } + return; + +} + +static void monitor(psm) + struct smRecord *psm; +{ + unsigned short monitor_mask; + float delta; + short stat,sevr,nsta,nsev; + + /* get previous stat and sevr and new stat and sevr*/ + stat=psm->stat; + sevr=psm->sevr; + nsta=psm->nsta; + nsev=psm->nsev; + /*set current stat and sevr*/ + psm->stat = nsta; + psm->sevr = nsev; + psm->nsta = 0; + psm->nsev = 0; + + /* anyone waiting for an event on this record */ + if (psm->mlis.count == 0) { + psm->mlst = psm->alst = psm->val; + return; + } + + /* Flags which events to fire on the value field */ + monitor_mask = 0; + + /* alarm condition changed this scan */ + if (stat!=nsta || sevr!=nsev) { + /* post events for alarm condition change*/ + monitor_mask = DBE_ALARM; + /* post stat and nsev fields */ + db_post_events(psm,&psm->stat,DBE_VALUE); + db_post_events(psm,&psm->sevr,DBE_VALUE); + } + /* check for value change */ + delta = psm->mlst - psm->val; + if(delta<0.0) delta = -delta; + if (delta > psm->mdel) { + /* post events for value change */ + monitor_mask |= DBE_VALUE; + /* update last value monitored */ + psm->mlst = psm->val; + } + /* check for archive change */ + delta = psm->alst - psm->val; + if(delta<0.0) delta = -delta; + if (delta > psm->adel) { + /* post events on value field for archive change */ + monitor_mask |= DBE_LOG; + /* update last archive value monitored */ + psm->alst = psm->val; + } + + /* send out monitors connected to the value field */ + if (monitor_mask){ + db_post_events(psm,&psm->val,monitor_mask); + } + return; +} + +/* + * SMCB_CALLBACK + * + * callback routine when a velocity is read + */ +static void smcb_callback(psm_data,psm) +struct motor_data *psm_data; +struct smRecord *psm; +{ + short stat,sevr,nsta,nsev; + + dbScanLock(psm); + if(psm->pact) { + dbScanUnlock(psm); + return; + } + psm->pact = TRUE; + if (psm->cmod == VELOCITY){ + /* check velocity */ + if (psm->rrbv != psm_data->velocity){ + psm->rrbv = psm_data->velocity; + psm->rbv = (float)psm_data->velocity / (float)psm->mres; + if (psm->mlis.count){ + db_post_events(psm,&psm->rbv,DBE_VALUE|DBE_LOG); + db_post_events(psm,&psm->rrbv,DBE_VALUE|DBE_LOG); + } + } + + /* direction */ + if (psm->dir != psm_data->direction){ + psm->dir == psm_data->direction; + if (psm->mlis.count) + db_post_events(psm,&psm->dir,DBE_VALUE); + } + + /* constant velocity */ + if (psm->cvel != psm_data->constant_velocity){ + psm->cvel = psm_data->constant_velocity; + if (psm->mlis.count) + db_post_events(psm,&psm->cvel,DBE_VALUE); + } + }else{ /* POSITION*/ + /* constant velocity */ + if (psm->cvel != psm_data->constant_velocity){ + psm->cvel = psm_data->constant_velocity; + if (psm->mlis.count) + db_post_events(psm,&psm->cvel,DBE_VALUE); + } + + /* direction */ + if (psm->dir != psm_data->direction){ + psm->dir == psm_data->direction; + if (psm->mlis.count) + db_post_events(psm,&psm->dir,DBE_VALUE); + } + + /* encoder position */ + /* the encoder is multiplied by 4 on the assumption that all indexers */ + /* use the quardrature encoding technique - if we use an encoder */ + /* that does not, then we need to make quadrature encoder a database */ + /* field and use the 4 on that condition !!!!! */ + if (psm->epos != psm_data->encoder_position){ + psm->epos = (psm_data->encoder_position * psm->dist * psm->mres) + / (psm->eres * 4); + if (psm->mlis.count) + db_post_events(psm,&psm->epos,DBE_VALUE); + } + + /* motor position */ + if (psm->mpos != psm_data->motor_position){ + psm->mpos = psm_data->motor_position * psm->dist; + if (psm->mlis.count) + db_post_events(psm,&psm->mpos,DBE_VALUE); + } + + /* limit switches */ + if (psm->mcw != psm_data->cw_limit){ + psm->mcw = psm_data->cw_limit; + if (psm->mlis.count) + db_post_events(psm,&psm->mcw,DBE_VALUE); + psm->cw = (psm->mcw)?0:1; + } + if (psm->mccw != psm_data->ccw_limit){ + psm->mccw = psm_data->ccw_limit; + if (psm->mlis.count) + db_post_events(psm,&psm->mccw,DBE_VALUE); + psm->ccw = (psm->mccw)?0:1; + } + + /* alarm conditions for limit switches */ + if ((psm->ccw == 0) || (psm->cw == 0)){ /* limit violation */ + if (psm->nsevhlsv) { + psm->nsta = HW_LIMIT_ALARM; + psm->nsev = psm->hlsv; + } + } + + /* get the read back value */ + sm_get_position(psm); + + /* get previous stat and sevr and new stat and sevr*/ + stat=psm->stat; + sevr=psm->sevr; + nsta=psm->nsta; + nsev=psm->nsev; + /*set current stat and sevr*/ + psm->stat = nsta; + psm->sevr = nsev; + psm->nsta = 0; + psm->nsev = 0; + + /* anyone waiting for an event on this record */ + if (psm->mlis.count!=0 && (stat!=nsta || sevr!=nsev) ){ + db_post_events(psm,&psm->ccw,DBE_VALUE); + db_post_events(psm,&psm->cw,DBE_VALUE); + db_post_events(psm,&psm->val,DBE_VALUE|DBE_ALARM); + db_post_events(psm,&psm->rbv,DBE_VALUE|DBE_ALARM); + db_post_events(psm,&psm->stat,DBE_VALUE); + db_post_events(psm,&psm->sevr,DBE_VALUE); + } + + + /* needs to follow get position to prevent moves with old readback */ + /* moving */ + if (psm->movn != psm_data->moving){ + psm->movn = psm_data->moving; + if (psm->mlis.count) + db_post_events(psm,&psm->movn,DBE_VALUE|DBE_LOG); + } + } + psm->pact = FALSE; + dbScanUnlock(psm); + return; +} + +/* + * INIT_SM + */ +static void init_sm(psm) +struct smRecord *psm; +{ + int acceleration,velocity; + short card,channel; + short status; + + /* the motor number is the card number */ + card = psm->out.value.vmeio.card; + channel = psm->out.value.vmeio.signal; + + /* check supported hardware */ + if (psm->out.type != VME_IO){ + psm->init = 1; + return; + } + + /* acceleration is in terms of seconds to reach velocity */ + acceleration = (1/psm->accl) * psm->velo * psm->mres; + + /* velocity is in terms of revolutions per second */ + velocity = psm->velo * psm->mres; + + /* initialize the motor */ + /* set mode - first command checks card present */ + if (sm_driver(psm->dtyp,card,channel,SM_MODE,psm->mode,0) < 0){ + if(psm->nsev < MAJOR_ALARM) { + psm->nsta = WRITE_ALARM; + psm->nsev = MAJOR_ALARM; + } + psm->init = 1; + return; + } + + /* set encoder/motor ratio */ + sm_driver(psm->dtyp,card,channel,SM_ENCODER_RATIO,psm->mres,psm->eres); + + /* set the velocity */ + sm_driver(psm->dtyp,card,channel,SM_VELOCITY,velocity,acceleration); + + /* set the callback routine */ + sm_driver(psm->dtyp,card,channel,SM_CALLBACK,smcb_callback,psm); + + /* initialize the limit values */ + psm->cw = psm->ccw = 1; /* 1 - means not at limit */ + + /* set initial position */ + if (psm->mode == POSITION){ + if (psm->ialg != 0){ + if (psm->ialg == POSITIVE_LIMIT){ + status = sm_driver(psm->dtyp,card,channel,SM_MOVE,0x0fffffff,0); + }else if (psm->ialg == NEGATIVE_LIMIT){ + status = sm_driver(psm->dtyp,card,channel,SM_MOVE,-0x0fffffff,0); + } + /* force a read of the position and status */ + }else{ + status = sm_driver(psm->dtyp,card,channel,SM_READ,0,0); + } + if (status < 0){ + if (psm->nsev < MAJOR_ALARM) { + psm->nsta = WRITE_ALARM; + psm->nsev = MAJOR_ALARM; + } + return; + } + }else if (psm->mode == VELOCITY){ + /* get the velocity */ +/* get_velocity(motor,smcb_velocity_req);*/ + psm->velo = 0; + psm->init = 1; + } + psm->cmod = psm->mode; + return; +} + +/* + * CONVERT_SM + * + */ +static void convert_sm(psm) +struct smRecord *psm; +{ + double temp; + + /* check drive limits */ + if (psm->dist > 0){ + if (psm->val > psm->drvh) psm->val = psm->drvh; + else if (psm->val < psm->drvl) psm->val = psm->drvl; + }else{ + if (-psm->val > psm->drvh) psm->val = -psm->drvh; + else if (-psm->val < psm->drvl) psm->val = -psm->drvl; + } + + /* convert */ + temp = psm->val / psm->dist; + psm->rval = temp; +} + +/* + * POSITIONAL_SM + * + * control a stepper motor through position + */ +static void positional_sm(psm) +struct smRecord *psm; +{ + short card,channel; + + /* only VME stepper motor cards supported */ + if (psm->out.type != VME_IO) return; + + /* the motor number is the card number */ + card = psm->out.value.vmeio.card; + channel = psm->out.value.vmeio.signal; + + /* emergency stop */ + if (psm->stop){ + sm_driver(psm->dtyp,card,channel,SM_MOTION,0,0); + psm->stop = 0; + if (psm->mlis.count) db_post_events(psm,&psm->stop,DBE_VALUE); + return; + } + + /* no need to do anymore if the motor is in motion */ + if (psm->movn != 0) + return; + + /* set home when requested */ + if (psm->sthm != 0){ + psm->sthm = 0; /* reset the set home field */ + psm->val = 0; /* make desired value = home */ + psm->rval = 0; /* make the raw value = 0 */ + psm->rbv = 0; + psm->rrbv = 0; + psm->ival = 0; /* resets initial offset */ + sm_driver(psm->dtyp,card,channel,SM_SET_HOME,0,0); + if(psm->mlis.count) { + db_post_events(psm,&psm->rbv,DBE_VALUE); + db_post_events(psm,&psm->val,DBE_VALUE); + db_post_events(psm,&psm->sthm,DBE_VALUE); + } + return; + } + + /* fetch the desired value if there is a database link */ + if (psm->dvl.type == DB_LINK && psm->omsl == CLOSED_LOOP){ + long options=0; + long nRequest=1; + + if(dbGetLink(&(psm->dvl.value.db_link),psm,DBR_FLOAT,&(psm->val),&options,&nRequest)){ + if (psm->nsev < MAJOR_ALARM) { + psm->nsta = READ_ALARM; + psm->nsev = MAJOR_ALARM; + } + return; + } + } + + /* Change of desired position */ + if (psm->lval != psm->val){ + psm->rcnt = 0; + psm->lval = psm->val; + if (psm->mlis.count){ + db_post_events(psm,&psm->rcnt,DBE_VALUE|DBE_LOG); + db_post_events(psm,&psm->lval,DBE_VALUE|DBE_LOG); + } + } + + /* difference between desired position and readback pos */ + if ( (psm->rbv < (psm->val - psm->rdbd)) + || (psm->rbv > (psm->val + psm->rdbd)) ){ + /* one attempt was made - record the error */ + if (psm->rcnt == 1){ + psm->miss = (psm->val - psm->rbv); + if (psm->mlis.count) + db_post_events(psm,&psm->miss,DBE_VALUE|DBE_LOG); + } + + /* should we retry */ + if (psm->rcnt <= psm->rtry){ + /* convert and write the desired value to position */ + convert_sm(psm); + + /* move motor */ + if (sm_driver(psm->dtyp,card,channel,SM_MOVE,psm->rval-psm->rrbv,0) < 0){ + if (psm->nsev < MAJOR_ALARM) { + psm->stat = WRITE_ALARM; + psm->sevr = MAJOR_ALARM; + } + return; + } + psm->movn = 1; + psm->rcnt++; + if (psm->mlis.count){ + db_post_events(psm,&psm->movn,DBE_VALUE|DBE_LOG); + db_post_events(psm,&psm->rcnt,DBE_VALUE|DBE_LOG); + } + /* no more retries - put the record in alarm */ + }else{ + alarm(psm); + } + } + return; +} + +/* + * VELOCITY_SM + * + * control a velocity stepper motor + */ +static void velocity_sm(psm) +struct smRecord *psm; +{ + float chng_vel; + int acceleration,velocity; + short card,channel; + + /* only VME stepper motor cards supported */ + if (psm->out.type != VME_IO) return; + + /* fetch the desired value if there is a database link */ + if (psm->dvl.type == DB_LINK && psm->omsl == CLOSED_LOOP){ + long options=0; + long nRequest=1; + + if(dbGetLink(&(psm->dvl.value.db_link),psm,DBR_FLOAT,&(psm->val),&options,&nRequest)) { + if (psm->nsev < MAJOR_ALARM) { + psm->nsta = READ_ALARM; + psm->nsev = MAJOR_ALARM; + } + return; + } + } + + /* the motor number is the card number */ + card = psm->out.value.vmeio.card; + channel = psm->out.value.vmeio.signal; + + /* Motor not at desired velocity */ + if ((psm->mlst == psm->val) && (psm->val != psm->rbv) && (psm->cvel)) { + alarm(psm); + return; + } + + /* convert the egu velocity to hardware understandable velocity */ + psm->rval = psm->val * psm->mres; + + /* send the new velocity */ + if (psm->rval != 0){ + + /* only if velocity has changed */ + if (psm->velo != psm->val){ + /* acceleration */ + chng_vel = psm->velo - psm->val; + if (chng_vel < 0) chng_vel = -chng_vel; + acceleration = (1/psm->accl) * chng_vel * psm->mres; + + /* velocity */ + psm->velo = psm->val; + velocity = ((psm->val >= 0) ? psm->rval : -psm->rval); + + /* motor commands */ + sm_driver(psm->dtyp,card,channel,SM_VELOCITY,velocity,acceleration); + + /*the last arg of next call is check for direction */ + if(sm_driver(psm->dtyp,card,channel,SM_MOTION,1,(psm->val < 0))){ + if (psm->nsev < MAJOR_ALARM) { + psm->stat = WRITE_ALARM; + psm->sevr = MAJOR_ALARM; + } + return; + } + psm->cvel = 0; + } + }else{ + /* trust it will stop - to read velocity here will not */ + /* work as the motor takes more time to stop than the */ + /* request for velocity takes to return */ + sm_driver(psm->dtyp,card,channel,SM_MOTION,0,0); + psm->rbv = 0; + psm->rrbv = 0; + psm->cvel = 0; + psm->velo = psm->val; + + /* post events */ + if(psm->mlis.count) { + db_post_events(psm,&psm->rbv,DBE_VALUE); + db_post_events(psm,&psm->velo,DBE_VALUE); + } + } +} + +/* + * SM_GET_POSITION + * + * get the stepper motor readback position + */ +static void sm_get_position(psm) +struct smRecord *psm; +{ + short reset; + float new_pos,delta; + + /* get readback position */ + if (psm->rdbl.type == DB_LINK){ + long options=0; + long nRequest=1; + + /* when readback comes from another field of this record */ + /* the fetch will fail if the record is uninitialized */ + reset = psm->init; + if (reset == 0) psm->init = 1; + if(dbGetLink(&(psm->rdbl.value.db_link),psm,DBR_FLOAT,&new_pos,&options,&nRequest)){ + if (psm->nsev < MAJOR_ALARM) { + psm->nsta = READ_ALARM; + psm->nsev = MAJOR_ALARM; + } + psm->init = reset; + return; + } + psm->init = reset; + /* default is the motor position returned from the driver */ + }else{ + new_pos = psm->mpos; + } + + /* readback position at initialization */ + if ((psm->init == 0) && (psm->movn == 0)){ + if (psm->ival == 0){ + psm->rbv = psm->val = new_pos; + }else{ + sm_driver(psm->dtyp, + psm->out.value.vmeio.card, + psm->out.value.vmeio.signal, + SM_SET_HOME,0,0); + psm->rbv = new_pos = psm->val = psm->ival; + } + psm->rval = psm->rrbv = psm->rbv / psm->dist; + psm->init = 1; + if (psm->mlis.count != 0){ + db_post_events(psm,&psm->val,DBE_VALUE|DBE_ALARM); + db_post_events(psm,&psm->rbv,DBE_VALUE|DBE_ALARM); + } + /* readback normally */ + }else{ + if (psm->ival != 0){ + /* adjust if initial position is not 0 */ + new_pos += psm->ival; + } + /* get the raw readback value */ + psm->rrbv = new_pos / psm->dist; + + /* post events */ + if (psm->mlis.count != 0){ + delta = new_pos - psm->rbv; + if ((delta > psm->mdel) || (delta < -psm->mdel)){ + psm->rbv = new_pos; + db_post_events(psm,&psm->rbv,DBE_VALUE|DBE_ALARM); + db_post_events(psm,&psm->rrbv,DBE_VALUE|DBE_ALARM); + } + } + } + +}