diff --git a/motorApp/OmsSrc/devOmsCom.cc b/motorApp/OmsSrc/devOmsCom.cc index 5f414603..4e67f6b9 100644 --- a/motorApp/OmsSrc/devOmsCom.cc +++ b/motorApp/OmsSrc/devOmsCom.cc @@ -66,6 +66,8 @@ HeadURL: $URL$ * .20 07-24-08 rls For MAXv, normalize PID values based on 32,767 maximum. * .21 03-01-10 rls Some MR commands still ignored; change fraction to .9. * .22 11-10-10 rls Error check for valid acceleration rate on STOP command. + * .23 10-26-11 rls Use MAXv motor type to support MRES and ERES with + * different polarity (signs). * */ @@ -81,6 +83,7 @@ HeadURL: $URL$ #include "motor.h" #include "motordevCom.h" #include "devOmsCom.h" +#include "drvMAXv.h" /* WARNING... The following is a COPY of a motorRecord.cc definition. @@ -203,12 +206,12 @@ RTN_STATUS oms_build_trans(motor_cmnd command, double *parms, struct motorRecord struct motor_trans *trans = (struct motor_trans *) mr->dpvt; struct mess_node *motor_call; struct controller *brdptr; + struct MAXvController *MAXvCntrl; char buffer[40]; msg_types cmnd_type; RTN_STATUS rtnind; static bool invalid_velmsg_latch = false; bool MAXv = false; - bool MAXv_PSE = false; int card, signal; long valid_acc; @@ -226,16 +229,15 @@ RTN_STATUS oms_build_trans(motor_cmnd command, double *parms, struct motorRecord return(rtnind = ERROR); brdptr = (*trans->tabptr->card_array)[card]; + if (strncmp(brdptr->ident, "MAXv", 4) == 0) { MAXv = true; - if (brdptr->motor_info[signal].encoder_present == YES && - brdptr->motor_info[signal].pid_present == NO) - MAXv_PSE = true; + MAXvCntrl = (struct MAXvController *) brdptr->DevicePrivate; /* Error check; ER command only for MAXv PSE type motors. */ - if (command == SET_ENC_RATIO && MAXv_PSE == false) + if (command == SET_ENC_RATIO && MAXvCntrl->typeID[signal] != PSE) { - trans->state = IDLE_STATE; /* No command sent to the controller. */ + trans->state = IDLE_STATE; /* No command sent to the controller. */ return(rtnind); } } @@ -514,6 +516,17 @@ errorexit: errMessage(-1, "Invalid device directive"); break; case LOAD_POS: + if ((MAXv == true) && (MAXvCntrl->typeID[signal] != PSO)) + { + long int ref = NINT(parms[0]); + long int fdbk = ref; + if ((mr->mres > 0.0 && mr->eres < 0.0) || + (mr->mres < 0.0 && mr->eres > 0.0)) + fdbk *= -1; + sprintf(motor_call->message, "LO%ld LPE%ld;", ref, fdbk); + } + break; + case JOG: case JOG_VELOCITY: strcat(motor_call->message, ";"); @@ -526,4 +539,3 @@ errorexit: errMessage(-1, "Invalid device directive"); return(rtnind); } - diff --git a/motorApp/OmsSrc/drvMAXv.cc b/motorApp/OmsSrc/drvMAXv.cc index 92d29776..2a3c6796 100644 --- a/motorApp/OmsSrc/drvMAXv.cc +++ b/motorApp/OmsSrc/drvMAXv.cc @@ -42,6 +42,7 @@ HeadURL: $URL$ * - MAXv ver:1.31 (fixes DPRAM encoder position data problem when using * mixed motor types.) * - MAXv ver:1.33, FPGA:B2:A6 BOOT:1.2 (Watchdog Timeout Counter added) + * - MAXv ver:1.34, FPGA:03:A6 BOOT:1.3 * * Modification Log: * ----------------- @@ -88,6 +89,11 @@ HeadURL: $URL$ * 21 02-04-11 rls - Added counter to send_mess()'s "waiting for message * acknowledgement" loop to prevent infinite loop. * 22 09-23-11 ajr - Added configuration word MAXvConfig. + * 23 10-26-11 rls - Changed Debug() to Mark River's variable arguments macro. + * - Added MAXvController data structure using private data in + * motor record to store motor type. Motor type used in + * device support (devOmsCom.cc) to allow MRES and ERES with + * different polarity (signs). * */ @@ -128,21 +134,21 @@ HeadURL: $URL$ #define DONE_QUERY "RA" /* ?? Is this needed?? */ #define PID_QUERY "?KA ID" - /*----------------debugging-----------------*/ -#ifdef __GNUG__ - #ifdef DEBUG - #define Debug(l, f, args...) {if (l <= drvMAXvdebug) \ - errlogPrintf(f, ## args);} - #else - #define Debug(l, f, args...) - #endif -#else - #define Debug -#endif volatile int drvMAXvdebug = 0; extern "C" {epicsExportAddress(int, drvMAXvdebug);} +static inline void Debug(int level, const char *format, ...) { + #ifdef DEBUG + if (level < drvMAXvdebug) { + va_list pVar; + va_start(pVar, format); + vprintf(format, pVar); + va_end(pVar); + } + #endif +} + #define pack2x16(p) ((epicsUInt32)(((p[0])<<16)|(p[1]))) #define INITSTR_SIZE 300 /* 300 byte configuration string. */ @@ -332,7 +338,7 @@ static int set_status(int card, int signal) bool got_encoder; msta_field status; - int absoluteAxis = (configurationFlags[card] & (1 << signal)); + int absoluteAxis = (configurationFlags[card] & (1 << signal)); int rtn_state; @@ -451,9 +457,9 @@ static int set_status(int card, int signal) /* Get encoder position */ if (absoluteAxis) - motorData = pmotor->absPos[signal]; + motorData = pmotor->absPos[signal]; else - motorData = pmotor->encPos[signal]; + motorData = pmotor->encPos[signal]; motor_info->encoder_position = motorData; @@ -1062,6 +1068,7 @@ static int motor_init() struct mess_info *motor_info; volatile struct controller *pmotorState; volatile struct MAXv_motor *pmotor; + struct MAXvController *pvtdata; long status; int card_index, motor_index, itera; char axis_pos[MAX_IDENT_LEN], encoder_pos[MAX_IDENT_LEN], **strptr; @@ -1149,6 +1156,9 @@ static int motor_init() pmotorState->motor_in_motion = 0; pmotorState->cmnd_response = false; + pvtdata = (struct MAXvController *) malloc(sizeof(struct MAXvController)); + pmotorState->DevicePrivate = pvtdata; + if (MAXvInterruptVector == 0) pmotor->IACK_vector = 0; else @@ -1205,6 +1215,7 @@ static int motor_init() for (total_encoders = total_pidcnt = 0, motor_index = 0; motor_index < total_axis; motor_index++) { + motor_info = (struct mess_info *) &pmotorState->motor_info[motor_index]; STATUS1 flag1; /* Test if motor has an encoder. */ @@ -1215,14 +1226,14 @@ static int motor_init() if (pmotor->status1_flag.Bits.cmndError) { Debug(2, "motor_init: No encoder on axis %d\n", motor_index); - pmotorState->motor_info[motor_index].encoder_present = NO; + motor_info->encoder_present = NO; flag1.All = pmotor->status1_flag.All; /* Clear command error. */ pmotor->status1_flag.All = flag1.All; } else { total_encoders++; - pmotorState->motor_info[motor_index].encoder_present = YES; + motor_info->encoder_present = YES; recv_mess(card_index, encoder_pos, 1); } @@ -1233,16 +1244,24 @@ static int motor_init() if (pmotor->status1_flag.Bits.cmndError) { Debug(2, "motor_init: No PID parameters on axis %d\n", motor_index); - pmotorState->motor_info[motor_index].pid_present = NO; + motor_info->pid_present = NO; flag1.All = pmotor->status1_flag.All; /* Clear command error. */ pmotor->status1_flag.All = flag1.All; } else { total_pidcnt++; - pmotorState->motor_info[motor_index].pid_present = YES; + motor_info->pid_present = YES; recv_mess(card_index, encoder_pos, FLUSH); /* Flush response. */ } + + // Set motor type. + if (motor_info->pid_present == YES) + pvtdata->typeID[motor_index] = PSM; + else if (motor_info->encoder_present == YES) + pvtdata->typeID[motor_index] = PSE; + else + pvtdata->typeID[motor_index] = PSO; } /* Enable interrupt-when-done if selected */ @@ -1335,14 +1354,15 @@ extern "C" // Oms Config arguments static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; static const iocshArg configArg1 = {"configuration string", iocshArgString}; + static const iocshArg configArg2 = {"configuration flags", }; static const iocshArg * const OmsSetupArgs[6] = {&setupArg0, &setupArg1, &setupArg2, &setupArg3, &setupArg4, &setupArg5}; - static const iocshArg * const OmsConfigArgs[2] = {&configArg0, &configArg1}; + static const iocshArg * const OmsConfigArgs[3] = {&configArg0, &configArg1, &configArg2}; static const iocshFuncDef setupMAXv = {"MAXvSetup", 6, OmsSetupArgs}; - static const iocshFuncDef configMAXv = {"MAXvConfig", 2, OmsConfigArgs}; + static const iocshFuncDef configMAXv = {"MAXvConfig", 3, OmsConfigArgs}; static void setupMAXvCallFunc(const iocshArgBuf *args) { diff --git a/motorApp/OmsSrc/drvMAXv.h b/motorApp/OmsSrc/drvMAXv.h index 5d90db14..1dda35a2 100644 --- a/motorApp/OmsSrc/drvMAXv.h +++ b/motorApp/OmsSrc/drvMAXv.h @@ -38,6 +38,8 @@ HeadURL: $URL$ * Modification Log: * ----------------- * 01 04-05-04 rls Copied for drvOms58.h + * 02 10-26-11 rls motor_init() sets motor typeID as boot-up. Used by device + * support to allow MRES and ERES to be opposite sign. * */ @@ -54,6 +56,17 @@ HeadURL: $URL$ #define BUFFER_SIZE 1024 + +enum MotorTypes {PSO, // Stepper; w/o encoder + PSE, // Stepper; with encoder + PSM}; // Servo + +struct MAXvController +{ + MotorTypes typeID[8]; +}; + + /* MAXv DUAL-PORT MEMORY MAP */