/** \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 #include "anstoutil.h" #define UNITSLEN 256 #define TEXTPARLEN 1024 #define CMDLEN 1024 #define STATE_TRACE (100) /** \brief Used to ensure that the getDMCSetting function is called * with valid values. * \see getDMCSetting */ enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration}; enum commandtype {CMD_RUN=1, CMD_HALT=2}; typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv; enum eventtype { eStateEvent, eTimerEvent, eMessageEvent, eCommandEvent, eTimeoutEvent }; typedef struct EvtEvent_s EvtEvent, *pEvtEvent; typedef void (*StateFunc)(pDMC2280Driv self, pEvtEvent event); typedef struct EvtState_s { } EvtState; typedef struct EvtTimer_s { int timerValue; } EvtTimer; typedef struct EvtMessage_s { pAsyncTxn cmd; } EvtMessage; typedef struct EvtCommand_s { enum commandtype cmd_type; } EvtCommand; typedef struct EvtTimeout_s { } EvtTimeout; struct EvtEvent_s { enum eventtype event_type; union { EvtState sta; EvtTimer tmr; EvtMessage msg; EvtCommand cmd; EvtTimeout tmo; } event; }; #define VAR_REQ (1<<0) #define VAR_RSP (1<<1) #define VAR_RUN (1<<2) #define VAR_DST (1<<3) #define VAR_POS (1<<4) #define VAR_HLT (1<<5) #define VAR_ENC (1<<6) #define VAR_SWI (1<<7) #define VAR_HOM (1<<8) #define VAR_STP (1<<9) static pAsyncProtocol DMC2280_Protocol = NULL; /*----------------------------------------------------------------------- The motor driver structure. Please note that the first set of fields has be identical with the fields of AbstractModriv in ../modriv.h ------------------------------------------------------------------------*/ 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 */ pAsyncUnit asyncUnit; 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 encoderAxis; char lastCmd[CMDLEN]; char dmc2280Error[CMDLEN]; double fHome; /**< home position for axis, default=0 */ int motorHome; /**< motor home position in steps */ int noPowerSave; /**< Flag = 1 to leave motors on after a move */ double stepsPerX; /**< steps per physical unit */ int abs_encoder; /**< Flag = 1 if there is an abs enc */ int absEncHome; /**< Home position in counts for abs enc */ double cntsPerX; /**< absolute encoder counts per physical unit */ int motOffDelay; /**< msec to wait before switching motor off, default=0 */ int currFlags; int currSteps; int currCounts; double currPosition; /**< Position at last position check */ double lastPosition; /**< Position at last position check */ int lastSteps; int lastCounts; double origPosition; /**< Position at last position check */ int origSteps; int origCounts; double minRatio; double maxRatio; int thread0; /**< last read of _XQ0 */ unsigned short int stopCode; /**< last read of _SCx */ unsigned short int inputByte; /**< last read of _TIx */ bool ampError; /**< amplifier error */ bool runError; /**< motor error */ bool threadError; /**< thread error */ bool moving; /**< true if moving */ 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 protocol; /**< protocol version 0..3 */ float backlash_offset; /**< signed offset to drive from */ double fTarget; /**< target passed from SICS to timer callback */ double fPreseek; /**< preseek target when preseek is active */ int settle; /**< motor settling time in seconds */ struct timeval time_settle_done; /**< time when settling will be over */ int debug; int stepCount; /**< number of step operations for this move cycle */ float creep_offset; /**< last little bit to creep in units */ float creep_precision; /**< how far away we can stop creeping */ int creep_val; int preseek; /**< Flag = 1 if current move is a backlash preseek */ int run_flag; /**< Flag = 1 if RUN command has been issued and deferred */ /**< Flag = -1 if HALT command has been issued and deferred */ int driver_status; bool faultPending; /**< set when waiting for a failing motor to stop */ int timerValue; /**< save for debug printing */ StateFunc myState; /**< pointer to state action method */ StateFunc myPrevState; /**< save for debug printing */ int subState; /**< tracks substate within state method */ bool waitResponse; /**< true is a message sent and we wait for response */ pNWTimer state_timer; /**< motor state timer */ SConnection *trace; int posit_count; double* positions; int variables; #if defined(STATE_TRACE) && (STATE_TRACE > 0) int state_trace_idx; char state_trace_text[STATE_TRACE][CMDLEN]; struct timeval state_trace_time[STATE_TRACE]; #endif int bias_bits; /**< number of bits to mask */ int bias_bias; /**< bias to add to encoder value */ }; int DMC2280MotionControl = 1; /* defaults to enabled */ static bool trace_switches = false; #define AIR_POLL_TIMER 1000 #define MOTOR_POLL_FAST 100 #define MOTOR_POLL_SLOW 500 #define ON_SETTLE_TIMER 200 #define MAX_CREEP_STEPS 100 #define MAX_RESTARTS 0 /*------------------- error codes ----------------------------------*/ #define BADADR -1 // NOT SET: Unknown host/port? #define BADBSY -2 #define BADCMD -3 #define BADPAR -4 // 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 // Air Cushion Fault #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 STATEERROR -20 #define THREADZERO -21 /* Thread zero not running */ //#define AMPERROR -22 /* Amplifier OK not asserted */ #define RUNERROR -23 /* Error from RUN variable */ /*--------------------------------------------------------------------*/ #define STATUSMOVING 128 /* Motor is moving */ #define STATUSERRORLIMIT 64 /* Number of errors 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" /* State Machine Events */ static int state_snd_callback(pAsyncTxn pCmd); static int state_msg_callback(pAsyncTxn pCmd); static int state_tmr_callback(void* ctx, int mode); static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd); static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); static bool check_positions(pDMC2280Driv self) { char line[CMDLEN]; int missing = 0; int direction = 0; int i; if (self->posit_count < 2) { snprintf(line, ERRLEN, "Insufficient positions on motor '%s': %d", self->name, self->posit_count); SICSLogWrite(line,eError); return false; } for (i = 0; i < self->posit_count; ++i) if (self->positions[i] < -9999999) ++missing; if (missing) { snprintf(line, ERRLEN, "Missing positions on motor '%s': %d", self->name, missing); SICSLogWrite(line,eError); return false; } for (i = 1; i < self->posit_count; ++i) { if (i == 1) { if (self->positions[i] > self->positions[i - 1]) direction = 1; else if (self->positions[i] < self->positions[i - 1]) direction = -1; else direction = 0; } if (direction == 0) { snprintf(line, ERRLEN, "Position order on motor '%s' : %d", self->name, i); SICSLogWrite(line,eError); } else { switch (direction) { case -1: if (!(self->positions[i] < self->positions[i - 1])) { direction = 0; } break; case 1: if (!(self->positions[i] > self->positions[i - 1])) { direction = 0; } break; } } } if (direction != 0) return true; for (i = 0; i < self->posit_count; ++i) { snprintf(line, ERRLEN, "Position %2d motor '%s' : %f", i + 1, self->name, self->positions[i]); /* TODO log me */ } return false; } static long long unit2count(pDMC2280Driv self, double target) { double absolute; long long result; if (self->abs_encoder == 0) { char line[CMDLEN]; snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder", self->name); SICSLogWrite(line, eStatus); return -1; } /* distance of target from home in units */ absolute = (double) target - self->fHome; /* convert to encoder counts */ absolute *= (double) self->cntsPerX; /* add home position in counts */ absolute += (double) self->absEncHome; if (absolute > 0) result = (int) (absolute + 0.5); else if (absolute < 0) result = (int) (absolute - 0.5); else result = (int) absolute; if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "unit2count Rounding %f to %lld", absolute, result); SICSLogWrite(line, eStatus); } return result; } static double count2unit(pDMC2280Driv self, long long counts) { double fPos; if (self->abs_encoder == 0) { char line[CMDLEN]; snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder", self->name); SICSLogWrite(line, eStatus); return -1; } fPos = (counts - self->absEncHome) / self->cntsPerX + self->fHome; return fPos; } static double unit2posit(pDMC2280Driv self, double target) { int i, j; double fPos; if (!check_positions(self)) return 0; if (self->positions[0] < self->positions[1]) { for (i = 1; i < self->posit_count - 1; ++i) { if (target < self->positions[i]) { break; } } } else { for (i = 1; i < self->posit_count - 1; ++i) { if (target > self->positions[i]) { break; } } } --i; j = i + 1; fPos = (double) j + ((double)target - self->positions[i]) / ((double)self->positions[j] - self->positions[i]); return fPos; } static double posit2unit(pDMC2280Driv self, double target) { double result; int i = ((int) target) - 1; if (i < 0) { i = 0; } else if (i > self->posit_count - 2) { i = self->posit_count - 2; } result = (double)self->positions[i] + ((double)target - ((double)i + 1)) * ((double)self->positions[i + 1] - self->positions[i]); return result; } static long long posit2count(pDMC2280Driv self, double target) { return unit2count(self, posit2unit(self, target)); } static double count2posit(pDMC2280Driv self, long long counts) { return unit2posit(self, count2unit(self, counts)); } /** \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, double axisSpeed) { int speed; speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5); return speed; } /** \brief Convert axis acceleration in physical units 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, double 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, double axisDecel) { int decel; decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5); return decel; } /** \brief Convert motor position to physical units * using the current motor steps or encoder counts * * \param self (r) provides access to the motor's data structure * \return the motor position in physical units */ static double motPosit(pDMC2280Driv self) { double fPos; if (self->abs_encoder) fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->fHome; else fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->fHome; return fPos; } /** \brief Convert motor target in physical units to * motor absolute position in steps * * \param self (r) provides access to the motor's data structure * \param target in physical units, eg mm, degrees * \return the absolute position in motor steps */ static int motAbsol(pDMC2280Driv self, double target) { double absolute; int result; if (self->abs_encoder) { /* distance of target from home in units */ absolute = target - self->fHome; /* subtract current encoder position in units */ absolute -= (self->currCounts - self->absEncHome) / self->cntsPerX; /* convert to motor steps */ absolute *= self->stepsPerX; /* add current position in steps */ absolute += (double) self->currSteps; } else { absolute = (target - self->fHome) * self->stepsPerX + self->motorHome; } if (absolute > 0) result = (int) (absolute + 0.5); else if (absolute < 0) result = (int) (absolute - 0.5); else result = (int) absolute; if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "motAbsol Rounding %f to %d", absolute, result); SICSLogWrite(line, eStatus); } return result; } /** \brief Convert motor target in physical units to * motor absolute position in steps * * \param self provides access to the motor's data structure * \param target in physical units, eg mm, degrees * \return the absolute position in motor steps */ static int motCreep(pDMC2280Driv self, double target) { int target_steps = motAbsol(self, target); if (!self->abs_encoder || /* no absolute encoder */ self->preseek || /* backlash preseek active */ self->creep_offset == 0) /* not creeping anyway */ return target_steps; else { int offset = target_steps - self->currSteps; if (self->creep_val == 0) { /* initial creep step */ if (offset > 0) /* moving up */ self->creep_val = -1; else if (offset < 0) /* moving down */ self->creep_val = +1; } else { if (self->creep_val > 0) /* moving down */ ++self->creep_val; else --self->creep_val; if (abs(self->creep_val) > MAX_CREEP_STEPS && abs(self->creep_val) > 2.0 * fabs(self->stepsPerX / self->cntsPerX)) { if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s creep stopped, stepcount = %d", self->name, self->stepCount); SICSLogWrite(line, eStatus); } return target_steps; } } if (self->debug) { char text[CMDLEN]; snprintf(text, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d", self->currSteps, target_steps, offset, self->currSteps + offset); SICSLogWrite(text, eStatus); } if (self->creep_val > 0) /* moving down handle as positive */ offset = -offset; if (offset > fabs(self->stepsPerX * self->creep_precision)) { #if 1 /* if half offset is more than creep_offset warp to creep_offset */ if (offset > (int) (2.0 * fabs(self->stepsPerX * self->creep_offset))) offset = offset - fabs(self->stepsPerX * self->creep_offset); else { /* if closer than one count, single step else go half way */ if (offset <= fabs(self->stepsPerX / self->cntsPerX)) offset = 1; else offset = offset / 2; } #else if (offset - (int) fabs(self->stepsPerX * self->creep_offset) > (int) fabs(self->stepsPerX / self->cntsPerX)) offset = offset - fabs(self->stepsPerX * self->creep_offset); else { if (offset <= fabs(self->stepsPerX / self->cntsPerX)) offset = 1; else offset = offset / 2; } #endif self->preseek = 1; } if (self->creep_val > 0) /* moving down restore to negative */ offset = - offset; if (self->debug) { char text[CMDLEN]; snprintf(text, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d", self->currSteps, target_steps, offset, self->currSteps + offset); SICSLogWrite(text, eStatus); } target_steps = self->currSteps + offset; } return target_steps; } static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx) { int iRet = 1; pAsyncTxn myCmd = (pAsyncTxn) ctx; if (myCmd) { myCmd->txn_status = ATX_ACTIVE; iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len); /* TODO handle errors */ if (iRet < 0) { /* TODO: EOF */ /* iRet = AsyncUnitReconnect(myCmd->unit); if (iRet == 0) */ return 0; } } return 1; } static int DMC_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) { int iRet = 1; pAsyncTxn myCmd = (pAsyncTxn) ctx; switch (myCmd->txn_state) { case 0: /* first character */ if (rxchar == ':') { /* normal prompt */ myCmd->txn_state = 99; myCmd->txn_status = ATX_COMPLETE; } else if (rxchar == '?') { /* error prompt, send TC1 ahead of any queued commands */ iRet = AsyncUnitWrite(myCmd->unit, "TC1\r\n", 5); myCmd->txn_state = 1; } else { /* normal data */ myCmd->txn_state = 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->txn_state = 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->txn_state = 0; } else myCmd->txn_state = 1; break; } if (myCmd->txn_state == 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]; } } iRet = 0; } if (iRet == 0) { /* end of command */ return AQU_POP_CMD; } return iRet; } static int DMC_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) { if (event == AQU_TIMEOUT) { /* handle command timeout */ pTxn->txn_status = ATX_TIMEOUT; return AQU_POP_CMD; } return AQU_POP_CMD; } static int DMC_SendReq(pDMC2280Driv self, char* command) { self->waitResponse = true; return AsyncUnitSendTxn(self->asyncUnit, command, strlen(command), state_msg_callback, self, CMDLEN); } /** * \brief SendCallback is the callback for the general command. */ static int SendCallback(pAsyncTxn pCmd) { char* cmnd = pCmd->out_buf; char* resp = pCmd->inp_buf; pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; if (pCmd->txn_status == ATX_TIMEOUT) { if (self->debug) { SICSLogWrite(pCmd->out_buf, eStatus); SICSLogWrite("", eStatus); } strncpy(self->lastCmd, pCmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; } else { switch (resp[0]) { case ':': /* prompt */ case ' ': /* leading blank */ case '-': /* leading minus sign */ 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; } } return 0; } /** \brief Sends a DMC2280 command to the motor controller. * * This function does not expect or care about the response. * * If the command fails it writes the DMC2280 error message 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 DMC_Send(pDMC2280Driv self, char *command) { return AsyncUnitSendTxn(self->asyncUnit, command, strlen(command), state_snd_callback, self, CMDLEN); } /** * \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 DMC_SendReceive(pDMC2280Driv self, char *cmd, char* reply) { int status; int cmd_len = CMDLEN; status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len); 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; } static void DMC_SetTimer(pDMC2280Driv self, int msecs) { self->timerValue = msecs; NetWatchRegisterTimer(&self->state_timer, msecs, state_tmr_callback, self); } /** \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, int steps, int counts) { assert(self != NULL); self->lastSteps = steps; self->lastCounts = counts; gettimeofday(&(self->time_lastPos_set), NULL); } static void report_motion(pDMC2280Driv self) { SConnection *pDumCon; MotCallback sCall; if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); assert(self->pMot); pDumCon = SCCreateDummyConnection(pServ->pSics); MotorGetSoftPosition(self->pMot,pDumCon,&sCall.fVal); SCDeleteConnection(pDumCon); sCall.pName = self->pMot->name; InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); } /** \brief Reads absolute encoder. * * \param *pos is the absolute encoder reading on SUCCESS. * \return * SUCCESS * FAILURE */ static int readAbsEnc(pDMC2280Driv self, long *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "TP%c", self->encoderAxis ? self->encoderAxis : self->axisLabel); if (FAILURE == DMC_SendReceive(self, cmd, reply)) return FAILURE; long iCounts = atol(reply); if (self->bias_bits > 0 && self->bias_bias != 0) iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1); *pos = iCounts; 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 == DMC_SendReceive(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 == DMC_SendReceive(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 == DMC_Send(self, cmd)) return FAILURE; return SUCCESS; } snprintf(cmd, CMDLEN, "XQ #HOME,7"); if (FAILURE == DMC_Send(self, cmd)) return FAILURE; return SUCCESS; } /** \brief Check if the axis is still within all the limits * * The position is checked against upper and lower limits * * \param *pData provides access to a motor's data * \return * - 1 MOTOR OK, position is within the specified limits * - 0 MOTOR BAD, position is not within hard and soft limits */ static int checkPosition(pDMC2280Driv self) { float fTarget; float fPrecision; bool moving_forward = false; MotorGetPar(self->pMot, "precision", &fPrecision); if (self->preseek) fTarget = self->fPreseek; else fTarget = self->fTarget; if (fTarget > self->lastPosition) moving_forward = true; else moving_forward = false; if (moving_forward) { if (self->currPosition > self->fUpper + fPrecision) return 0; if (self->currPosition > fTarget + fPrecision) return 0; } else { if (self->currPosition < self->fLower - fPrecision) return 0; if (self->currPosition < fTarget - fPrecision) return 0; } return 1; } /** \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(pDMC2280Driv self) { int iRet = 1; double ratio_obs, ratio_exp, ratio_cmp; long int usec_TimeDiff; struct timeval now; bool bNotYet = false; /* set true if too soon */ assert(self != NULL); /* we can only test if there is an absolute encoder */ if (!self->abs_encoder) return 1; if (self->time_lastPos_set.tv_sec == 0) { /* first time - initialise the data */ set_lastMotion(self, self->currSteps, self->currCounts); bNotYet = true; } /* * calculate the time since the last check */ 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 too soon, say not yet */ if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval)) bNotYet = true; /* * calculate the distance since the last check * If too short, say not yet */ #if 0 if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ set_lastMotion(self, self->lastSteps, self->lastCounts); bNotYet = true; } #else /* TODO: make relative to lastPosition */ if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ set_lastMotion(self, self->lastSteps, self->lastCounts); bNotYet = true; } #endif /* calculate observed and expected steps per count ratios */ if (self->currCounts == self->lastCounts) /* prevent divide by zero */ ratio_obs = (self->currSteps - self->lastSteps); else { ratio_obs = (double) (self->currSteps - self->lastSteps); ratio_obs /= (double) (self->currCounts - self->lastCounts); } ratio_exp = (double) self->stepsPerX / (double) self->cntsPerX; ratio_cmp = ratio_obs / ratio_exp; if (self->minRatio == 0.0 || self->minRatio > ratio_cmp) self->minRatio = ratio_cmp; if (self->maxRatio == 0.0 || self->maxRatio < ratio_cmp) self->maxRatio = ratio_cmp; /* 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]; double cmp = ratio_cmp; if (fabs(cmp) > -1.0 && fabs(cmp) < 1.0) cmp = 1.0 / cmp; if (self->debug || bNotYet == false) { snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f", self->name, ratio_obs, ratio_exp, cmp); SICSLogWrite(msg, eError); snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f", self->currSteps, self->lastSteps, self->currCounts, self->lastCounts, self->stepsPerX, self->cntsPerX); SICSLogWrite(msg, eError); } if (bNotYet == false) { if (self->blockage_fail) iRet = 0; else set_lastMotion(self, self->currSteps, self->currCounts); } } else { if (self->debug) { char msg[132]; double cmp = ratio_cmp; if (fabs(cmp) > 0 && fabs(cmp) < 1) cmp = 1.0 / cmp; snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f", self->name, ratio_obs, ratio_exp, cmp); SICSLogWrite(msg, eError); snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f", self->currSteps, self->lastSteps, self->currCounts, self->lastCounts, self->stepsPerX, self->cntsPerX); SICSLogWrite(msg, eError); } if (bNotYet == false) { set_lastMotion(self, self->currSteps, self->currCounts); } } return iRet; } /** * \brief test if there is a variable of the form VVVx where VVV is the * variable name and x is the axis label. * * The variable name is contstructed by appending the axis label to the given * name and looking for the construct "VVVx=" at the start of a line in the * variable list. This will reject finds which are just the last part of a * variable like "ZVVV=" * * \param self motor data * \param vars string returned from LV command * \param VVV part of variable name * * \return true if found else false */ static bool has_var(pDMC2280Driv self, const char* vars, const char* name) { char* p; char tmp[20]; snprintf(tmp, 20, "%s=", name); p = strstr(vars, tmp); if (p == NULL) /* not found at all */ return false; if (p == vars) /* found at start of first line */ return true; if (p[-1] == '\n') /* found at start of another line */ return true; return false; /* found partial (ZVVVx=) only */ } static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) { char tmp[20]; snprintf(tmp, 20, "%s%c", name, self->axisLabel); return has_var(self, vars, tmp); } /** * \brief request standard state information for the motor and controller * * \param self access to the drive data */ static int cmdStatus(pDMC2280Driv self) { char cmd[CMDLEN]; char encoder = self->axisLabel; int io_byte = 0; if (self->encoderAxis && !(self->variables & VAR_ENC)) encoder = self->encoderAxis; if (self->axisLabel >= 'A' && self->axisLabel <= 'D') io_byte = 0; else io_byte = 1; /* Use POSx, ENCx, RUNx, SWIx if it has these variables */ snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,%s%c,_TI%d,_XQ0", (self->variables & VAR_POS) ? "POS" : "_TD", 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, (self->variables & VAR_STP) ? "STP" : "_SC", self->axisLabel, io_byte); return DMC_SendReq(self, cmd); } static int cmdVars(pDMC2280Driv self) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "LV"); return DMC_SendReq(self, cmd); } static int cmdOn(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->protocol == 1) { snprintf(cmd, CMDLEN, "FTUBE=1"); } else if (self->protocol == 2 || self->protocol == 3) { snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel); } else { snprintf(cmd, CMDLEN, "SH%c", self->axisLabel); } return DMC_SendReq(self, cmd); } static int cmdPosition(pDMC2280Driv self, int target) { char cmd[CMDLEN]; self->lastPosition = motPosit(self); if (self->protocol == 3) snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target); else snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target); return DMC_SendReq(self, cmd); } static int cmdBegin(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->protocol == 3) snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel); else snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); return DMC_SendReq(self, cmd); } static int cmdPoll(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->protocol == 2 || self->protocol == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); else if (self->protocol == 1) snprintf(cmd, CMDLEN, "MG APDONE"); return DMC_SendReq(self, cmd); } static void cmdHalt(pDMC2280Driv self) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); (void) DMC_Send(self, cmd); if (self->variables & VAR_HLT) { snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel); /* Note that this does not expect a reply or confirmation */ (void) DMC_Send(self, cmd); } } static int cmdOff(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->protocol == 1) { snprintf(cmd, CMDLEN, "FTUBE=0"); } else if (self->protocol == 2 || self->protocol == 3) { snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); } else { snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); } return DMC_SendReq(self, cmd); } static int rspStatus(pDMC2280Driv self, const char* text) { int iRet, iFlags; int iSteps, iCounts; int iIOByte, iStopCode, iXQ0, iBG; iRet = sscanf(text, "%d %d %d %d %d %d %d", &iSteps, &iCounts, &iFlags, &iBG, &iStopCode, &iIOByte, &iXQ0); if (iRet != 7) return 0; if ((trace_switches || self->debug) && self->currFlags != iFlags) { char line[CMDLEN]; char *sw; if ((self->currFlags & (8 + 4)) != (iFlags & (8 + 4))) { switch (iFlags & (8 + 4)) { case (8 + 4): sw = "none"; break; case (8 ): sw = "reverse"; break; case ( 4): sw = "forward"; break; case ( 0 ): sw = "both"; break; } snprintf(line, CMDLEN, "Motor %s limits: %s", self->name, sw); if (trace_switches) ServerWriteGlobal(line, eStatus); else SICSLogWrite(line, eStatus); } if ((self->currFlags & (32)) != (iFlags & (32))) { if (iFlags & (32)) { sw = "off"; } else { sw = "on"; } snprintf(line, CMDLEN, "Motor %s motor: %s", self->name, sw); if (trace_switches) ServerWriteGlobal(line, eStatus); else SICSLogWrite(line, eStatus); } if ((self->currFlags & (128)) != (iFlags & (128))) { if (iFlags & (128)) { sw = "moving"; } else { sw = "stopped"; } snprintf(line, CMDLEN, "Motor %s motor: %s", self->name, sw); if (trace_switches) ServerWriteGlobal(line, eStatus); else SICSLogWrite(line, eStatus); } } self->currFlags = iFlags; self->currSteps = iSteps; if (self->bias_bits > 0 && self->bias_bias != 0) iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1); self->currCounts = iCounts; self->currPosition = motPosit(self); self->thread0 = iXQ0; self->stopCode = iStopCode; self->inputByte = iIOByte; #ifdef AMPERROR if (self->variables & VAR_RUN) self->ampError = 0; else if (self->axisLabel >= 'A' && self->axisLabel <= 'D') self->ampError = !(1 & (iIOByte >> (self->axisLabel - 'A'))); else self->ampError = !(1 & (iIOByte >> (self->axisLabel - 'E'))); #endif self->runError = iBG < 0 ? iBG : 0; if (iBG < 0) snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER RUN ERROR: %d", iBG); self->threadError = self->thread0 < 0; self->moving = iBG > 0; if (self->protocol == 3 && !(self->variables & VAR_STP)) { if (self->moving) self->stopCode = 0; else self->stopCode = 1; } return 1; } static int rspVars(pDMC2280Driv self, const char* text) { self->variables = 0; if (has_var(self, text, "HOMERUN")) self->variables |= VAR_HOM; if (has_var_x(self, text, "REQ")) self->variables |= VAR_REQ; if (has_var_x(self, text, "RSP")) self->variables |= VAR_RSP; if (has_var_x(self, text, "RUN")) self->variables |= VAR_RUN; if (has_var_x(self, text, "DST")) self->variables |= VAR_DST; if (has_var_x(self, text, "POS")) self->variables |= VAR_POS; if (has_var_x(self, text, "HLT")) self->variables |= VAR_HLT; if (has_var_x(self, text, "ENC")) self->variables |= VAR_ENC; if (has_var_x(self, text, "SWI")) self->variables |= VAR_SWI; if (has_var_x(self, text, "STP")) self->variables |= VAR_STP; if ((self->variables & VAR_REQ) && (self->variables & VAR_RSP)) { self->protocol = 2; if ((self->variables & VAR_RUN) && (self->variables & VAR_DST) && (self->variables & VAR_POS)) { self->protocol = 3; } } return 1; } static int rspPoll(pDMC2280Driv self, const char* text) { int iReply = atoi(text); if (iReply < 0) snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER REQ ERROR: %d", iReply); return iReply; } static void state_trace_prn(pDMC2280Driv self) { #if defined(STATE_TRACE) && (STATE_TRACE > 0) int idx = self->state_trace_idx; SICSLogWrite("state_trace_prn trace listing start", eStatus); do { char* line; struct timeval *tp; tp = &self->state_trace_time[self->state_trace_idx]; line = &self->state_trace_text[self->state_trace_idx++][0]; if (self->state_trace_idx >= STATE_TRACE) self->state_trace_idx = 0; if (*line) SICSLogWriteTime(line, eStatus, tp); } while (idx != self->state_trace_idx); SICSLogWrite("state_trace_prn trace listing end", eStatus); #else SICSLogWrite("state_trace_prn not implemented", eStatus); #endif } /* State Functions */ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event); static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event); static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event); static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event); static void DMCState_Error(pDMC2280Driv self, pEvtEvent event); static char* state_name(StateFunc func) { if (func == DMCState_Unknown) return "DMCState_Unknown"; if (func == DMCState_Idle) return "DMCState_Idle"; if (func == DMCState_MotorStart) return "DMCState_MotorStart"; if (func == DMCState_MotorOn) return "DMCState_MotorOn"; if (func == DMCState_Moving) return "DMCState_Moving"; if (func == DMCState_MotorHalt) return "DMCState_MotorHalt"; if (func == DMCState_OffTimer) return "DMCState_OffTimer"; if (func == DMCState_MotorStop) return "DMCState_MotorStop"; if (func == DMCState_Error) return "DMCState_Error"; return ""; } static void str_n_cat(char* s1, int len, const char* s2) { int i = strlen(s1); const char* p = s2; while (i < len - 3 && *p) { if (*p == '\r') { s1[i++] = '\\'; s1[i++] = 'r'; ++p; } else if (*p == '\n') { s1[i++] = '\\'; s1[i++] = 'n'; ++p; } else s1[i++] = *p++; } s1[i] = '\0'; } static char* event_name(pEvtEvent event, char* text, int length) { switch (event->event_type) { case eStateEvent: snprintf(text, length, "eStateEvent"); return text; case eTimerEvent: snprintf(text, length, "eTimerEvent (%d mSec)", event->event.tmr.timerValue); return text; case eMessageEvent: snprintf(text, length, "eMessageEvent:"); str_n_cat(text, length, event->event.msg.cmd->out_buf); str_n_cat(text, length, "|"); str_n_cat(text, length, event->event.msg.cmd->inp_buf); return text; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: snprintf(text, length, "eCommandEvent:RUN"); return text; case CMD_HALT: snprintf(text, length, "eCommandEvent:HALT"); return text; } snprintf(text, length, "eCommandEvent:unknown"); return text; case eTimeoutEvent: snprintf(text, length, "eTimeoutEvent"); return text; default: snprintf(text, length, ""); return text; } } static void report_event(pDMC2280Driv self, pEvtEvent event) { #if defined(STATE_TRACE) && (STATE_TRACE > 0) if (true) { char* line; gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL); line = &self->state_trace_text[self->state_trace_idx++][0]; if (self->state_trace_idx >= STATE_TRACE) self->state_trace_idx = 0; #else if (self->debug || self->trace) { char line[CMDLEN]; #endif char text[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), event=%s", self->name, state_name(self->myState), self->subState, event_name(event, text, CMDLEN)); if (self->debug) SICSLogWrite(line, eStatus); if (self->trace) SCWrite(self->trace, line, eStatus); } } static void change_state(pDMC2280Driv self, StateFunc func) { #if defined(STATE_TRACE) && (STATE_TRACE > 0) if (true) { char* line; gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL); line = &self->state_trace_text[self->state_trace_idx++][0]; if (self->state_trace_idx >= STATE_TRACE) self->state_trace_idx = 0; #else if (self->debug || self->trace) { char line[CMDLEN]; #endif snprintf(line, CMDLEN, "Motor=%s, OldState=%s(%d), NewState=%s", self->name, state_name(self->myState), self->subState, state_name(func)); if (self->debug) SICSLogWrite(line, eStatus); if (self->trace) SCWrite(self->trace, line, eStatus); } self->myPrevState = self->myState; self->myState = func; self->subState = 0; EvtEvent evt; evt.event_type = eStateEvent; self->myState(self, &evt); } static void unhandled_event(pDMC2280Driv self, pEvtEvent event) { #if defined(STATE_TRACE) && (STATE_TRACE > 0) char* line; gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL); line = &self->state_trace_text[self->state_trace_idx++][0]; if (self->state_trace_idx >= STATE_TRACE) self->state_trace_idx = 0; #else char line[CMDLEN]; #endif char text[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), unhandled event=%s", self->name, state_name(self->myState), self->subState, event_name(event, text, CMDLEN)); SICSLogWrite(line, eError); } static void handle_event(pDMC2280Driv self, pEvtEvent event) { StateFunc oldState; #if defined(STATE_TRACE) && (STATE_TRACE > 0) #else if (self->debug || self->trace) #endif report_event(self, event); oldState = self->myState; self->myState(self, event); } static int state_snd_callback(pAsyncTxn pCmd) { pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; SendCallback(pCmd); #if defined(STATE_TRACE) && (STATE_TRACE > 0) char* line; char text[CMDLEN]; gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL); line = &self->state_trace_text[self->state_trace_idx++][0]; if (self->state_trace_idx >= STATE_TRACE) self->state_trace_idx = 0; #else char line[CMDLEN]; char text[CMDLEN]; #endif text[0] = '\0'; str_n_cat(text, CMDLEN, pCmd->out_buf); str_n_cat(text, CMDLEN, "|"); str_n_cat(text, CMDLEN, pCmd->inp_buf); snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), send complete=%s", self->name, state_name(self->myState), self->subState, text); if (self->debug) SICSLogWrite(line, eStatus); if (self->trace) SCWrite(self->trace, line, eStatus); return 0; } static int state_msg_callback(pAsyncTxn pCmd) { pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; EvtEvent event; SendCallback(pCmd); if (pCmd->txn_status == ATX_TIMEOUT) { event.event_type = eTimeoutEvent; event.event.msg.cmd = pCmd; } else { event.event_type = eMessageEvent; event.event.msg.cmd = pCmd; } self->waitResponse = false; handle_event(self, &event); return 0; } static int state_tmr_callback(void* ctx, int mode) { pDMC2280Driv self = (pDMC2280Driv) ctx; EvtEvent event; self->state_timer = 0; event.event_type = eTimerEvent; event.event.tmr.timerValue = self->timerValue; handle_event(self, &event); return 0; } static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd) { EvtEvent event; event.event_type = eCommandEvent; event.event.cmd.cmd_type = cmd; handle_event(self, &event); return 0; } static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; int value; switch (event->event_type) { case eStateEvent: self->run_flag = 0; self->driver_status = HWBusy; self->errorCode = 0; /* handle pending message event */ if (self->waitResponse) { self->subState = 0; /* FIXME: FIXME_DOUG */ return; } /* Set speed */ value = motSpeed(self, self->speed); snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value); if (FAILURE == DMC_Send(self, cmd)) { break; } /* Set acceleration */ value = motAccel(self, self->accel); snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value); if (FAILURE == DMC_Send(self, cmd)) { break; } /* Set deceleration */ value = motDecel(self, self->decel); snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value); if (FAILURE == DMC_Send(self, cmd)) { break; } cmdVars(self); self->subState = 1; return; case eTimerEvent: if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); if (self->pMot) { self->pMot->fTarget = self->pMot->fPosition = motPosit(self); change_state(self, DMCState_Idle); return; } DMC_SetTimer(self, MOTOR_POLL_SLOW); return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 0) { /* waitResponse */ change_state(self, DMCState_Unknown); return; } if (self->subState == 1) { /* Vars */ rspVars(self, pCmd->inp_buf); cmdStatus(self); self->subState = 2; return; } if (self->subState == 2) { /* Status Request */ int iRet; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); DMC_SetTimer(self, 0); self->subState = 0; return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: self->driver_status = HWBusy; self->run_flag = 1; return; case CMD_HALT: /* handle halt command */ self->run_flag = -1; return; } break; case eTimeoutEvent: /* handle timeout */ change_state(self, DMCState_Unknown); return; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; if (self->driver_status == HWBusy) self->driver_status = HWIdle; if (self->variables & VAR_RUN) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel); (void) DMC_Send(self, cmd); } DMC_SetTimer(self, 0); self->subState = 0; return; case eTimerEvent: cmdStatus(self); self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* Status Response */ int iRet; long lDelta; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; /* action deferred RUN command */ if (self->run_flag == 1) { self->run_flag = 0; change_state(self, DMCState_MotorStart); return; } /* if the motor moved, update any observers */ if (self->abs_encoder) lDelta = fabs(self->currCounts - self->lastCounts); else lDelta = fabs(self->currSteps - self->lastSteps); if (lDelta > 10) { set_lastMotion(self, self->currSteps, self->currCounts); report_motion(self); } if (trace_switches || self->debug) DMC_SetTimer(self, MOTOR_POLL_FAST); else DMC_SetTimer(self, MOTOR_POLL_SLOW); self->subState = 0; return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: self->driver_status = HWBusy; if (self->waitResponse == false) { self->run_flag = 0; change_state(self, DMCState_MotorStart); } else { self->run_flag = 1; } return; case CMD_HALT: /* we are already halted so just reset run_flag*/ self->run_flag = 0; return; } break; case eTimeoutEvent: /* handle message timeout */ strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; DMC_SetTimer(self, MOTOR_POLL_SLOW); self->subState = 0; return; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: self->run_flag = 0; if (self->state_timer) { NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; } /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { state_cmd_execute(self, CMD_HALT); if (DMC2280MotionControl == 0) self->errorCode = MOTIONCONTROLOFF; else self->errorCode = MOTIONCONTROLUNK; self->driver_status = HWFault; } else if (self->threadError) { self->errorCode = THREADZERO; self->driver_status = HWFault; } else if (self->runError) { self->errorCode = RUNERROR; self->driver_status = HWFault; } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; self->driver_status = HWFault; } #endif else { int iFlags; int fwd_limit_active, rvrs_limit_active, errorlimit; /* Handle limit switches from latest status response */ iFlags = self->currFlags; fwd_limit_active = !(iFlags & STATUSFWDLIMIT); rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT); errorlimit = (iFlags & STATUSERRORLIMIT); if (fwd_limit_active && rvrs_limit_active) { self->errorCode = IMPOSSIBLE_LIM_SW; self->driver_status = HWFault; } else if (errorlimit) { self->errorCode = ERRORLIM; self->driver_status = HWFault; } } if (self->driver_status == HWFault) { change_state(self, DMCState_Idle); return; } cmdOn(self); self->subState = 1; return; case eTimerEvent: cmdPoll(self); self->subState = 2; return; case eMessageEvent: if (self->run_flag > 0) { self->run_flag = 0; change_state(self, DMCState_MotorStart); return; } else if (self->run_flag < 0) { self->run_flag = 0; change_state(self, DMCState_MotorHalt); return; } do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { if (self->protocol == 0) { /* cmdOn */ change_state(self, DMCState_MotorOn); return; } else { DMC_SetTimer(self, AIR_POLL_TIMER); self->subState = 0; return; } } else if (self->subState == 2) { /* Poll */ int iRet; iRet = rspPoll(self, pCmd->inp_buf); if (iRet > 0) { /* state has changed to ON */ change_state(self, DMCState_MotorOn); return; } else if (iRet < 0) { /* Handle error return */ self->errorCode = RUNERROR; self->driver_status = HWFault; change_state(self, DMCState_MotorStop); return; } /* TODO handle airpad timeout */ DMC_SetTimer(self, AIR_POLL_TIMER); self->subState = 0; return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: self->driver_status = HWBusy; if (self->waitResponse == false) { self->run_flag = 0; change_state(self, DMCState_MotorStart); } else { self->run_flag = 1; } return; case CMD_HALT: /* handle halt command */ if (self->waitResponse == false) { self->run_flag = 0; change_state(self, DMCState_MotorStop); } else { self->run_flag = -1; } return; } break; case eTimeoutEvent: /* handle message timeout */ strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; change_state(self, DMCState_MotorStop); return; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: DMC_SetTimer(self, ON_SETTLE_TIMER); return; case eTimerEvent: cmdStatus(self); self->subState = 1; return; case eMessageEvent: if (self->run_flag > 0) { self->run_flag = 0; change_state(self, DMCState_MotorStart); return; } else if (self->run_flag < 0) { self->run_flag = 0; change_state(self, DMCState_MotorHalt); return; } do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* Status Response */ int iRet, absolute; double target; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; if (self->threadError) { self->errorCode = THREADZERO; self->driver_status = HWFault; change_state(self, DMCState_MotorStop); return; } else if (self->runError) { self->errorCode = RUNERROR; self->driver_status = HWFault; change_state(self, DMCState_MotorStop); return; } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; self->driver_status = HWFault; change_state(self, DMCState_MotorStop); return; } #endif set_lastMotion(self, self->currSteps, self->currCounts); /* compute position for PA command */ target = self->fTarget; self->preseek = 0; self->fPreseek = self->fTarget; if (self->backlash_offset) { if (self->backlash_offset > 0) { /* * We want to be moving from high to low, * if the target is higher than current * we must pre-seek to the higher side */ if (target > self->currPosition) { self->preseek = 1; target += self->backlash_offset; if (target > self->fUpper) target = self->fUpper; self->fPreseek = target; } } else if (self->backlash_offset < 0) { /* * We want to be moving from low to high, * if the target is lower than current * we must pre-seek to the lower side */ if (target < self->currPosition) { self->preseek = 1; target += self->backlash_offset; if (target < self->fLower) target = self->fLower; self->fPreseek = target; } } } if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n", self->name, self->currPosition, self->fPreseek, self->fTarget); SICSLogWrite(line, eStatus); } self->creep_val = 0; absolute = motCreep(self, target); cmdPosition(self, absolute); self->subState = 2; /* save pre-motion values for logging */ self->origPosition = self->currPosition; self->origCounts = self->currCounts; self->origSteps = self->currSteps; self->minRatio = 0.0; self->maxRatio = 0.0; return; } else if (self->subState == 2) { /* PA */ change_state(self, DMCState_Moving); return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: self->driver_status = HWBusy; if (self->waitResponse == false) { self->run_flag = 0; change_state(self, DMCState_MotorStart); } else { self->run_flag = 1; } return; case CMD_HALT: /* handle halt command */ if (self->waitResponse == false) { self->run_flag = 0; change_state(self, DMCState_MotorHalt); } else self->run_flag = -1; return; } break; case eTimeoutEvent: /* handle message timeout */ strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; change_state(self, DMCState_MotorHalt); return; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: cmdBegin(self); self->stepCount = 1; self->subState = 1; return; case eTimerEvent: /* Note that the substate has already been set */ cmdStatus(self); return; case eMessageEvent: if (self->run_flag != 0) { change_state(self, DMCState_MotorHalt); return; } do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* BG */ /* Check if BG worked (reply != '?') */ if (pCmd->inp_buf[0] == '?') { /* TODO: what happens when BGx fails? */ self->errorCode = BADCMD; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } self->subState = 2; DMC_SetTimer(self, MOTOR_POLL_FAST); return; } else if (self->subState == 3) { /* PA */ cmdBegin(self); self->stepCount++; self->subState = 1; return; } else if (self->subState == 2 || self->subState == 4) { /* Status */ int iRet, iFlags; bool fwd_limit_active, rvrs_limit_active, errorlimit; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; iFlags = self->currFlags; fwd_limit_active = !(iFlags & STATUSFWDLIMIT); rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT); errorlimit = (iFlags & STATUSERRORLIMIT); if (self->threadError) { self->errorCode = THREADZERO; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } else if (self->runError) { self->errorCode = RUNERROR; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; self->driver_status = HWFault; change_state(self, DMCState_MotorHalt); return; } #endif if (self->stopCode != 0 && self->stopCode != 1) { if (self->stopCode == 2) { self->errorCode = FWDLIM; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } else if (self->stopCode == 3) { self->errorCode = RVRSLIM; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } else if (self->stopCode == 4) { if (fwd_limit_active && rvrs_limit_active) { self->errorCode = IMPOSSIBLE_LIM_SW; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } else if (fwd_limit_active) { self->errorCode = FWDLIM; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } else if (rvrs_limit_active) { self->errorCode = RVRSLIM; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } } self->errorCode = RUNERROR; snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d", self->stopCode); self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } if (self->moving) { /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { if (DMC2280MotionControl == 0) self->errorCode = MOTIONCONTROLOFF; else self->errorCode = MOTIONCONTROLUNK; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } if (self->abs_encoder) { if (checkMotion(self) == 0) { /* handle blocked */ self->errorCode = BLOCKED; self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } if (checkPosition(self) == 0) { /* handle runaway */ self->errorCode = POSFAULT; /* recoverable fault */ self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); return; } } self->subState = 2; DMC_SetTimer(self, MOTOR_POLL_FAST); return; } /* Motor has stopped */ /* Handle limit switches */ if (fwd_limit_active && rvrs_limit_active) { self->errorCode = IMPOSSIBLE_LIM_SW; self->driver_status = HWFault; } else if (errorlimit) { self->errorCode = ERRORLIM; self->driver_status = HWFault; } if (self->driver_status == HWFault) { change_state(self, DMCState_OffTimer); return; } /* * If this was a pre-seek then compute the next iteration */ if (self->preseek) { double target; int absolute; self->preseek = 0; target = self->fTarget; self->fPreseek = target; /* if we are not creeping it is backlash correction */ if (self->creep_val == 0) { float precision; /* take precision into account */ MotorGetPar(self->pMot, "precision", &precision); if (self->backlash_offset > 0) { if (target + self->backlash_offset > self->currPosition + precision) { self->preseek = 1; target += self->backlash_offset + precision; if (target > self->fUpper) target = self->fUpper; self->fPreseek = target; } } else if (self->backlash_offset < 0) { if (target + self->backlash_offset < self->currPosition - precision) { self->preseek = 1; target += self->backlash_offset - precision; if (target < self->fLower) target = self->fLower; self->fPreseek = target; } } if (self->preseek && self->stepCount > 10) { /* limit the maximum number of tries */ if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d", self->name, self->stepCount); SICSLogWrite(line, eStatus); } self->preseek = 0; absolute = motCreep(self, target); } else if (self->preseek) { absolute = motCreep(self, target); } else { /* change of direction, reset motion check */ if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s changed direction", self->name); SICSLogWrite(line, eStatus); } set_lastMotion(self, self->currSteps, self->currCounts); absolute = motCreep(self, target); } } else absolute = motCreep(self, target); if (self->debug && self->preseek) { char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n", self->name, self->currPosition, self->fPreseek, self->fTarget); SICSLogWrite(line, eStatus); } /* * If we are still iterating, continue */ if (self->preseek) { if (absolute == self->currSteps) { if (self->debug) { char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s motion stopped, absolute = %d", self->name, absolute); SICSLogWrite(line, eStatus); } self->preseek = 0; } else { if (self->settle && self->subState == 2 && abs(absolute - self->currSteps) < 100) { self->subState = 4; DMC_SetTimer(self, self->settle); return; } cmdPosition(self, absolute); self->subState = 3; return; } } /* * We are no longer iterating, so fall through */ } /* handle settle time */ if (self->settle && self->time_settle_done.tv_sec == 0) { gettimeofday(&self->time_settle_done, NULL); self->time_settle_done.tv_sec += self->settle; self->subState = 2; DMC_SetTimer(self, self->settle); return; } /* We get here when the motor stops normally */ if (true /*self->debug*/) { double units = self->currPosition - self->origPosition; long int steps = self->currSteps - self->origSteps; long int counts = self->currCounts - self->origCounts; char line[CMDLEN]; snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f," " steps=%ld, counts=%ld, stepsPerX=%.6f," " minRatio=%.6f, maxRatio=%.6f", self->name, units, steps, counts, (double) steps / units, self->minRatio, self->maxRatio); SICSLogWrite(line, eStatus); } change_state(self, DMCState_OffTimer); return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* TODO: FIXME RUN command while running */ if (self->driver_status == HWIdle) self->driver_status = HWBusy; self->run_flag = 1; if (self->waitResponse == false) { change_state(self, DMCState_MotorHalt); } return; case CMD_HALT: /* handle halt command, send message */ self->run_flag = -1; if (self->waitResponse == false) { change_state(self, DMCState_MotorHalt); } return; } break; case eTimeoutEvent: strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; change_state(self, DMCState_MotorHalt); return; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; cmdHalt(self); /* Note fall through */ case eTimerEvent: cmdStatus(self); self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* Status Response */ int iRet; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; if (self->moving) { DMC_SetTimer(self, MOTOR_POLL_FAST); return; } if (self->faultPending) { self->run_flag = 0; } if (self->run_flag == 1) change_state(self, DMCState_MotorStart); else change_state(self, DMCState_MotorStop); return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* RUN command deferred*/ if (self->driver_status == HWIdle) self->driver_status = HWBusy; self->run_flag = 1; return; case CMD_HALT: /* HALT command ignored */ self->run_flag = 0; return; } break; case eTimeoutEvent: /* handle message timeout */ strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; change_state(self, DMCState_MotorHalt); return; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: if (self->run_flag == 0) { if (self->driver_status == HWBusy) self->driver_status = HWIdle; if (self->motOffDelay) { DMC_SetTimer(self, self->motOffDelay); return; } } DMC_SetTimer(self, 0); return; case eTimerEvent: if (self->run_flag <= 0) change_state(self, DMCState_MotorStop); else change_state(self, DMCState_MotorOn); return; case eMessageEvent: /* no need to handle message event */ break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* handle run command, convert to motor on timer expired */ if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; if (self->driver_status == HWIdle) self->driver_status = HWBusy; change_state(self, DMCState_MotorOn); return; case CMD_HALT: /* handle halt command, convert to motor off timer expired */ if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; change_state(self, DMCState_MotorStop); return; } return; case eTimeoutEvent: /* no need to handle message timeout */ break; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: self->run_flag = 0; cmdOff(self); return; case eTimerEvent: cmdPoll(self); self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 0) { /* Off command */ if (self->protocol == 0) { if (self->faultPending) { self->faultPending = false; self->driver_status = HWFault; } if (self->driver_status != HWFault && self->run_flag == 1) { self->run_flag = 0; change_state(self, DMCState_MotorStart); return; } change_state(self, DMCState_Idle); return; } } else if (self->subState == 1) { /* Poll */ int iRet; iRet = rspPoll(self, pCmd->inp_buf); if (iRet == 0) { /* state has changed to OFF */ if (self->faultPending) { self->faultPending = false; self->driver_status = HWFault; } if (self->driver_status != HWFault && self->run_flag == 1) { self->run_flag = 0; change_state(self, DMCState_MotorStart); return; } change_state(self, DMCState_Idle); return; } else if (iRet < 0) { /* Handle error return */ self->errorCode = RUNERROR; self->driver_status = HWFault; change_state(self, DMCState_Idle); return; } /* TODO handle airpad timeout */ } DMC_SetTimer(self, AIR_POLL_TIMER); } while (0); return; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* handle run command */ if (self->faultPending) return; if (self->driver_status == HWIdle) self->driver_status = HWBusy; self->run_flag = 1; return; case CMD_HALT: /* ignore it as we are stopped */ self->run_flag = 0; return; } break; case eTimeoutEvent: /* handle message timeout */ strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; change_state(self, DMCState_MotorStop); return; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_Error(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: state_trace_prn(self); return; case eTimerEvent: case eMessageEvent: return; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* TODO: handle run command */ break; case CMD_HALT: /* TODO: handle halt command */ break; } break; case eTimeoutEvent: /* TODO handle message timeout */ break; } unhandled_event(self, event); self->errorCode = STATEERROR; return; } /** \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; self = (pDMC2280Driv)pData; assert(self != NULL); *fPos = motPosit(self); 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->settle) self->time_settle_done.tv_sec = 0; do { float currPos; DMC2280GetPos(pData, &currPos); self->currPosition = currPos; self->lastPosition = currPos; self->time_lastPos_set.tv_sec = 0; } while (0); /* Reject Infinity and Not a Number (NaN) */ switch (fpclassify(fValue)) { case FP_ZERO: case FP_NORMAL: break; default: self->errorCode = BADPAR; return HWFault; } /* Only do it if it is within our hard limits */ if (fValue >= self->fLower && fValue <= self->fUpper) { self->fTarget = fValue; state_cmd_execute(self, CMD_RUN); return 1; } /* If we didn't do it then it must have been bad */ self->errorCode = BADPAR; return HWFault; } /** \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; self = (pDMC2280Driv)pData; assert(self != NULL); return self->driver_status; } /** \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){ 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, are they plugged in?", (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; case STATEERROR: strncpy(error, "ERROR IN DMC2280 FINITE STATE MACHINE", (size_t)errLen); break; case THREADZERO: strncpy(error, "THREAD ZERO NOT RUNNING ON CONTROLLER", (size_t)errLen); break; #ifdef AMPERROR case AMPERROR: strncpy(error, "DRIVE AMPLIFIER ERROR", (size_t)errLen); break; #endif case RUNERROR: strncpy(error, self->dmc2280Error, (size_t)errLen); break; default: /* 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: case THREADZERO: #ifdef AMPERROR case AMPERROR: #endif case RUNERROR: return MOTFAIL; case POSFAULT: self->faultPending = false; return MOTREDO; case STATEERROR: /* recover state error */ if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; change_state(self, DMCState_Unknown); return MOTFAIL; 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; self = (pDMC2280Driv)pData; assert(self != NULL); state_trace_prn(self); state_cmd_execute(self, CMD_HALT); return 1; } /** \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->fHome; return 1; } if(strcasecmp(name, "stepsPerX") == 0) { *fValue = self->stepsPerX; 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 || strcasecmp(name,"protocol") == 0) { *fValue = self->protocol; 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(strcasecmp(name,"bias_bias") == 0) { *fValue = self->bias_bias; return 1; } if(strcasecmp(name,"bias_bits") == 0) { *fValue = self->bias_bits; return 1; } if (self->posit_count > 0) { if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) { int index; *fValue = -1.0; index = strtol(&name[6], NULL, 10); // if (index < 1 || index > self->posit_count) // return 0; *fValue = posit2count(self, index); return 1; } } if (strcasecmp(name,"absencoder") == 0) { if (self->abs_encoder) *fValue = 1.0; else *fValue = 0.0; return 1; } if (self->abs_encoder != 0) { if (strcasecmp(name,"absenc") == 0) { long lValue; if (readAbsEnc(self, &lValue) == SUCCESS) { *fValue = (double) lValue; return 1; } else { *fValue = -1.0; return 0; } } if (strcasecmp(name,"absenchome") == 0) { *fValue = self->absEncHome; return 1; } if (strcasecmp(name,"cntsperx") == 0) { *fValue = self->cntsPerX; return 1; } if(strcasecmp(name,"creep_offset") == 0) { *fValue = self->creep_offset; return 1; } if(strcasecmp(name,"creep_precision") == 0) { *fValue = self->creep_precision; 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]; self = (pDMC2280Driv)pData; if(strcasecmp(name,SETPOS) == 0) { float oldZero, newZero; if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); assert(self->pMot); 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 || strcasecmp(name,"protocol") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->protocol = 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; } } if (self->abs_encoder) { /* If we DO have an absolute encoder */ /* Set creep offset */ if (strcasecmp(name,"creep_offset") == 0) { if(!SCMatchRights(pCon,usMugger)) /* managers only */ return 1; else { self->creep_offset = fabs(newValue); return 1; } } /* Set creep_precision */ if (strcasecmp(name,"creep_precision") == 0) { if(!SCMatchRights(pCon,usMugger)) /* managers only */ return 1; else { self->creep_precision = fabs(newValue); return 1; } } } else { /* If we do NOT have an absolute encoder */ /* Invoke Home Run routine in controller */ if(strcasecmp(name,"homerun") == 0) { if(!SCMatchRights(pCon,usMugger)) /* managers only */ 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 == DMC_Send(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 == DMC_Send(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 == DMC_Send(self, cmd)) return 0; /* FIXME should signal a HWFault */ return 1; } if (strcasecmp("hardlowerlim", name) == 0) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) return 0; self->fLower = newValue; return 1; } if (strcasecmp("hardupperlim", name) == 0) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) return 0; self->fUpper = newValue; return 1; } if (strcasecmp("stepsPerX", name) == 0) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) return 0; self->stepsPerX = newValue; return 1; } if (self->abs_encoder && strcasecmp("cntsPerX", name) == 0) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) return 0; self->cntsPerX = newValue; return 1; } if (self->abs_encoder && strcasecmp("bias_bias", name) == 0) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) return 0; self->bias_bias = newValue; return 1; } if (self->abs_encoder && strcasecmp("bias_bits", name) == 0) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) return 0; self->bias_bits = newValue; return 1; } /* XXX Maybe move this to a configuration parameter.*/ /* Set home, managers only. Users should set softposition */ if(strcasecmp(name, HOME) == 0) { /* Debug Managers only */ 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->fHome = newValue; return 1; } } if (self->posit_count > 0) { int index; if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) { index = strtol(&name[6], NULL, 10); if (index < 1 || index > self->posit_count) return 0; self->positions[index - 1] = count2unit(self, newValue); 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 DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){ char buffer[BUFFLEN]; snprintf(buffer, BUFFLEN, "%s.part = %s\n", name, self->part); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, self->long_name); 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; } /** \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 *pData, char *name, SConnection *pCon){ pDMC2280Driv self = (pDMC2280Driv) pData; char buffer[BUFFLEN]; snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->fHome); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, self->speed); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, self->maxSpeed); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, self->accel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, self->maxAccel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, self->decel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, self->maxDecel); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, self->motOffDelay); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, self->debug); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, self->settle); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, self->blockage_ratio); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, self->blockage_fail); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, self->backlash_offset); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Protocol = %d\n", name, self->protocol); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.absEncoder = %d\n", name, self->abs_encoder); SCWrite(pCon, buffer, eStatus); if (self->abs_encoder) { snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision); SCWrite(pCon, buffer, eStatus); } if (self->posit_count > 0) { int i; snprintf(buffer, BUFFLEN, "%s.posit_count = %d\n", name, self->posit_count); SCWrite(pCon, buffer, eStatus); for (i = 0; i < self->posit_count; ++i) { snprintf(buffer, BUFFLEN, "%s.posit_%d = %lld\n", name, i + 1, posit2count(self, i + 1)); SCWrite(pCon, buffer, eStatus); } } snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX); SCWrite(pCon, buffer, eStatus); return; } static void DMC_Notify(void* context, int event) { pDMC2280Driv self = (pDMC2280Driv) context; char line[132]; switch (event) { case AQU_DISCONNECT: snprintf(line, 132, "Disconnect on Motor '%s'", self->name); SICSLogWrite(line, eStatus); /* TODO: disconnect */ break; case AQU_RECONNECT: snprintf(line, 132, "Reconnect on Motor '%s'", self->name); SICSLogWrite(line, eStatus); /* TODO: reconnect */ /* Reset the state machine */ if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; change_state(self, DMCState_Unknown); break; } 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->asyncUnit) { AsyncUnitDestroy(self->asyncUnit); self->asyncUnit = NULL; } if (self->positions) { free(self->positions); self->positions = 0; } self->posit_count = 0; /* 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]; 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 AsyncQueue from the list of named parameters */ if ((pPtr=getParam(pCon, interp, params, "multichan", _OPTIONAL)) != NULL || (pPtr=getParam(pCon, interp, params, "asyncqueue", _OPTIONAL)) != NULL || (pPtr=getParam(pCon, interp, params, "asyncunit", _OPTIONAL)) != NULL) { if (!AsyncUnitCreate(pPtr, &pNew->asyncUnit)) { snprintf(pError, ERRLEN, "Cannot find AsyncQueue '%s' when creating DMC2280 motor '%s'", pPtr, motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew); free(pNew); return NULL; } AsyncUnitSetNotify(pNew->asyncUnit, 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); free(pNew); return NULL; } /* AsyncUnit */ if (!AsyncUnitCreateHost(host, pPtr, &pNew->asyncUnit)) { snprintf(pError, ERRLEN, "Cannot create AsyncUnit '%s:%s' for DMC2280 motor '%s'", host, pPtr, motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew); free(pNew); return NULL; } AsyncUnitSetNotify(pNew->asyncUnit, pNew, DMC_Notify); } else { snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor); SCWrite(pCon,pError,eError); KillDMC2280(pNew); free(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); free(pNew); return NULL; } pNew->pMot = NULL; strcpy(pNew->name, motor); pNew->fHome = 0.0; pNew->fLower = 0.0; pNew->fUpper = 0.0; 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; /* minimum distance in units */ pNew->blockage_ratio = 2.0; /* maximum ratio observed/expected */ pNew->blockage_fail = 1; /* fail the motor */ pNew->backlash_offset = 0.0; pNew->myState = NULL; /* PARAMETERS: Fetch parameter values */ /* Debug: this motor driver logs exchanges */ if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL) pNew->debug=0; else { sscanf(pPtr,"%d",&(pNew->debug)); } 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); free(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->fLower)); if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->fUpper)); if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); return NULL; } sscanf(pPtr,"%s",pNew->units); if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->speed)); pNew->maxSpeed = pNew->speed; if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->accel)); pNew->maxAccel = pNew->accel; if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->decel)); pNew->maxDecel = pNew->decel; if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); return NULL; } sscanf(pPtr,"%c",&(pNew->axisLabel)); if (islower(pNew->axisLabel)) pNew->axisLabel = toupper(pNew->axisLabel); if (!(pNew->axisLabel >= 'A' && pNew->axisLabel <= 'H')) { snprintf(pError, ERRLEN, "\tInvalid axis on motor '%s':%c", motor, pNew->axisLabel); SCWrite(pCon,pError,eError); KillDMC2280(pNew); free(pNew); 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); return NULL; } sscanf(pPtr,"%lg",&(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)); /* 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 controls unidirectional driving */ 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) { if ((pPtr=getParam(pCon, interp, params,"protocol",_OPTIONAL)) == NULL) pNew->protocol=0; else sscanf(pPtr,"%d",&(pNew->protocol)); } else { sscanf(pPtr,"%d",&(pNew->protocol)); } /* 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_encoder = 0; else { sscanf(pPtr,"%d",&(pNew->abs_encoder)); 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,"%lg",&(pNew->cntsPerX)); /* CREEP_OFFSET: this controls unidirectional driving */ if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL) pNew->creep_offset = 0.0; else { sscanf(pPtr, "%f", &(pNew->creep_offset)); if (pNew->creep_offset < 0) pNew->creep_offset = -pNew->creep_offset; } /* CREEP_PRECISION: this controls unidirectional driving */ if ((pPtr=getParam(pCon, interp, params,"creep_precision",_OPTIONAL)) == NULL) pNew->creep_precision = 0.0; else { sscanf(pPtr, "%f", &(pNew->creep_precision)); if (pNew->creep_precision < 0) pNew->creep_precision = -pNew->creep_precision; } /* BIAS for encoder - add this to value read */ if ((pPtr=getParam(pCon, interp, params,"bias_bias",_OPTIONAL)) == NULL) pNew->bias_bias = 0; else { sscanf(pPtr, "%d", &(pNew->bias_bias)); } /* BIAS for encoder - mask this many bits */ if ((pPtr=getParam(pCon, interp, params,"bias_bits",_OPTIONAL)) == NULL) pNew->bias_bits = 0.0; else { sscanf(pPtr, "%d", &(pNew->bias_bits)); if (pNew->bias_bits < 0) pNew->bias_bits = -pNew->bias_bits; } } if ((pPtr=getParam(pCon, interp, params,"posit_count",_OPTIONAL)) == NULL) pNew->posit_count = 0; else { sscanf(pPtr,"%d",&(pNew->posit_count)); if (pNew->posit_count < 1 || pNew->posit_count > 99) { snprintf(pError, ERRLEN, "Invalid posit_count on motor '%s': %d", motor, pNew->posit_count); SCWrite(pCon,pError,eError); pNew->posit_count = 0; } else { int i; char line[80]; pNew->positions = malloc(pNew->posit_count * sizeof(*pNew->positions)); for (i = 0; i < pNew->posit_count; ++i) { snprintf(line, 80, "posit_%d", i + 1); if ((pPtr=getParam(pCon, interp, params,line,_OPTIONAL)) == NULL) { pNew->positions[i] = 0.0; } else { pNew->positions[i] = count2unit(pNew, strtol(pPtr, NULL, 10)); } } } } change_state(pNew, DMCState_Unknown); 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("send", argv[1]) == 0) { char cmd[CMDLEN]; char rsp[CMDLEN]; int idx = 0; int i; /* Managers only */ if (!SCMatchRights(pCon, usMugger)) return 0; cmd[0] = '\0'; for (i = 2; i < argc; ++i) { int j, k; j = snprintf(&cmd[idx], CMDLEN - idx, "%s%s", (i > 2) ? " " : "", argv[i]); if (j < 0) break; for ( k = 0; k < j && cmd[idx + k]; ++k) { if (cmd[idx + k] == '%' || cmd[idx + k] == '`') cmd[idx + k] = self->axisLabel; if (islower(cmd[idx + k])) cmd[idx + k] = toupper(cmd[idx + k]); } idx += j; } DMC_SendReceive(self, cmd, rsp); SCWrite(pCon, rsp, eValue); return 1; } else if(strcasecmp("data", argv[1]) == 0) { char line[132]; snprintf(line, 132, "%s.motor_step = %10g %s", self->name, 1.0 / self->stepsPerX, self->units); SCWrite(pCon, line, eValue); if (self->abs_encoder) { snprintf(line, 132, "%s.encoder_count = %10g %s", self->name, 1.0 / self->cntsPerX, self->units); SCWrite(pCon, line, eValue); snprintf(line, 132, "%s.steps_per_count = %10g steps", self->name, self->stepsPerX / self->cntsPerX); SCWrite(pCon, line, eValue); } return 1; } else if (strcasecmp("axis", 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->axisLabel = argv[2][0]; else { char line[132]; snprintf(line, 132, "Invalid axis on motor '%s':%c", self->name, argv[2][0]); SCWrite(pCon,line,eError); return 0; } } else { char line[132]; snprintf(line, 132, "%s.axis = %c", self->name, self->axisLabel); SCWrite(pCon, line, eValue); } 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)); 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; } else 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; } else 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; } else if (strcasecmp("list", argv[1]) == 0) { /* Handle in generic motor driver */ } else if (strcasecmp("slist", argv[1]) == 0) { DMC2280StrList(self, argv[0], pCon); return 1; } else if(strcasecmp(SETPOS, argv[1]) == 0) { float oldZero, newZero, currPos, newValue; if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); assert(self->pMot); MotorGetPar(self->pMot, "softzero", &oldZero); if (argc > 3) { sscanf(argv[2], "%f", &currPos); currPos += oldZero; sscanf(argv[3], "%f", &newValue); } else if (argc > 2 ){ sscanf(argv[2], "%f", &newValue); currPos = self->pMot->fPosition; } else { char buffer[BUFFLEN]; snprintf(buffer, BUFFLEN, "%s.setPos = %f\n", self->name, oldZero); SCWrite(pCon, buffer, eStatus); return 1; } newZero = (currPos - newValue); return MotorSetPar(self->pMot, pCon, "softzero", newZero); } else if(strcasecmp("reset", argv[1]) == 0) { /* Reset the state machine */ if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; self->driver_status = HWIdle; change_state(self, DMCState_Unknown); while (self->myState == DMCState_Unknown) { pTaskMan pTasker = GetTasker(); TaskYield(pTasker); } /* Handle further in generic motor driver */ return MotorAction(pCon, pSics, pData, argc, argv); } else if(strcasecmp("state", argv[1]) == 0) { char line[132]; snprintf(line, 132, "%s.state = %s(%d) (timer=%s)", self->name, state_name(self->myState), self->subState, self->state_timer ? "active" : "inactive"); SCWrite(pCon, line, eValue); return 1; } else if(strcasecmp("trace", argv[1]) == 0) { if (argc > 2 && strcasecmp("on", argv[2]) == 0) { self->trace = pCon; SCWrite(pCon, "TRACE ON", eValue); } else { self->trace = 0; SCWrite(pCon, "TRACE OFF", eValue); } return 1; } else if(strcasecmp("trace_switches", argv[1]) == 0) { if (argc > 2 && strcasecmp("on", argv[2]) == 0) { trace_switches = true; SCWrite(pCon, "TRACE SWITCHES ON", eValue); } else { trace_switches = false; SCWrite(pCon, "TRACE SWITCHES OFF", eValue); } return 1; } else if(strcasecmp("unit2count", argv[1]) == 0) { char line[132]; if (argc > 2) { double target; target = strtod(argv[2], NULL); snprintf(line, 132, "%s.unit2count = %lld", self->name, unit2count(self, target)); } else strcpy(line, "missing value"); SCWrite(pCon, line, eValue); return 1; } else if(strcasecmp("count2unit", argv[1]) == 0) { char line[132]; if (argc > 2) { long long target; target = strtoll(argv[2], NULL, 10); snprintf(line, 132, "%s.count2unit = %f", self->name, count2unit(self, target)); } else strcpy(line, "missing value"); SCWrite(pCon, line, eValue); return 1; } else if(strcasecmp("positions", argv[1]) == 0) { if (argc == 2) { int i, k = 0; char line[1320]; k += snprintf(line, sizeof(line) - k, "%s.positions ", self->name); for (i = 0; i < self->posit_count; ++i) { k += snprintf(&line[k], sizeof(line) - k, " %f", self->positions[i]); } SCWrite(pCon, line, eValue); return 1; } if (self->posit_count > 0) { self->posit_count = 0; } if (self->positions) { free(self->positions); self->positions = NULL; self->posit_count = 0; } if (argc > 3 && strcasecmp("erase", argv[2]) == 0) { char line[132]; snprintf(line, 132, "%s.posit_count = %d", self->name, self->posit_count); SCWrite(pCon, line, eValue); return 1; } self->posit_count = argc - 2; self->positions = malloc(self->posit_count * sizeof(*self->positions)); int i; for (i = 0; i < self->posit_count; ++i) { self->positions[i] = strtod(argv[i + 2], NULL); } return 1; } if (self->abs_encoder && strcasecmp("absEncHome", argv[1]) == 0) { if (argc > 2) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) return 0; long lValue = strtol(argv[2], NULL, 10); self->absEncHome = lValue; return 1; } else { char line[132]; snprintf(line, 132, "%s.absEncHome = %d.000000", self->name, self->absEncHome); SCWrite(pCon, line, eValue); return 1; } } if (self->abs_encoder && strcasecmp("absenc", argv[1]) == 0) { long lValue; if (readAbsEnc(self, &lValue) != SUCCESS) { lValue = -1; } char line[132]; snprintf(line, 132, "%s.absEnc = %ld.000000", self->name, lValue); SCWrite(pCon, line, eValue); return 1; } if (self->posit_count > 0) { if(strcasecmp("posit2count", argv[1]) == 0) { char line[132]; if (argc > 2) { double target; target = strtod(argv[2], NULL); snprintf(line, 132, "%s.posit2count = %lld", self->name, posit2count(self, target)); } else strcpy(line, "missing value"); SCWrite(pCon, line, eValue); return 1; } else if(strcasecmp("count2posit", argv[1]) == 0) { char line[132]; if (argc > 2) { long long target; target = strtoll(argv[2], NULL, 10); snprintf(line, 132, "%s.count2posit = %f", self->name, count2posit(self, target)); } else strcpy(line, "missing value"); SCWrite(pCon, line, eValue); return 1; } else if(strcasecmp("unit2posit", argv[1]) == 0) { char line[132]; if (argc > 2) { double target; target = strtod(argv[2], NULL); snprintf(line, 132, "%s.unit2posit = %f", self->name, unit2posit(self, target)); } else strcpy(line, "missing value"); SCWrite(pCon, line, eValue); return 1; } else if(strcasecmp("posit2unit", argv[1]) == 0) { char line[132]; if (argc > 2) { double target; target = strtod(argv[2], NULL); snprintf(line, 132, "%s.posit2unit = %f", self->name, posit2unit(self, target)); } else strcpy(line, "missing value"); SCWrite(pCon, line, eValue); return 1; } else if(strcasecmp("posit", argv[1]) == 0) { char line[132]; int target = self->currCounts; if (argc > 2) target = strtol(argv[2], NULL, 10); snprintf(line, 132, "%s.posit = %f", self->name, count2posit(self, target)); SCWrite(pCon, line, eValue); return 1; } } } return MotorAction(pCon, pSics, pData, argc, argv); } void DMC2280InitProtocol(SicsInterp *pSics) { if (DMC2280_Protocol == NULL) { DMC2280_Protocol = AsyncProtocolCreate(pSics, "DMC2280", NULL, NULL); DMC2280_Protocol->sendCommand = DMC_Tx; DMC2280_Protocol->handleInput = DMC_Rx; DMC2280_Protocol->handleEvent = DMC_Ev; DMC2280_Protocol->prepareTxn = NULL; DMC2280_Protocol->killPrivate = NULL; } }