diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 8e152e54..a14ca04b 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -133,6 +133,7 @@ struct __MoDriv { float decel; /**< physical units per second^2 */ float maxDecel; /**< physical units per second^2 */ char axisLabel; + char encoderAxis; char lastCmd[CMDLEN]; char dmc2280Error[CMDLEN]; double fHome; /**< home position for axis, default=0 */ @@ -1000,7 +1001,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; - snprintf(cmd, CMDLEN, "TP%c", self->axisLabel); + snprintf(cmd, CMDLEN, "TP%c", self->encoderAxis ? self->encoderAxis : self->axisLabel); if (FAILURE == DMC_SendReceive(self, cmd, reply)) return FAILURE; @@ -1201,11 +1202,15 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) { */ static int cmdStatus(pDMC2280Driv self) { char cmd[CMDLEN]; + char encoder = self->axisLabel; + if (self->encoderAxis && !(self->variables & VAR_ENC)) + encoder = self->encoderAxis; + /* TODO: Use POSx, ENCx, RUNx, SWIx if it has these variables */ snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,_TI0,_TI1,_XQ0", (self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel, - (self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel, + (self->variables & VAR_ENC) ? "ENC" : "_TP", encoder, (self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel, (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel); return DMC_SendReq(self, cmd); @@ -3182,6 +3187,10 @@ static void DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, self->axisLabel); SCWrite(pCon, buffer, eStatus); + if (self->encoderAxis) { + snprintf(buffer, BUFFLEN, "%s.encoderAxis = %c\n", name, self->encoderAxis); + SCWrite(pCon, buffer, eStatus); + } snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, self->units); SCWrite(pCon, buffer, eStatus); return; @@ -3515,6 +3524,13 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { return NULL; } + if ((pPtr=getParam(pCon, interp, params,"encoderaxis",_OPTIONAL)) == NULL) + pNew->encoderAxis=0; + else { + sscanf(pPtr,"%c",&(pNew->encoderAxis)); + if (islower(pNew->encoderAxis)) + pNew->encoderAxis = toupper(pNew->encoderAxis); + } if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); @@ -3687,7 +3703,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, else { char line[132]; snprintf(line, 132, "Invalid axis on motor '%s':%c", self->name, - self->axisLabel); + argv[2][0]); SCWrite(pCon,line,eError); return 0; } @@ -3699,6 +3715,27 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, } return 1; } + else if (strcasecmp("encoderaxis", argv[1]) == 0) { + if (argc > 2) { + if (islower(argv[2][0])) + argv[2][0] = toupper(argv[2][0]); + if (argv[2][0] >= 'A' && argv[2][0] <= 'H') + self->encoderAxis = argv[2][0]; + else { + char line[132]; + snprintf(line, 132, "Invalid encoder axis on motor '%s':%c", self->name, + argv[2][0]); + SCWrite(pCon,line,eError); + return 0; + } + } + else { + char line[132]; + snprintf(line, 132, "%s.encoderAxis = %c", self->name, self->encoderAxis); + SCWrite(pCon, line, eValue); + } + return 1; + } else if (strcasecmp("units", argv[1]) == 0) { if (argc > 2) { strncpy(self->units, argv[2], sizeof(self->units));