/** \file motor_dmc2280.c * \brief Driver for Galil DMC2280 motor controller. * * Implements a SICS motor object with a MotorDriver interface. * * Copyright: see file Copyright.txt * * Ferdi Franceschini November 2005 * * TODO * - check for motors enabled on plc * - Check error bit, see Dan's email */ #include /* ISO C Standard: 7.16 Boolean type and values */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "anstoutil.h" #define UNITSLEN 256 #define TEXTPARLEN 1024 #define CMDLEN 1024 /** \brief Used to ensure that the getDMCSetting function is called * with valid values. * \see getDMCSetting */ enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration}; /*----------------------------------------------------------------------- The motor driver structure. Please note that the first set of fields has be identical with the fields of AbstractModriv in ../modriv.h ------------------------------------------------------------------------*/ typedef struct __MoDriv { /* general motor driver interface fields. _REQUIRED! */ float fUpper; /**< hard upper limit */ float fLower; /**< hard lower limit */ char *name; /**< motor name */ int (*GetPosition)(void *self, float *fPos); int (*RunTo)(void *self, float fNewVal); int (*GetStatus)(void *self); void (*GetError)(void *self, int *iCode, char *buffer, int iBufLen); int (*TryAndFixIt)(void *self,int iError, float fNew); int (*Halt)(void *self); int (*GetDriverPar)(void *self, char *name, float *value); int (*SetDriverPar)(void *self,SConnection *pCon, char *name, float newValue); void (*ListDriverPar)(void *self, char *motorName, SConnection *pCon); void (*KillPrivate)(/*@only@*/void *self); int (*GetDriverTextPar)(void *self, char *name, char *textPar); /* DMC-2280 specific fields */ pMultiChan mcc; pMotor pMot; /**< Points to logical motor object */ int errorCode; char *errorMsg; /**< Points to memory for error messages */ char units[256]; /**< physical units for axis */ char long_name[256]; /**< long name of motor */ char part[256]; /**< assembly which motor belongs to */ float speed; /**< physical units per second */ float maxSpeed; /**< physical units per second */ float accel; /**< physical units per second^2 */ float maxAccel; /**< physical units per second^2 */ float decel; /**< physical units per second^2 */ float maxDecel; /**< physical units per second^2 */ char axisLabel; char lastCmd[CMDLEN]; char dmc2280Error[CMDLEN]; float home; /**< home position for axis, default=0 */ int motorHome; /**< motor home position in steps */ int noPowerSave; /**< Flag = 1 to leave motors on after a move */ float stepsPerX; /**< steps per physical unit */ int abs_endcoder; /**< Flag = 1 if there is an abs enc */ int absEncHome; /**< Home position in counts for abs enc */ float cntsPerX; /**< absolute encoder counts per physical unit */ int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */ float lastPosition; /**< Position at last position check */ float lastSteps; float lastCounts; struct timeval time_lastPos_set; /**< Time when lastPosition was set */ float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */ float blockage_thresh; /**< motion threshold for blockage checking */ float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */ int blockage_fail; /**< flag =1 if we should fail the motor */ int has_airpads; /**< Flag = 1 if there is are airpads for this motor */ float backlash_offset; /**< signed offset to drive from */ float fTarget; /**< target passed from SICS to timer callback */ int settle; /**< motor settling time in seconds */ struct timeval time_settle_done; /**< time when settling will be over */ int airpad_state; /**< state of the airpads finite state machine */ int airpad_counter; /**< airpad timer retry counter */ pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */ pNWTimer motor_timer; /**< motor off timer */ int debug; } DMC2280Driv, *pDMC2280Driv; int DMC2280MotionControl = 1; /* defaults to enabled */ #define AIRPADS_DOWN 0 #define AIRPADS_RAISE 1 #define AIRPADS_UP 2 #define AIRPADS_LOWER 3 /*------------------- error codes ----------------------------------*/ #define BADADR -1 // NOT SET: Unknown host/port? #define BADBSY -2 #define BADCMD -3 #define BADPAR -4 // NOT SET: Does SICS already check parameter types? #define BADUNKNOWN -5 #define BADSTP -6 // NOT SET #define BADEMERG -7 // NOT SET: ESTOP #define RVRSLIM -8 #define FWDLIM -9 #define POSFAULT -11 // NOT SET #define BADCUSHION -12 // NOT SET #define ERRORLIM -13 #define IMPOSSIBLE_LIM_SW -14 #define BGFAIL -15 // NOT SET #define BLOCKED -16 #define MOTIONCONTROLOFF -17 #define MOTIONCONTROLUNK -18 #define MOTCMDTMO -19 /* Motor Command Timeout */ /*--------------------------------------------------------------------*/ #define STATUSMOVING 128 /* Motor is moving */ #define STATUSERRORLIMIT 64 /* Number of errorss exceed limit */ #define STATUSOFF 32 /* Motor off */ #define STATUSFWDLIMIT 8 /* Forward limit switch active */ #define STATUSRVRSLIMIT 4 /* Reverse limit switch active */ #define INIT_STR_SIZE 256 #define STR_RESIZE_LENGTH 256 #define BUFFLEN 512 #define _SAVEPOWER 0 #define SETPOS "setpos" #define HOME "home" #define HARDLOWERLIM "hardlowerlim" #define HARDUPPERLIM "hardupperlim" #define UNITS "units" #define SPEED "speed" #define MAXSPEED "maxspeed" #define ACCEL "accel" #define MAXACCEL "maxaccel" #define DECEL "decel" #define MAXDECEL "maxdecel" #define MOTOFFDELAY "motoffdelay" #define AIRPADS "airpads" #define SETTLE "settle" #define LONG_NAME "long_name" #define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval" static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); typedef struct __command Command, *pCommand; typedef int (*CommandCallback)(pCommand pCmd); struct __command { pMultiChan unit; int cstate; int lstate; char* out_buf; int out_len; int out_idx; char* inp_buf; int inp_len; int inp_idx; CommandCallback func; void* cntx; }; /** \brief Convert axis speed in physical units to * motor speed in steps/sec. * \param self (r) provides access to the motor's data structure * \param speed in physical units, eg mm/sec degrees/sec * \return the speed in motor steps/sec */ static int motSpeed(pDMC2280Driv self, float axisSpeed) { int speed; speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5); return speed; } /** \brief Convert axis acceleration in physical units to * to motor speed in steps/sec^2 * \param self (r) provides access to the motor's data structure * \param acceleration in physical units, eg mm/sec^2 degrees/sec^2 * \return the acceleration in motor steps/sec^2 */ static int motAccel(pDMC2280Driv self, float axisAccel) { int accel; accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5); return accel; } /** \brief Convert axis deceleration in physical units to * motor deceleration in steps/sec^2 * \param self (r) provides access to the motor's data structure * \param deceleration in physical units, eg mm/sec^2 degrees/sec^2 * \return the deceleration in motor steps/sec^2 */ static int motDecel(pDMC2280Driv self, float axisDecel) { int decel; decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5); return decel; } static int DMC_Tx(void* ctx) { int iRet = 1; pCommand myCmd = (pCommand) ctx; if (myCmd) { iRet = MultiChanWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len); /* TODO handle errors */ if (iRet < 0) { /* TODO: EOF */ /* iRet = MultiChanReconnect(myCmd->unit); if (iRet == 0) */ return 0; } } return 1; } static int DMC_Rx(void* ctx, int rxchar) { int iRet = 1; pCommand myCmd = (pCommand) ctx; if (rxchar == MCC_TIMEOUT) { /* handle command timeout */ myCmd->inp_idx = MCC_TIMEOUT; if (myCmd->func) iRet = myCmd->func(myCmd); free(myCmd->out_buf); free(myCmd->inp_buf); free(myCmd); return MCC_POP_CMD; } switch (myCmd->cstate) { case 0: /* first character */ if (rxchar == ':') { /* normal prompt */ myCmd->cstate = 99; } else if (rxchar == '?') { /* error prompt, send TC1 ahead of any queued commands */ iRet = MultiChanWrite(myCmd->unit, "TC1\r\n", 5); myCmd->cstate = 1; } else { /* normal data */ myCmd->cstate = 1; } /* note fallthrough */ case 1: /* receiving reply */ if (myCmd->inp_idx < myCmd->inp_len) myCmd->inp_buf[myCmd->inp_idx++] = rxchar; if (rxchar == 0x0D) myCmd->cstate = 2; break; case 2: /* received CR and looking for LF */ if (myCmd->inp_idx < myCmd->inp_len) myCmd->inp_buf[myCmd->inp_idx++] = rxchar; if (rxchar == 0x0A) { /* end of line */ /* myCmd->inp_idx -= 2; myCmd->inp_buf[myCmd->inp_idx++] = rxchar; */ myCmd->cstate = 0; } else myCmd->cstate = 1; break; } if (myCmd->cstate == 99) { myCmd->inp_buf[myCmd->inp_idx] = '\0'; if (strncmp(myCmd->inp_buf, myCmd->out_buf, myCmd->out_len) == 0) { int i; SICSLogWrite("Line echo detected", eStatus); for (i = myCmd->out_len; i <= myCmd->inp_idx; ++i) { myCmd->inp_buf[i - myCmd->out_len] = myCmd->inp_buf[i]; } } if (myCmd->func) iRet = myCmd->func(myCmd); else iRet = 0; myCmd->cstate = 0; myCmd->inp_idx = 0; } if (iRet == 0) { /* end of command */ free(myCmd->out_buf); free(myCmd->inp_buf); free(myCmd); return MCC_POP_CMD; } return iRet; } static int DMC_SendCmd(pMultiChan unit, char* command, int cmd_len, CommandCallback callback, void* context, int rsp_len) { pCommand myCmd = NULL; assert(unit); myCmd = (pCommand) malloc(sizeof(Command)); assert(myCmd); memset(myCmd, 0, sizeof(Command)); myCmd->out_buf = (char*) malloc(cmd_len + 5); memcpy(myCmd->out_buf, command, cmd_len); myCmd->out_len = cmd_len; if (myCmd->out_len < 2 || myCmd->out_buf[myCmd->out_len - 1] != 0x0A || myCmd->out_buf[myCmd->out_len - 2] != 0x0D) { myCmd->out_buf[myCmd->out_len++] = 0x0D; myCmd->out_buf[myCmd->out_len++] = 0x0A; } myCmd->out_buf[myCmd->out_len] = '\0'; myCmd->func = callback; myCmd->cntx = context; if (rsp_len == 0) myCmd->inp_buf = NULL; else { myCmd->inp_buf = malloc(rsp_len + 1); memset(myCmd->inp_buf, 0, rsp_len + 1); } myCmd->inp_len = rsp_len; myCmd->unit = unit; return MultiChanEnque(unit, myCmd, DMC_Tx, DMC_Rx); } static void DMC_Notify(void* context, int event) { pDMC2280Driv self = (pDMC2280Driv) context; char line[132]; switch (event) { case MCC_DISCONNECT: snprintf(line, 132, "Disconnect on Motor '%s'", self->name); SICSLogWrite(line, eStatus); /* TODO: disconnect */ break; case MCC_RECONNECT: snprintf(line, 132, "Reconnect on Motor '%s'", self->name); SICSLogWrite(line, eStatus); /* TODO: reconnect */ break; } return; } typedef struct txn_s { char* transReply; int transWait; } TXN, *pTXN; /** * \brief SendCallback is the callback for the general command. */ static int SendCallback(pCommand pCmd) { char* cmnd = pCmd->out_buf; char* resp = pCmd->inp_buf; int resp_len = pCmd->inp_idx; pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; if (resp_len > 0) { switch (resp[0]) { case ':': case ' ': if (self->debug) { SICSLogWrite(cmnd, eStatus); SICSLogWrite(resp, eStatus); } break; case '?': strncpy(self->lastCmd, pCmd->out_buf, CMDLEN); strncpy(self->dmc2280Error, &resp[1], CMDLEN); SICSLogWrite(cmnd, eError); SICSLogWrite(resp, eError); self->errorCode = BADCMD; break; default: SICSLogWrite(cmnd, eError); SICSLogWrite(resp, eError); self->errorCode = BADUNKNOWN; break; } } else { /* TODO: timeout */ } return 0; } /** * \brief TransCallback is the callback for the general command transaction. */ static int TransCallback(pCommand pCmd) { char* resp = pCmd->inp_buf; int resp_len = pCmd->inp_idx; pTXN self = (pTXN) pCmd->cntx; if (resp_len < 0) { self->transReply[0] = '\0'; self->transWait = -1; } else { memcpy(self->transReply, resp, resp_len); self->transReply[resp_len] = '\0'; self->transWait = 0; } return 0; } /*------------------------------------------------------------------------*/ static int DMC_transact(pDMC2280Driv self, void *send, int sendLen, void *reply, int replyLen) { TXN txn; assert(self); txn.transReply = reply; txn.transWait = 1; DMC_SendCmd(self->mcc, send, sendLen, TransCallback, &txn, replyLen); while (txn.transWait == 1) TaskYield(pServ->pTasker); if (txn.transWait < 0) return txn.transWait; return 1; } static int DMC2280Queue(pDMC2280Driv self, char *cmd, CommandCallback cb) { if (cb == NULL) cb = SendCallback; return DMC_SendCmd(self->mcc, cmd, strlen(cmd), cb, self, CMDLEN); } /** \brief Sends a DMC2280 command to the motor controller. * * If the command fails it displays the DMC2280 error message to the client * and writes it to the log file, also sets errorCode field in motor's * data structure. * * \param self (rw) provides access to the motor's data structure * \param *command (r) DMC2280 command * \return * - SUCCESS * - FAILURE * \see SUCCESS FAILURE */ /* First character returned by controller is '?' for an invalid command or ':' for a valid command with no response ' ' for a valid command with a response (':' follows) */ static int DMC2280Send(pDMC2280Driv self, char *command) { return DMC2280Queue(self, command, SendCallback); } /** * \brief Sends a command and waits for a response * * \param self motor data * \param cmd command to send * \param reply space to return response * \return */ static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) { int status; status = DMC_transact(self, cmd, strlen(cmd), reply, CMDLEN); if (status != 1) { if (self->debug) SICSLogWrite(cmd, eStatus); if (status == -1) self->errorCode = MOTCMDTMO; else self->errorCode = BADUNKNOWN; return FAILURE; } switch (reply[0]) { case ':': if (self->debug) { SICSLogWrite(cmd, eStatus); SICSLogWrite(reply, eStatus); } return SUCCESS; case ' ': if (self->debug) { SICSLogWrite(cmd, eStatus); SICSLogWrite(reply, eStatus); } return SUCCESS; case '?': strncpy(self->lastCmd, cmd, CMDLEN); strncpy(self->dmc2280Error, &reply[1], CMDLEN); SICSLogWrite(cmd, eError); SICSLogWrite(reply, eError); self->errorCode = BADCMD; return FAILURE; default: strncpy(self->lastCmd, cmd, CMDLEN); strncpy(self->dmc2280Error, &reply[0], CMDLEN); SICSLogWrite(cmd, eError); SICSLogWrite(reply, eError); self->errorCode = BADUNKNOWN; return FAILURE; } return OKOK; } /** \brief Record the given posn and timestamp it. * * \param *pData provides access to a motor's data * \param posn, the axis position which you want to remember. * */ static void set_lastMotion(pDMC2280Driv self, float steps, float counts) { assert(self != NULL); self->lastSteps = steps; self->lastCounts = counts; gettimeofday(&(self->time_lastPos_set), NULL); } /** \brief Reads motion. * * \param *steps motor steps * \param *counts encoder counts * \return * SUCCESS * FAILURE */ static int readMotion(pDMC2280Driv self, float *steps, float *counts) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; if (2 != sscanf(reply, "%f %f", steps, counts)) return FAILURE; return SUCCESS; } /** \brief Reads absolute encoder. * * \param *pos is the absolute encoder reading on SUCCESS. * \return * SUCCESS * FAILURE */ static int readAbsEnc(pDMC2280Driv self, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "TP%c", self->axisLabel); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; *pos = (float) atoi(reply); return SUCCESS; } /** \brief Reads Thread variable * * \param *pos is the thread variable value on SUCCESS. * \return * SUCCESS * FAILURE */ static int ReadThread(pDMC2280Driv self, int thread, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG _XQ%d", thread); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return 0; *pos = (float) atoi(reply); return 1; } /** \brief Reads HOMERUN variable * * \param *pos is the homerun variable value on SUCCESS. * \return * SUCCESS * FAILURE */ static int readHomeRun(pDMC2280Driv self, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG HOMERUN"); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; *pos = (float) atoi(reply); return SUCCESS; } /** \brief Runs the HOMERUN routine on the MC * * \return * SUCCESS * FAILURE */ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { float fValue; char cmd[CMDLEN]; /* Read HOMERUN should get error if it does not have one */ if (FAILURE == readHomeRun(self, &fValue)) return FAILURE; /* 0 => reset homerun */ if (newValue < 0.5) { snprintf(cmd, CMDLEN, "HOMERUN=0"); if (FAILURE == DMC2280Send(self, cmd)) return FAILURE; return SUCCESS; } snprintf(cmd, CMDLEN, "XQ #HOME,1"); if (FAILURE == DMC2280Send(self, cmd)) return FAILURE; return SUCCESS; } /** \brief Reads motor position, implements the GetPosition * method in the MotorDriver interface. * * \param *pData provides access to a motor's data * \param *fPos contains the motor position in physical units after a call. * \return * - OKOK request succeeded * - HWFault request failed * */ static int DMC2280GetPos(void *pData, float *fPos){ pDMC2280Driv self = NULL; char reply[CMDLEN]; char cmd[CMDLEN]; float absEncPos, motorPos; reply[0]='\0'; self = (pDMC2280Driv)pData; assert(self != NULL); if (1 == self->abs_endcoder) { if (readAbsEnc(self, &absEncPos) == FAILURE) return HWFault; *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home; } else { snprintf(cmd, ERRLEN, "TD%c", self->axisLabel); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return HWFault; motorPos =(float)atof(reply); *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home; } return OKOK; } /** * \brief calculate and issue the motion commands * * \param self provides access to a motor's data * \param fValue target position in physical units, software zeros * have already been applied. * \return * - OKOK request succeeded * - HWFault request failed */ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ char axis; char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN]; int absEncHome, motorHome, newAbsPosn; float stepsPerX, cntsPerX; float target; axis=self->axisLabel; motorHome = self->motorHome; stepsPerX=self->stepsPerX; snprintf(SHx, CMDLEN, "SH%c", axis); snprintf(BGx, CMDLEN, "BG%c", axis); target = fValue - self->home; if (self->backlash_offset > FLT_EPSILON && target < self->lastPosition) { target += self->backlash_offset; if (target > self->fUpper) target = self->fUpper; } if (self->backlash_offset < -FLT_EPSILON && target > self->lastPosition) { target += self->backlash_offset; if (target < self->fLower) target = self->fLower; } if (1 == self->abs_endcoder) { absEncHome = self->absEncHome; cntsPerX = self->cntsPerX; #if 0 /* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */ snprintf(absPosCmd, CMDLEN, "PA%c=(((%d-_TP%c)/%f)+%f)*%f + _TD%c", axis, absEncHome, axis, cntsPerX, target, stepsPerX, axis); #else /* PAZ=((absEncHome-_TPZ) + (cntsPerX * target)) * stepsPerX / cntsPerX + _TDZ */ char s_cnts[20]; char s_trgt[20]; char s_stps[20]; int i; snprintf(s_cnts, sizeof(s_cnts), "%.4f", cntsPerX); for (i = strlen(s_cnts); i > 0; --i) if (s_cnts[i-1] == '.') { s_cnts[i-1] = '\0'; break; } else if (s_cnts[i-1] == '0') s_cnts[i-1] = '\0'; else break; snprintf(s_trgt, sizeof(s_trgt), "%.4f", target); for (i = strlen(s_trgt); i > 0; --i) if (s_trgt[i-1] == '.') { s_trgt[i-1] = '\0'; break; } else if (s_trgt[i-1] == '0') s_trgt[i-1] = '\0'; else break; snprintf(s_stps, sizeof(s_stps), "%.4f", stepsPerX); for (i = strlen(s_stps); i > 0; --i) if (s_stps[i-1] == '.') { s_stps[i-1] = '\0'; break; } else if (s_stps[i-1] == '0') s_stps[i-1] = '\0'; else break; snprintf(absPosCmd, CMDLEN, "PA%c=((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c", axis, absEncHome, axis, s_cnts, s_trgt, s_stps, s_cnts, axis); #endif #ifdef BACKLASHFIX do { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis, (int) (target * cntsPerX + absEncHome + 0.5)); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; } while (0); #endif } else { newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5); snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn); } if (FAILURE == DMC2280Send(self, SHx)) return HWFault; if (FAILURE == DMC2280Send(self, absPosCmd)) return HWFault; if (FAILURE == DMC2280Send(self, BGx)) return HWFault; return OKOK; } /** * \brief process the airpad status response */ static int airpad_callback(pCommand pCmd) { char* resp = pCmd->inp_buf; int resp_len = pCmd->inp_idx; pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; if (resp_len > 0) { float fReply; if (self->debug) SICSLogWrite(resp, eStatus); fReply = (float) atof(resp); if (self->airpad_state == AIRPADS_RAISE && fReply > 0) { int iRet; self->airpad_state = AIRPADS_UP; iRet = DMC2280RunCommon(self, self->fTarget); if (iRet != OKOK) self->errorCode = BADCUSHION; return 0; } if (self->airpad_state == AIRPADS_LOWER && fReply == 0) { self->airpad_state = AIRPADS_DOWN; return 0; } } else { /* TODO: timeout */ } return 0; } /** * \brief request the airpad status periodically */ static int airpad_timeout(void* ctx, int mode) { pDMC2280Driv self = (pDMC2280Driv) ctx; self->airpad_timer = NULL; if (self->airpad_state == AIRPADS_UP || self->airpad_state == AIRPADS_DOWN) return 0; if (self->airpad_counter <= 0) { self->errorCode = BADCUSHION; self->airpad_state = AIRPADS_DOWN; return 0; } --self->airpad_counter; NetWatchRegisterTimer(&self->airpad_timer, 1000, airpad_timeout, self); if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) { self->errorCode = BADCUSHION; self->airpad_state = AIRPADS_DOWN; return 0; } return 0; } /** * \brief initiate the raising or lowering of the airpads * * \param self motor data * \param flag 1 for raise and 0 for lower * \return 1 for SUCCESS or 0 for FAILURE */ static int DMC_AirPads(pDMC2280Driv self, int flag) { char *cmd = NULL; if (self->airpad_timer) NetWatchRemoveTimer(self->airpad_timer); self->airpad_timer = NULL; if (flag) { cmd = "FTUBE=1"; self->airpad_state = AIRPADS_RAISE; } else { cmd = "FTUBE=0"; self->airpad_state = AIRPADS_LOWER; } if (FAILURE == DMC2280Send(self, cmd)) { self->airpad_state = AIRPADS_DOWN; self->errorCode = BADCUSHION; return 0; } self->airpad_counter = 10; NetWatchRegisterTimer(&self->airpad_timer, 1000, airpad_timeout, self); return 1; } /** * \brief turn the motor off after a delay * * \param context motor data * \param mode */ static int motor_timeout(void* context, int mode) { pDMC2280Driv self = (pDMC2280Driv) context; char cmd[CMDLEN]; self->motor_timer = NULL; if (self->has_airpads) { DMC_AirPads(self, 0); return 0; } snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); DMC2280Send(self, cmd); return 0; } /** * \brief handle the run command for a motor with airpads * * \param self motor data * \param fValue new motor position */ static int DMC2280RunAir(pDMC2280Driv self, float fValue) { self->fTarget = fValue; if (!DMC_AirPads(self, 1)) return HWFault; return OKOK; } /** \brief DMC2280 implementation of the RunTo * method in the MotorDriver interface. * * \param *pData provides access to a motor's data * \param fValue target position in physical units, software zeros * have already been applied. * \return * - OKOK request succeeded * - HWFault request failed * */ static int DMC2280Run(void *pData,float fValue){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; assert(self != NULL); /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { if (DMC2280MotionControl == 0) self->errorCode = MOTIONCONTROLOFF; else self->errorCode = MOTIONCONTROLUNK; return HWFault; } if (self->motor_timer) NetWatchRemoveTimer(self->motor_timer); self->motor_timer = NULL; if (self->settle) self->time_settle_done.tv_sec = 0; /* * Note: this will read the current position which will block */ do { float currPos; DMC2280GetPos(pData, &currPos); self->lastPosition = currPos; #if 1 self->time_lastPos_set.tv_sec = 0; #endif } while (0); /* * Note: this passes control to a timer routine */ if (self->has_airpads) if (self->airpad_state != AIRPADS_UP) return DMC2280RunAir(self, fValue); return DMC2280RunCommon(self, fValue); } /** \brief Check if the axis has moved significantly since * the last check. * * The motion is checked against the expected at intervals of * pDMC2280Driv->blockage_ckInterval. * * \param *pData provides access to a motor's data * \return * - 1 MOTOR OK, position has changed significantly during move * - 0 MOTOR BLOCKED, no significant change in position detected. */ static int checkMotion(void *pData) { float precision, steps, counts, ratio_obs, ratio_exp, ratio_cmp; long int usec_TimeDiff; struct timeval now; pDMC2280Driv self; self = (pDMC2280Driv)pData; assert(self != NULL); /* we can only test if there is an absolute encoder */ if (!self->abs_endcoder) return 1; if (self->time_lastPos_set.tv_sec == 0) { /* first time - initialise the data */ if (FAILURE == readMotion(self, &steps, &counts)) return 0; set_lastMotion(pData, steps, counts); return 1; } gettimeofday(&now, NULL); usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec; usec_TimeDiff *= 1000000; usec_TimeDiff += now.tv_usec; usec_TimeDiff -= self->time_lastPos_set.tv_usec; if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval)) return 1; if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); MotorGetPar(self->pMot,"precision",&precision); if (FAILURE == readMotion(self, &steps, &counts)) return 0; /* If not stepping, then not blocked */ if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ set_lastMotion(pData, self->lastSteps, self->lastCounts); return 1; } /* calculate observed and expected steps per count ratios */ if (counts == self->lastCounts) /* prevent divide by zero */ ratio_obs = (steps - self->lastSteps); else ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts); ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX; ratio_cmp = ratio_obs / ratio_exp; /* wrong signs, less than half, or more than double is trouble */ if (ratio_cmp < 0.0 || ratio_cmp > self->blockage_ratio || (1.0 / ratio_cmp) > self->blockage_ratio) { char msg[132]; snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f", ratio_obs, ratio_exp); SICSLogWrite(msg, eError); snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%f/%f", steps, self->lastSteps, counts, self->lastCounts, self->stepsPerX, self->cntsPerX); SICSLogWrite(msg, eError); if (self->blockage_fail) return 0; set_lastMotion(pData, steps, counts); return 1; } else { if (self->debug) { char msg[132]; snprintf(msg, sizeof(msg), "Motion check pass: obs=%f, exp=%f", ratio_obs, ratio_exp); SICSLogWrite(msg, eError); } set_lastMotion(pData, steps, counts); return 1; } return 1; } /** \brief Returns the motor status while it's moving, * implements the GetStatus method in the MotorDriver interface. * * \param *pData provides access to a motor's data * \return * - HWFault hardware fault or status cannot be read. * - HWPosFault controller was unable to position the motor. * - HWBusy The motor is still driving. * - HWWarn There is a warning from the controller. * - HWIdle The motor has finished driving and is idle. */ static int DMC2280Status(void *pData){ pDMC2280Driv self = NULL; char cmd[CMDLEN]; int switches; char switchesAscii[CMDLEN]; #ifdef BACKLASHFIX char reply[CMDLEN]; int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus; int SHOULD_FIXPOS=1, should_fixpos; #endif bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; self = (pDMC2280Driv)pData; assert(self != NULL); /* * If we are waiting for the motor or airpads then we * are busy */ if (self->motor_timer || self->airpad_timer) return HWBusy; /* Make sure that speed, accel and decel are set correctly */ /* ckSpeedAccelDecel(self); */ /* Get status of switches * see TS (Tell Switches) in Galil manc2xx.pdf */ snprintf(cmd, CMDLEN, "TS%c", self->axisLabel); if (FAILURE == DMC2280SendReceive(self, cmd, switchesAscii)) return HWFault; sscanf(switchesAscii, "%d", &switches); moving = (switches & STATUSMOVING)>0; fwd_limit_active = !((switches & STATUSFWDLIMIT)>0); rvrs_limit_active = !((switches & STATUSRVRSLIMIT)>0); errorlimit = (switches & STATUSERRORLIMIT)>0; if (fwd_limit_active && rvrs_limit_active) { self->errorCode = IMPOSSIBLE_LIM_SW; return HWFault; } if (moving) { int iRet; /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { if (DMC2280MotionControl == 0) self->errorCode = MOTIONCONTROLOFF; else self->errorCode = MOTIONCONTROLUNK; return HWFault; } /* If pos hasn't changed since last * check then stop and scream */ #if 0 iRet = checkPosition(pData); #else iRet = checkMotion(pData); #endif if (iRet == 0) { DMC2280Halt(pData); self->errorCode = BLOCKED; return HWFault; } else { self->errorCode = BADBSY; return HWBusy; } } else { /* If motor stopped check limits and error status */ if (fwd_limit_active) { self->errorCode = FWDLIM; return HWFault; } else if (rvrs_limit_active) { self->errorCode = RVRSLIM; return HWFault; } else if (errorlimit) { self->errorCode = ERRORLIM; return HWFault; } #ifdef BACKLASHFIX if (self->abs_endcoder == 1) { /* Make sure that the servo loop is closed by checking if * the CLSLOOP thread is running on the controller.*/ if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply)) return HWFault; sscanf(reply, "%d", &servoLoopStatus); if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) { /* Start subroutine on controller to close the servo loop */ if (FAILURE == DMC2280Send(self, "XQ#CLSLOOP")) return HWFault; } snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return HWFault; sscanf(reply, "%d", &should_fixpos); if (should_fixpos == SHOULD_FIXPOS) { snprintf(cmd, CMDLEN, "%cFIXPOS=1", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; self->errorCode=BADBSY; return HWBusy; } } #endif /* * When we get here, the motion has completed and we * must determine when and how to shut off the motor */ if (self->settle) { struct timeval *then = &self->time_settle_done; struct timeval now; gettimeofday(&now, NULL); if (then->tv_sec == 0 || (then->tv_sec - now.tv_sec) > self->settle) { gettimeofday(then, NULL); then->tv_sec += self->settle; return HWBusy; } else { if ((now.tv_sec > then->tv_sec) || ((now.tv_sec == then->tv_sec) && (now.tv_usec >= then->tv_usec))) { /* it's finished, fall through */ } else { return HWBusy; } } } if (self->has_airpads) { if (self->airpad_state == AIRPADS_DOWN) return HWIdle; if (self->airpad_state == AIRPADS_LOWER) return HWBusy; if (self->motOffDelay > 0 ) { NetWatchRegisterTimer(&self->motor_timer, self->motOffDelay, motor_timeout, self); return HWIdle; } if (!DMC_AirPads(self, 0)) return HWFault; return HWIdle; } if (self->noPowerSave == _SAVEPOWER) { if (self->motOffDelay > 0 ) { #if 0 snprintf(cmd, CMDLEN, "AT %d; MO%c", self->motOffDelay, self->axisLabel); #else NetWatchRegisterTimer(&self->motor_timer, self->motOffDelay, motor_timeout, self); return HWIdle; #endif } else { snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); } DMC2280Send(self, cmd); } return HWIdle; } } /** \brief DMC2280 implementation of the GetError * method in the MotorDriver interface. * * \param *pData provides access to a motor's data * \param *iCode error code returned to abstract motor * \param *error error message * \param errLen maximum error message length allowed by abstract motor */ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; assert(self != NULL); /* Allocate errLen bytes for error messages */ if (self->errorMsg == NULL) { self->errorMsg = (char *) malloc(errLen); if (self->errorMsg == NULL) { *iCode = 0; return; } } *iCode = self->errorCode; switch(*iCode){ #ifdef HAS_RS232 case NOTCONNECTED: case BADSEND: case BADMEMORY: case INCOMPLETE: strncpy(error, "General Error(TODO)", (size_t)errLen); /* TODO */ break; #endif case MOTCMDTMO: strncpy(error, "Command Timeout", (size_t)errLen); break; case BADADR: strncpy(error, "Bad address", (size_t)errLen); break; case BADBSY: strncpy(error, "Motor still busy", (size_t)errLen); break; case BADCMD: snprintf(error, (size_t)errLen, "Bad command: '%s'\ndmcError: ", self->lastCmd); strncat(error, self->dmc2280Error, (size_t)errLen); break; case BADPAR: strncpy(error, "Bad parameter", (size_t)errLen); break; case BADUNKNOWN: strncpy(error, "Unknown error condition", (size_t)errLen); break; case BADSTP: strncpy(error, "Motor is stopped", (size_t)errLen); break; case BADEMERG: strncpy(error, "Emergency stop is engaged", (size_t)errLen); break; case BGFAIL: strncpy(error, "Begin not possible due to limit switch", (size_t)errLen); break; case RVRSLIM: strncpy(error, "Crashed into limit switch", (size_t)errLen); break; case FWDLIM: strncpy(error, "Crashed into limit switch", (size_t)errLen); break; case POSFAULT: strncpy(error, "Positioning fault detected", (size_t)errLen); break; case BADCUSHION: strncpy(error, "Air cushion problem", (size_t)errLen); break; case ERRORLIM: strncpy(error, "Axis error exceeds error limit", (size_t)errLen); break; case IMPOSSIBLE_LIM_SW: strncpy(error, "Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen); break; case BLOCKED: strncpy(error, "STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen); break; case MOTIONCONTROLOFF: strncpy(error, "MOTION CONTROL SEEMS TO BE DISABLED", (size_t)errLen); break; case MOTIONCONTROLUNK: strncpy(error, "MOTION CONTROL SEEMS TO BE UNKNOWN", (size_t)errLen); break; default: /* FIXME What's the default */ snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode); break; } strncpy(self->errorMsg, error, (size_t)errLen); } /** \brief Attempts to recover from an error. Implements the TryAndFixIt * method in the MotorDriver interface. * * \param *pData provides access to a motor's data * \param iCode error code returned by DMC2280Error * \param fValue unused, target position * \return A return code which informs the abstract motors next action. * - MOTREDO try to redo the last move. * - MOTFAIL move failed, give up. */ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; assert(self != NULL); switch(iCode){ case BADADR: return MOTFAIL; case BADCMD: case BADPAR: case BLOCKED: case MOTIONCONTROLOFF: case MOTIONCONTROLUNK: case MOTCMDTMO: return MOTFAIL; case POSFAULT: #if HAS_RS232 case BADSEND: case TIMEOUT: case BADMEMORY: /* Won't happen if MonConnect sets the send terminator */ case INCOMPLETE: return MOTREDO; case NOTCONNECTED: #endif return MOTREDO; break; default: return MOTFAIL; break; } } /** \brief Emergency halt. Implements the Halt * method in the MotorDriver interface. * * Cannot set maximum deceleration because the DC command * is not valid for absolute (ie PA) moves. See DC description * in DMC-2xxx command ref (ie manc2xxx.pdf) * \param *pData provides access to a motor's data * * XXX Does abstract motor use the return values? */ static int DMC2280Halt(void *pData){ pDMC2280Driv self = NULL; char cmd[CMDLEN]; self = (pDMC2280Driv)pData; assert(self != NULL); /* Stop motor */ snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; /* TODO: FIXME cannot do this until motor stops */ snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; else return 1; } /** \brief Allow motor drivers to return text values like units. * * Refactoring: Should replace this with a function that will * return hdb values. * This is the first step in the refactoring process. */ int DMC2280GetTextPar(void *pData, char *name, char *textPar) { pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; if(strcasecmp(name,UNITS) == 0) { snprintf(textPar, UNITSLEN, self->units); return 1; } return 0; } /** \brief Fetches the value of the named parameter, * implements the GetDriverPar method in the MotorDriver interface. * * Note: The GetDriverPar method in the MotorDriver interface only * allows float values to be returned. * * If the speed, acceleration or deceleration is requested then * this compares the setting on the controller to the required setting, * if they don't match then the controller is set to the required value. * * Note: Doesn't warn if the speed, acceleration, or deceleration set on * the controller differ from the required settings. * * \param *pData (r) provides access to a motor's data * \param *name (r) the name of the parameter to fetch. * \param *fValue (w) the parameter's value. * \return * - 1 request succeeded * - 0 request failed * */ static int DMC2280GetPar(void *pData, char *name, float *fValue){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; /* XXX Maybe move this to a configuration parameter.*/ if(strcasecmp(name,HOME) == 0) { *fValue = self->home; return 1; } if(strcasecmp(name,HARDLOWERLIM) == 0) { *fValue = self->fLower; return 1; } if(strcasecmp(name,HARDUPPERLIM) == 0) { *fValue = self->fUpper; return 1; } if(strcasecmp(name,SPEED) == 0) { *fValue = self->speed; return 1; } if(strcasecmp(name,MAXSPEED) == 0) { *fValue = self->maxSpeed; return 1; } if(strcasecmp(name,ACCEL) == 0) { *fValue = self->accel; return 1; } if(strcasecmp(name,MAXACCEL) == 0) { *fValue = self->maxAccel; return 1; } if(strcasecmp(name,DECEL) == 0) { *fValue = self->decel; return 1; } if(strcasecmp(name,MAXDECEL) == 0) { *fValue = self->maxDecel; return 1; } if(strcasecmp(name,MOTOFFDELAY) == 0) { *fValue = self->motOffDelay; return 1; } if(strcasecmp(name,"debug") == 0) { *fValue = self->debug; return 1; } if(strcasecmp(name,SETTLE) == 0) { *fValue = self->settle; return 1; } if(strcasecmp(name,AIRPADS) == 0) { *fValue = self->has_airpads; return 1; } if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { *fValue = self->blockage_ckInterval; return 1; } if(strcasecmp(name,"blockage_thresh") == 0) { *fValue = self->blockage_thresh; return 1; } if(strcasecmp(name,"blockage_ratio") == 0) { *fValue = self->blockage_ratio; return 1; } if(strcasecmp(name,"blockage_fail") == 0) { *fValue = self->blockage_fail; return 1; } if(strcasecmp(name,"backlash_offset") == 0) { *fValue = self->backlash_offset; return 1; } if (self->abs_endcoder != 0) { if (strcasecmp(name,"absenc") == 0) { if (readAbsEnc(self, fValue) == SUCCESS) return 1; else return 0; } if (strcasecmp(name,"absenchome") == 0) { *fValue = self->absEncHome; return 1; } } else { if (strcasecmp(name,"homerun") == 0) { if (readHomeRun(self, fValue) == SUCCESS) return 1; else return 0; } } if(strcasecmp(name,"thread0") == 0) return ReadThread(self, 0, fValue); if(strcasecmp(name,"thread1") == 0) return ReadThread(self, 1, fValue); if(strcasecmp(name,"thread2") == 0) return ReadThread(self, 2, fValue); if(strcasecmp(name,"thread3") == 0) return ReadThread(self, 3, fValue); if(strcasecmp(name,"thread4") == 0) return ReadThread(self, 4, fValue); if(strcasecmp(name,"thread5") == 0) return ReadThread(self, 5, fValue); if(strcasecmp(name,"thread6") == 0) return ReadThread(self, 6, fValue); if(strcasecmp(name,"thread7") == 0) return ReadThread(self, 7, fValue); return 0; } /** \brief Sets the named parameter, implements the SetDriverPar * method in the MotorDriver interface. * * Note: The SetDriverPar method in the MotorDriver interface only * allows float values to be set. * \param *pData (rw) provides access to a motor's data * \param *pCon (r) connection object. * \param *name (r) of the parameter to set. * \param *newValue (r) new value. * \return * - 1 request succeeded * - 0 request failed * */ static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue){ pDMC2280Driv self = NULL; char pError[ERRLEN]; char cmd[CMDLEN]; float oldZero, newZero; self = (pDMC2280Driv)pData; /* XXX Maybe move this to a configuration parameter.*/ /* Set home, managers only. Users should set softposition */ if(strcasecmp(name,HOME) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { if ( (self->fLower - newValue) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower); SCWrite(pCon, pError, eError); return 1; } if ( (newValue - self->fUpper) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper); SCWrite(pCon, pError, eError); return 1; } self->home = newValue; return 1; } } if(strcasecmp(name,SETPOS) == 0) { if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); MotorGetPar(self->pMot,"softzero",&oldZero); newZero = (self->pMot->fPosition - newValue) + oldZero; MotorSetPar(self->pMot,pCon,"softzero",newZero); return 1; } /* Set motor off delay, managers only */ if(strcasecmp(name,MOTOFFDELAY) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->motOffDelay = newValue; return 1; } } /* Debug, managers only */ if(strcasecmp(name,"debug") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->debug = newValue; return 1; } } /* Setttle, managers only */ if(strcasecmp(name,SETTLE) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->settle = newValue; return 1; } } /* Set airpads, managers only */ if(strcasecmp(name,AIRPADS) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->has_airpads = newValue; return 1; } } /* Set interval between blocked motor checks, * managers only */ if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_ckInterval = newValue; return 1; } } /* Set movement threshold for blocked motor checks, * managers only */ if(strcasecmp(name,"blockage_thresh") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_thresh = newValue; return 1; } } /* Set ratio for blocked motor checks, * managers only */ if(strcasecmp(name,"blockage_ratio") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_ratio = newValue; return 1; } } /* Set blocked motor checks failure mode, * managers only */ if(strcasecmp(name,"blockage_fail") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_fail = newValue; return 1; } } /* Set backlash offset, * managers only */ if(strcasecmp(name,"backlash_offset") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->backlash_offset = newValue; return 1; } } /* Invoke Home Run routine in controller, * managers only */ if(self->abs_endcoder == 0 && strcasecmp(name,"homerun") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { if (DMC2280MotionControl != 1 && newValue > 0.5) { snprintf(pError, ERRLEN,"ERROR: Motion Control must be on"); SCWrite(pCon, pError, eError); } RunHomeRoutine(self, newValue); return 1; } } /* Set speed */ if(strcasecmp(name,SPEED) == 0) { if ((0.0 - newValue) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, SPEED, 0.0); SCWrite(pCon, pError, eError); return 1; } if ((newValue - self->maxSpeed ) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, SPEED, self->maxSpeed); SCWrite(pCon, pError, eError); return 1; } self->speed = newValue; snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed)); if (FAILURE == DMC2280Send(self, cmd)) return 0; /* FIXME should signal a HWFault */ return 1; } /* Set Acceleration */ if(strcasecmp(name,ACCEL) == 0) { if ((0.0 - newValue) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, ACCEL, 0.0); SCWrite(pCon, pError, eError); return 1; } if ((newValue - self->maxAccel ) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, ACCEL, self->maxAccel); SCWrite(pCon, pError, eError); return 1; } self->accel = newValue; snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel)); if (FAILURE == DMC2280Send(self, cmd)) return 0; /* FIXME should signal a HWFault */ return 1; } /* Set Deceleration */ if(strcasecmp(name,DECEL) == 0) { if ((0.0 - newValue) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, DECEL, 0.0); SCWrite(pCon, pError, eError); return 1; } if ((newValue - self->maxDecel ) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, DECEL, self->maxDecel); SCWrite(pCon, pError, eError); return 1; } self->decel = newValue; snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel)); if (FAILURE == DMC2280Send(self, cmd)) return 0; /* FIXME should signal a HWFault */ return 1; } return 0; } /** \brief List the motor parameters to the client. * \param self (r) provides access to the motor's data structure * \param *name (r) name of motor. * \param *pCon (r) connection object. */ static void DMC2280List(void *self, char *name, SConnection *pCon){ char buffer[BUFFLEN]; snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, ((pDMC2280Driv)self)->long_name); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, ((pDMC2280Driv)self)->axisLabel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, ((pDMC2280Driv)self)->stepsPerX); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, ((pDMC2280Driv)self)->home); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, ((pDMC2280Driv)self)->units); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, ((pDMC2280Driv)self)->speed); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, ((pDMC2280Driv)self)->maxSpeed); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, ((pDMC2280Driv)self)->accel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, ((pDMC2280Driv)self)->maxAccel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, ((pDMC2280Driv)self)->decel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, ((pDMC2280Driv)self)->maxDecel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, ((pDMC2280Driv)self)->motOffDelay); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, ((pDMC2280Driv)self)->debug); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, ((pDMC2280Driv)self)->blockage_ckInterval); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, ((pDMC2280Driv)self)->blockage_thresh); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, ((pDMC2280Driv)self)->blockage_ratio); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, ((pDMC2280Driv)self)->blockage_fail); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %d\n", name, ((pDMC2280Driv)self)->backlash_offset); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder); SCWrite(pCon, buffer, eStatus); if (((pDMC2280Driv)self)->abs_endcoder) { snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, ((pDMC2280Driv)self)->absEncHome); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, ((pDMC2280Driv)self)->cntsPerX); SCWrite(pCon, buffer, eStatus); } return; } /** \brief Free memory if motor is removed * \param *pData (rw) provides access to the motor's data structure */ static void KillDMC2280(/*@only@*/void *pData){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; assert(self != NULL); if (self->name) { free(self->name); self->name = NULL; } if (self->errorMsg) { free(self->errorMsg); self->errorMsg = NULL; } if (self->mcc) { MultiChanDestroy(self->mcc); self->mcc = NULL; } /* Not required as performed in caller * free(self); */ return; } /** \brief Create a driver for the DMC2280 Galil controller. * * This is called by the Motor configuration command in the * SICS configuration file when you create a DMC2280 motor. * * Usage:\n * Motor stth DMC2280 paramArray\n * - stth is the motor name * - DMC2280 is the motor type that will lead to calling this function. * - paramArray is a Tcl array of the motor parameters. * * \param *pCon (r) connection object. * \param *motor (r) motor name * \param *params (r) configuration parameter array. * \return a reference to Motordriver structure * * NOTES:\n * -Adding parameters * - Add a field for the parameter to the DMC2280Driv struct * - Get the parameter from the parameter array, see PARAMETERS: below * - If the parameter requires an abs enc then add it after ABSENC: */ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pDMC2280Driv pNew = NULL; char *pPtr = NULL; char buffer[132]; char pError[ERRLEN]; char cmd[CMDLEN]; Tcl_Interp *interp; buffer[0]='\0'; interp = InterpGetTcl(pServ->pSics); /* allocate and initialize data structure */ pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv)); if(NULL == pNew){ snprintf(pError, ERRLEN, "ERROR: no memory when creating DMC2280 motor '%s'", motor); SCWrite(pCon, pError, eError); return NULL; } memset(pNew, 0, sizeof(DMC2280Driv)); /* Get multichan from the list of named parameters */ if ((pPtr=getParam(pCon, interp, params, "multichan", _OPTIONAL)) != NULL) { /* MultiChan */ if (!MultiChanCreate(pPtr, &pNew->mcc)) { snprintf(pError, ERRLEN, "Cannot find MultiChan '%s' when creating DMC2280 motor '%s'", pPtr, motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew); return NULL; } MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify); } else if ((pPtr=getParam(pCon, interp, params, "host", _OPTIONAL)) != NULL) { char* host = pPtr; if ((pPtr=getParam(pCon, interp, params,"port",_REQUIRED)) == NULL) { snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew); return NULL; } /* MultiChan */ if (!MultiChanCreateHost(host, pPtr, &pNew->mcc)) { snprintf(pError, ERRLEN, "Cannot create MultiChan '%s:%s' for DMC2280 motor '%s'", host, pPtr, motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew); return NULL; } MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify); } else { snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew); return NULL; } pNew->name = (char *)malloc(sizeof(char)*(strlen(motor)+1)); if (pNew->name == NULL) { (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver", eError); KillDMC2280(pNew); return NULL; } pNew->pMot = NULL; strcpy(pNew->name, motor); pNew->home = 0.0; pNew->fLower = 0.0;//(float)atof(argv[2]); pNew->fUpper = 0.0;//(float)atof(argv[3]); pNew->GetPosition = DMC2280GetPos; pNew->RunTo = DMC2280Run; pNew->GetStatus = DMC2280Status; pNew->GetError = DMC2280Error; pNew->TryAndFixIt = DMC2280Fix; pNew->Halt = DMC2280Halt; pNew->GetDriverPar = DMC2280GetPar; pNew->SetDriverPar = DMC2280SetPar; pNew->ListDriverPar = DMC2280List; pNew->KillPrivate = KillDMC2280; pNew->GetDriverTextPar = NULL; pNew->blockage_ckInterval = 0.5; pNew->blockage_thresh = 0.5; pNew->blockage_ratio = 2.0; pNew->blockage_fail = 0; pNew->backlash_offset = 0.0; /* PARAMETERS: Fetch parameter values */ if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) { strncpy(pNew->long_name, pPtr, sizeof(pNew->long_name)); pNew->long_name[sizeof(pNew->long_name) - 1] = '\0'; } if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->fLower)); if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->fUpper)); if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%s",pNew->units); if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->speed)); pNew->maxSpeed = pNew->speed; if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->accel)); pNew->maxAccel = pNew->accel; if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->decel)); pNew->maxDecel = pNew->decel; if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%c",&(pNew->axisLabel)); if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) { KillDMC2280(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->stepsPerX)); if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL) pNew->motorHome=0; else sscanf(pPtr,"%d",&(pNew->motorHome)); if ((pPtr=getParam(pCon, interp, params,"nopowersave",_OPTIONAL)) == NULL) pNew->noPowerSave=_SAVEPOWER; else sscanf(pPtr,"%d",&(pNew->noPowerSave)); if ((pPtr=getParam(pCon, interp, params,"motoffdelay",_OPTIONAL)) == NULL) pNew->motOffDelay=0; else sscanf(pPtr,"%d",&(pNew->motOffDelay)); /* Debug: this motor driver logs exchanges */ if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL) pNew->debug=0; else { sscanf(pPtr,"%d",&(pNew->debug)); } /* SETTLE: this motor need time to settle */ if ((pPtr=getParam(pCon, interp, params,"settle",_OPTIONAL)) == NULL) pNew->settle=0; else { sscanf(pPtr,"%d",&(pNew->settle)); } /* BACKLASH: this motor need airpads */ if ((pPtr=getParam(pCon, interp, params,"backlash_offset",_OPTIONAL)) == NULL) pNew->backlash_offset=0.0; else { sscanf(pPtr,"%f",&(pNew->backlash_offset)); } /* AIRPADS: this motor need airpads */ if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL) pNew->has_airpads=0; else { sscanf(pPtr,"%d",&(pNew->has_airpads)); } /* ABSENC: If the parameter requires an abs enc add it to the else block */ if ((pPtr=getParam(pCon, interp, params,"absenc",_OPTIONAL)) == NULL) pNew->abs_endcoder=0; else { sscanf(pPtr,"%d",&(pNew->abs_endcoder)); if ((pPtr=getParam(pCon, interp, params,"absenchome",_REQUIRED)) == NULL) pNew->absEncHome=0; else sscanf(pPtr,"%d",&(pNew->absEncHome)); if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL) pNew->cntsPerX=1.0; else sscanf(pPtr,"%f",&(pNew->cntsPerX)); } /* Set speed */ snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed)); if (FAILURE == DMC2280Send(pNew, cmd)) return NULL; /* Set acceleration */ snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel)); if (FAILURE == DMC2280Send(pNew, cmd)) return NULL; /* Set deceleration */ snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel)); if (FAILURE == DMC2280Send(pNew, cmd)) return NULL; /* TODO Initialise current position and target to get a sensible initial list output */ return (MotorDriver *)pNew; } int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) { pMotor pm = (pMotor) pData; pDMC2280Driv self = (pDMC2280Driv) pm->pDriver; if (argc > 1) { if (strcasecmp("units", argv[1]) == 0) { if (argc > 2) { strncpy(self->units, argv[2], sizeof(self->units)); self->units[sizeof(self->units) - 1] = '\0'; } else { char line[132]; snprintf(line, 132, "%s.units = %s", self->name, self->units); SCWrite(pCon, line, eValue); } return 1; } if (strcasecmp("long_name", argv[1]) == 0) { if (argc > 2) { strncpy(self->long_name, argv[2], sizeof(self->long_name)); self->long_name[sizeof(self->long_name) - 1] = '\0'; } else { char line[132]; snprintf(line, 132, "%s.long_name = %s", self->name, self->long_name); SCWrite(pCon, line, eValue); } return 1; } if (strcasecmp("part", argv[1]) == 0) { if (argc > 2) { strncpy(self->part, argv[2], sizeof(self->part)); self->part[sizeof(self->part) - 1] = '\0'; } else { char line[132]; snprintf(line, 132, "%s.part = %s", self->name, self->part); SCWrite(pCon, line, eValue); } return 1; } if (strcasecmp("list", argv[1]) == 0) { } } return MotorAction(pCon, pSics, pData, argc, argv); }