/** \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 /** \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 { } 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 #define VAR_RSP 2 #define VAR_RUN 4 #define VAR_DST 8 #define VAR_POS 16 #define VAR_HLT 32 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 lastCmd[CMDLEN]; char dmc2280Error[CMDLEN]; float 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 */ float 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 */ float cntsPerX; /**< absolute encoder counts per physical unit */ int motOffDelay; /**< msec to wait before switching motor off, default=0 */ int currFlags; int currSteps; int currCounts; float currPosition; /**< Position at last position check */ float lastPosition; /**< Position at last position check */ int lastSteps; int lastCounts; int thread0; /**< last read of _XQ0 */ unsigned short int input0; /**< last read of _TI0 */ unsigned short int input1; /**< last read of _TI1 */ bool ampError; /**< amplifier 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 has_airpads; /**< Flag = 1 if there is are airpads for this motor */ float backlash_offset; /**< signed offset to drive from */ float fTarget; /**< target passed from SICS to timer callback */ float 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 airpad_state; /**< state of the airpads finite state machine */ int airpad_counter; /**< airpad timer retry counter */ pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */ pNWTimer motoff_timer; /**< motor off timer */ 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; int creep_val; int preseek; /**< Flag = 1 if current move is a backlash preseek */ int has_fsm; /**< Flag = 1 if motor has finite state machine driver model */ int run_flag; /**< Flag = 1 if RUN command has been issued and deferred */ int driver_status; StateFunc myState; StateFunc myPrevState; int subState; pNWTimer state_timer; /**< motor state timer */ SConnection *trace; int posit_count; #ifdef POSIT_COUNTS long long* posit_array; #endif double* positions; int variables; }; int DMC2280MotionControl = 1; /* defaults to enabled */ #define AIR_POLL_TIMER 1000 #define MOTOR_POLL_FAST 200 #define MOTOR_POLL_SLOW 5000 #define ON_SETTLE_TIMER 1000 #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 // NOT SET: Does SICS already check parameter types? #define BADUNKNOWN -5 #define BADSTP -6 // NOT SET #define BADEMERG -7 // NOT SET: ESTOP #define RVRSLIM -8 #define FWDLIM -9 #define POSFAULT -11 // NOT SET #define BADCUSHION -12 // 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 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" static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); #ifdef POSIT_COUNTS static bool check_posits(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->posit_array[i] < 0) ++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->posit_array[i] > self->posit_array[i - 1]) direction = 1; else if (self->posit_array[i] < self->posit_array[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->posit_array[i] < self->posit_array[i - 1])) { direction = 0; } break; case 1: if (!(self->posit_array[i] > self->posit_array[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' : %lld", i + 1, self->name, self->posit_array[i]); /* TODO log me */ } return false; } #endif 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, float 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 float count2unit(pDMC2280Driv self, long long counts) { float 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; } #ifdef POSIT_COUNTS static long long posit2count(pDMC2280Driv self, float target) { double absolute; long long result; if (self->abs_encoder == 0) { char line[CMDLEN]; snprintf(line, CMDLEN, "posit2count motor %s has no absolute encoder", self->name); SICSLogWrite(line, eStatus); return -1; } if (!check_posits(self)) return -1; int i = ((int) target) - 1; if (i < 0) { i = 0; } else if (i > self->posit_count - 2) { i = self->posit_count - 2; } absolute = (double)self->posit_array[i] + ((double)target - ((double)i + 1)) * ((double)self->posit_array[i + 1] - self->posit_array[i]); 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, "posit2count Rounding %f to %lld", absolute, result); SICSLogWrite(line, eStatus); } return result; } static float count2posit(pDMC2280Driv self, long long counts) { int i, j; float fPos; if (self->abs_encoder == 0) { char line[CMDLEN]; snprintf(line, CMDLEN, "count2posit motor %s has no absolute encoder", self->name); SICSLogWrite(line, eStatus); return 0; } if (!check_posits(self)) return 0; if (self->posit_array[0] < self->posit_array[1]) { for (i = 1; i < self->posit_count - 1; ++i) { if (counts < self->posit_array[i]) { break; } } } else { for (i = 1; i < self->posit_count - 1; ++i) { if (counts > self->posit_array[i]) { break; } } } --i; j = i + 1; fPos = (double) j + ((double)counts - self->posit_array[i]) / ((double)self->posit_array[j] - self->posit_array[i]); return fPos; } static float unit2posit(pDMC2280Driv self, float target) { return count2posit(self, unit2count(self, target)); } static float posit2unit(pDMC2280Driv self, float target) { return count2unit(self, posit2count(self, target)); } #else static float unit2posit(pDMC2280Driv self, float target) { int i, j; float 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 float posit2unit(pDMC2280Driv self, float 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, float target) { return unit2count(self, posit2unit(self, target)); } static float count2posit(pDMC2280Driv self, long long counts) { return unit2posit(self, count2unit(self, counts)); } #endif /** \brief Convert axis speed in physical units to * motor speed in steps/sec. * * \param self (r) provides access to the motor's data structure * \param speed in physical units, eg mm/sec degrees/sec * \return the speed in motor steps/sec */ static int motSpeed(pDMC2280Driv self, float axisSpeed) { int speed; speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5); return speed; } /** \brief Convert axis acceleration in physical units to * motor speed in steps/sec^2 * * \param self (r) provides access to the motor's data structure * \param acceleration in physical units, eg mm/sec^2 degrees/sec^2 * \return the acceleration in motor steps/sec^2 */ static int motAccel(pDMC2280Driv self, float axisAccel) { int accel; accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5); return accel; } /** \brief Convert axis deceleration in physical units to * motor deceleration in steps/sec^2 * * \param self (r) provides access to the motor's data structure * \param deceleration in physical units, eg mm/sec^2 degrees/sec^2 * \return the deceleration in motor steps/sec^2 */ static int motDecel(pDMC2280Driv self, float axisDecel) { int decel; decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5); return decel; } /** \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 float motPosit(pDMC2280Driv self) { float 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, float 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, float 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_SendCmd(pDMC2280Driv self, char* command, AsyncTxnHandler callback) { return AsyncUnitSendTxn(self->asyncUnit, command, strlen(command), 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 ':': case ' ': if (self->debug) { SICSLogWrite(cmnd, eStatus); SICSLogWrite(resp, eStatus); } break; case '?': strncpy(self->lastCmd, pCmd->out_buf, CMDLEN); strncpy(self->dmc2280Error, &resp[1], CMDLEN); SICSLogWrite(cmnd, eError); SICSLogWrite(resp, eError); self->errorCode = BADCMD; break; default: SICSLogWrite(cmnd, eError); SICSLogWrite(resp, eError); self->errorCode = BADUNKNOWN; break; } } return 0; } static int DMC2280Queue(pDMC2280Driv self, char *cmd, AsyncTxnHandler cb) { if (cb == NULL) cb = SendCallback; return DMC_SendCmd(self, cmd, cb); } /** \brief Sends a DMC2280 command to the motor controller. * * If the command fails it displays the DMC2280 error message to the client * and writes it to the log file, also sets errorCode field in motor's * data structure. * * \param self (rw) provides access to the motor's data structure * \param *command (r) DMC2280 command * \return * - SUCCESS * - FAILURE * \see SUCCESS FAILURE */ /* First character returned by controller is '?' for an invalid command or ':' for a valid command with no response ' ' for a valid command with a response (':' follows) */ static int DMC2280Send(pDMC2280Driv self, char *command) { return DMC2280Queue(self, command, SendCallback); } /** * \brief Sends a command and waits for a response * * \param self motor data * \param cmd command to send * \param reply space to return response * \return */ static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) { int status; status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, CMDLEN); if (status != 1) { if (self->debug) SICSLogWrite(cmd, eStatus); if (status == -1) self->errorCode = MOTCMDTMO; else self->errorCode = BADUNKNOWN; return FAILURE; } switch (reply[0]) { case ':': if (self->debug) { SICSLogWrite(cmd, eStatus); SICSLogWrite(reply, eStatus); } return SUCCESS; case ' ': if (self->debug) { SICSLogWrite(cmd, eStatus); SICSLogWrite(reply, eStatus); } return SUCCESS; case '?': strncpy(self->lastCmd, cmd, CMDLEN); strncpy(self->dmc2280Error, &reply[1], CMDLEN); SICSLogWrite(cmd, eError); SICSLogWrite(reply, eError); self->errorCode = BADCMD; return FAILURE; default: strncpy(self->lastCmd, cmd, CMDLEN); strncpy(self->dmc2280Error, &reply[0], CMDLEN); SICSLogWrite(cmd, eError); SICSLogWrite(reply, eError); self->errorCode = BADUNKNOWN; return FAILURE; } return OKOK; } /** \brief Record the given posn and timestamp it. * * \param *pData provides access to a motor's data * \param posn, the axis position which you want to remember. * */ static void set_lastMotion(pDMC2280Driv self, float steps, float counts) { assert(self != NULL); self->lastSteps = steps; self->lastCounts = counts; gettimeofday(&(self->time_lastPos_set), NULL); } 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, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "TP%c", self->axisLabel); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; *pos = (float) atoi(reply); return SUCCESS; } /** \brief Reads Thread variable * * \param *pos is the thread variable value on SUCCESS. * \return * SUCCESS * FAILURE */ static int readThread(pDMC2280Driv self, int thread, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG _XQ%d", thread); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return 0; *pos = (float) atoi(reply); return 1; } /** \brief Reads HOMERUN variable * * \param *pos is the homerun variable value on SUCCESS. * \return * SUCCESS * FAILURE */ static int readHomeRun(pDMC2280Driv self, float *pos) { char reply[CMDLEN]; char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG HOMERUN"); if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; *pos = (float) atoi(reply); return SUCCESS; } /** \brief Runs the HOMERUN routine on the MC * * \return * SUCCESS * FAILURE */ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { float fValue; char cmd[CMDLEN]; /* Read HOMERUN should get error if it does not have one */ if (FAILURE == readHomeRun(self, &fValue)) return FAILURE; /* 0 => reset homerun */ if (newValue < 0.5) { snprintf(cmd, CMDLEN, "HOMERUN=0"); if (FAILURE == DMC2280Send(self, cmd)) return FAILURE; return SUCCESS; } snprintf(cmd, CMDLEN, "XQ #HOME,1"); if (FAILURE == DMC2280Send(self, cmd)) return FAILURE; return SUCCESS; } /** \brief Check if the axis has moved significantly since * the last check. * * The motion is checked against the expected at intervals of * pDMC2280Driv->blockage_ckInterval. * * \param *pData provides access to a motor's data * \return * - 1 MOTOR OK, position has changed significantly during move * - 0 MOTOR BLOCKED, no significant change in position detected. */ static int checkMotion(void *pData) { float ratio_obs, ratio_exp, ratio_cmp; long int usec_TimeDiff; struct timeval now; pDMC2280Driv self; self = (pDMC2280Driv)pData; assert(self != NULL); /* we can only test if there is an absolute encoder */ if (!self->abs_encoder) return 1; if (self->time_lastPos_set.tv_sec == 0) { /* first time - initialise the data */ set_lastMotion(self, self->currSteps, self->currCounts); return 1; } gettimeofday(&now, NULL); usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec; usec_TimeDiff *= 1000000; usec_TimeDiff += now.tv_usec; usec_TimeDiff -= self->time_lastPos_set.tv_usec; if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval)) return 1; /* If not stepping, then not blocked */ if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ set_lastMotion(self, self->lastSteps, self->lastCounts); return 1; } /* 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 = (self->currSteps - self->lastSteps) / (self->currCounts - self->lastCounts); ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX; ratio_cmp = ratio_obs / ratio_exp; /* wrong signs, less than half, or more than double is trouble */ if (ratio_cmp < 0.0 || ratio_cmp > self->blockage_ratio || (1.0 / ratio_cmp) > self->blockage_ratio) { char msg[132]; float cmp = ratio_cmp; if (fabs(cmp) > 0 && fabs(cmp) < 1) cmp = 1.0 / cmp; 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 (self->blockage_fail) return 0; set_lastMotion(self, self->currSteps, self->currCounts); return 1; } else { if (self->debug) { char msg[132]; float 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); } set_lastMotion(self, self->currSteps, self->currCounts); return 1; } return 1; } /** * \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_x(pDMC2280Driv self, const char* vars, const char* name) { char tmp[20]; char* p; snprintf(tmp, 20, "%s%c=", name, self->axisLabel); p = strstr(vars, tmp); if (p == NULL) /* not found at all */ return false; if (p == vars) /* found at start of first line */ return false; if (p[-1] == '\n') /* found at start of another line */ return true; return false; /* found partial (ZVVVx=) only */ } /* State Machine Events */ 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); /** * \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]; if (self->has_airpads == 3) snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,RUN%c,_TI0,_TI1,_XQ0", self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel); else snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_BG%c,_TI0,_TI1,_XQ0", self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel); return DMC_SendCmd(self, cmd, state_msg_callback); } static int cmdVars(pDMC2280Driv self) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "LV"); return DMC_SendCmd(self, cmd, state_msg_callback); } static int cmdOn(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->has_airpads == 1) { snprintf(cmd, CMDLEN, "FTUBE=1"); } else if (self->has_airpads == 2 || self->has_airpads == 3) { snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel); } else { snprintf(cmd, CMDLEN, "SH%c", self->axisLabel); } return DMC_SendCmd(self, cmd, state_msg_callback); } static int cmdPosition(pDMC2280Driv self, int target) { char cmd[CMDLEN]; if (self->has_airpads == 3) snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target); else snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target); return DMC_SendCmd(self, cmd, state_msg_callback); } static int cmdBegin(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->has_airpads == 3) snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel); else snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); return DMC_SendCmd(self, cmd, state_msg_callback); } static int cmdPoll(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->has_airpads == 2 || self->has_airpads == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); else if (self->has_airpads == 1) snprintf(cmd, CMDLEN, "MG APDONE"); return DMC_SendCmd(self, cmd, state_msg_callback); } static int cmdHalt(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->variables & VAR_HLT) snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel); else snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); return DMC_SendCmd(self, cmd, state_msg_callback); } static int cmdOff(pDMC2280Driv self) { char cmd[CMDLEN]; if (self->has_airpads == 1) { snprintf(cmd, CMDLEN, "FTUBE=0"); } else if (self->has_airpads == 2 || self->has_airpads == 3) { snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); } else { snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); } return DMC_SendCmd(self, cmd, state_msg_callback); } static int rspStatus(pDMC2280Driv self, const char* text) { int iRet, iFlags; int iSteps, iCounts; int iTI0, iTI1, iXQ0, iBG; /* TODO: add _RUNx for has_airpads == 3 */ iRet = sscanf(text, "%d %d %d %d %d %d %d", &iSteps, &iCounts, &iFlags, &iBG, &iTI0, &iTI1, &iXQ0); if (iRet != 7) return 0; self->currFlags = iFlags; self->currSteps = iSteps; self->currCounts = iCounts; self->currPosition = motPosit(self); self->thread0 = iXQ0; self->input0 = iTI0; self->input1 = iTI1; if (self->axisLabel >= 'A' && self->axisLabel <= 'D') self->ampError = !(1 & (iTI0 >> (self->axisLabel - 'A'))); else self->ampError = !(1 & (iTI1 >> (self->axisLabel - 'E'))); self->threadError = self->thread0 < 0; self->moving = iBG > 0; return 1; } static int rspVars(pDMC2280Driv self, const char* text) { self->variables = 0; 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 ((self->variables & VAR_REQ) && (self->variables & VAR_RSP)) { self->has_airpads = 2; if ((self->variables & VAR_RUN) && (self->variables & VAR_DST) && (self->variables & VAR_POS)) { self->has_airpads = 3; } } return 1; } /* 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_MotorOff(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_MotorOff) return "DMCState_MotorOff"; if (func == DMCState_Error) return "DMCState_Error"; return ""; } 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"); 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 (self->debug || self->trace) { char line[CMDLEN]; 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 (self->debug || self->trace) { char line[CMDLEN]; 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) { char line[CMDLEN]; 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, eStatus); } static void handle_event(pDMC2280Driv self, pEvtEvent event) { StateFunc oldState; if (self->debug || self->trace) report_event(self, event); oldState = self->myState; self->myState(self, event); } static int state_msg_callback(pAsyncTxn pCmd) { pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; EvtEvent event; if (pCmd->txn_status == ATX_TIMEOUT) { if (self->debug) { SICSLogWrite(pCmd->out_buf, eStatus); SICSLogWrite("", eStatus); } event.event_type = eTimeoutEvent; event.event.msg.cmd = pCmd; } else { if (self->debug) { SICSLogWrite(pCmd->out_buf, eStatus); SICSLogWrite(pCmd->inp_buf, eStatus); } event.event_type = eMessageEvent; event.event.msg.cmd = pCmd; } 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; 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 eTimerEvent: return; case eStateEvent: /* Set speed */ value = motSpeed(self, self->speed); snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value); if (FAILURE == DMC2280Send(self, cmd)) { break; } /* Set acceleration */ value = motAccel(self, self->accel); snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value); if (FAILURE == DMC2280Send(self, cmd)) { break; } /* Set deceleration */ value = motDecel(self, self->decel); snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value); if (FAILURE == DMC2280Send(self, cmd)) { break; } cmdStatus(self); self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* Status Request */ int iRet; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); if (self->pMot) { self->pMot->fTarget = self->pMot->fPosition = motPosit(self); } cmdVars(self); self->subState = 2; return; } if (self->subState == 2) { /* Vars */ rspVars(self, pCmd->inp_buf); change_state(self, DMCState_Idle); return; } } while (0); break; case eTimeoutEvent: /* TODO: handle timeout */ break; } 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); NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_SLOW, state_tmr_callback, self); 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; float fDelta; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; /* action deferred RUN command */ if (self->run_flag) { self->run_flag = 0; change_state(self, DMCState_MotorStart); return; } /* if the motor moved, update any observers */ if (self->abs_encoder) fDelta = fabs(self->currCounts - self->lastCounts); else fDelta = fabs(self->currSteps - self->lastSteps); if (fDelta > 10) { set_lastMotion(self, self->currSteps, self->currCounts); report_motion(self); } NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_SLOW, state_tmr_callback, self); self->subState = 0; return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); assert(self->pMot); self->driver_status = HWBusy; if (self->subState == 0) { change_state(self, DMCState_MotorStart); } else { self->run_flag = 1; } return; case CMD_HALT: /* handle halt command */ self->run_flag = 0; return; } break; case eTimeoutEvent: /* TODO handle message timeout */ break; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { case eStateEvent: 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; } #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: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { if (self->has_airpads == 0) { /* SH */ change_state(self, DMCState_MotorOn); return; } else { NetWatchRegisterTimer(&self->state_timer, AIR_POLL_TIMER, state_tmr_callback, self); self->subState = 0; return; } } else if (self->subState == 2) { /* Poll */ float fReply; fReply = (float) atof(pCmd->inp_buf); if (fReply > 0) { change_state(self, DMCState_MotorOn); return; } /* TODO handle airpad timeout */ NetWatchRegisterTimer(&self->state_timer, AIR_POLL_TIMER, state_tmr_callback, self); self->subState = 0; return; } } while (0); break; /* TODO: handle halt command */ case eTimeoutEvent: /* TODO handle message timeout */ break; } 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: NetWatchRegisterTimer(&self->state_timer, ON_SETTLE_TIMER, state_tmr_callback, self); 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, absolute; float 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_Error); return; } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; self->driver_status = HWFault; change_state(self, DMCState_Error); 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; return; } else if (self->subState == 2) { /* PA */ change_state(self, DMCState_Moving); return; } } while (0); break; /* TODO: handle halt command */ case eTimeoutEvent: /* TODO handle message timeout */ break; } 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: cmdStatus(self); self->subState = 2; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* BG */ NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); return; } else if (self->subState == 3) { /* PA */ cmdBegin(self); self->stepCount++; self->subState = 1; return; } else if (self->subState == 2) { /* MG */ int iRet, iFlags; bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; if (self->threadError) { self->errorCode = THREADZERO; self->driver_status = HWFault; change_state(self, DMCState_Error); return; } #ifdef AMPERROR else if (self->ampError) { self->errorCode = AMPERROR; self->driver_status = HWFault; change_state(self, DMCState_Error); return; } #endif iFlags = self->currFlags; moving = (iFlags & STATUSMOVING); if (moving) { /* If Motion Control is off, report HWFault */ if (DMC2280MotionControl != 1) { if (DMC2280MotionControl == 0) self->errorCode = MOTIONCONTROLOFF; else self->errorCode = MOTIONCONTROLUNK; self->driver_status = HWFault; change_state(self, DMCState_MotorHalt); return; } if (self->abs_encoder && checkMotion(self) == 0) { /* handle blocked */ self->errorCode = BLOCKED; self->driver_status = HWFault; change_state(self, DMCState_MotorHalt); return; } NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); self->subState = 0; return; } /* Motor has stopped */ /* Handle limit switches */ 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 (fwd_limit_active) { self->errorCode = FWDLIM; self->driver_status = HWFault; } else if (rvrs_limit_active) { self->errorCode = RVRSLIM; self->driver_status = HWFault; } else if (errorlimit) { self->errorCode = ERRORLIM; self->driver_status = HWFault; } if (self->driver_status == HWFault) { change_state(self, DMCState_MotorHalt); return; } /* * If this was a pre-seek then compute the next iteration */ if (self->preseek) { float target; int absolute; self->preseek = 0; target = self->fTarget; self->fPreseek = target; /* if we are not creeping it is backlash correction */ /* TODO: ensure minimum progress so it doesn't stall */ /* TODO: handle edge conditions like limits */ if (self->creep_val == 0) { float precision; MotorGetPar(self->pMot, "precision", &precision); /* TODO: take precision into account */ 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 { 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; NetWatchRegisterTimer(&self->state_timer, self->settle, state_tmr_callback, self); return; } if (self->motOffDelay) NetWatchRegisterTimer(&self->state_timer, self->motOffDelay, state_tmr_callback, self); else NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); self->driver_status = HWIdle; change_state(self, DMCState_OffTimer); return; } } while (0); break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_HALT: /* handle halt command, send message */ 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) { char cmd[CMDLEN]; switch (event->event_type) { case eStateEvent: if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; cmdHalt(self); return; case eTimerEvent: cmdStatus(self); self->subState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 0) { /* ST */ NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); return; } else if (self->subState == 1) { /* Status Response */ int iRet, iFlags; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; iFlags = self->currFlags; if (iFlags & STATUSMOVING) { NetWatchRegisterTimer(&self->state_timer, MOTOR_POLL_FAST, state_tmr_callback, self); return; } change_state(self, DMCState_MotorStop); return; } } while (0); break; case eTimeoutEvent: /* TODO handle message timeout */ break; } 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: return; case eTimerEvent: change_state(self, DMCState_MotorStop); return; 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; 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; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { case eStateEvent: 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->has_airpads == 0) { change_state(self, DMCState_MotorOff); return; } } else if (self->subState == 1) { /* Poll */ float fReply; fReply = (float) atof(pCmd->inp_buf); if (fReply == 0) { change_state(self, DMCState_MotorOff); return; } } NetWatchRegisterTimer(&self->state_timer, AIR_POLL_TIMER, state_tmr_callback, self); } while (0); return; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* TODO: handle run command */ return; case CMD_HALT: /* TODO: handle halt command */ return; } break; case eTimeoutEvent: /* TODO handle message timeout */ break; } unhandled_event(self, event); self->errorCode = STATEERROR; change_state(self, DMCState_Error); return; } static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: NetWatchRegisterTimer(&self->state_timer, ON_SETTLE_TIMER, state_tmr_callback, self); return; case eTimerEvent: case eMessageEvent: /* progress to IDLE */ if (self->driver_status == HWBusy) self->driver_status = HWIdle; change_state(self, DMCState_Idle); return; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* TODO: handle run command */ return; case CMD_HALT: /* TODO: handle halt command */ return; } break; case eTimeoutEvent: /* TODO handle message timeout */ break; } 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: return; case eTimerEvent: case eMessageEvent: return; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: /* TODO: handle run command */ return; case CMD_HALT: /* TODO: handle halt command */ return; } 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); if (self->has_fsm) { // if (self->myState == DMCState_Unknown) // SicsWait(1); *fPos = motPosit(self); return OKOK; } 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->motoff_timer) NetWatchRemoveTimer(self->motoff_timer); self->motoff_timer = NULL; if (self->settle) self->time_settle_done.tv_sec = 0; /* * Note: this will read the current position which will block */ do { float currPos; DMC2280GetPos(pData, &currPos); self->lastPosition = currPos; self->time_lastPos_set.tv_sec = 0; } while (0); self->fTarget = fValue; /* TODO: FIXME and handle RUN command everywhere */ while (self->myState == DMCState_MotorStop || self->myState == DMCState_MotorOff) { SicsWait(1); } state_cmd_execute(self, CMD_RUN); return 1; } /** \brief Returns the motor status while it's moving, * implements the GetStatus method in the MotorDriver interface. * * \param *pData provides access to a motor's data * \return * - HWFault hardware fault or status cannot be read. * - HWPosFault controller was unable to position the motor. * - HWBusy The motor is still driving. * - HWWarn There is a warning from the controller. * - HWIdle The motor has finished driving and is idle. */ static int DMC2280Status(void *pData){ pDMC2280Driv self = NULL; 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){ #ifdef HAS_RS232 case NOTCONNECTED: case BADSEND: case BADMEMORY: case INCOMPLETE: strncpy(error, "General Error(TODO)", (size_t)errLen); /* TODO */ break; #endif case MOTCMDTMO: strncpy(error, "Command Timeout", (size_t)errLen); break; case BADADR: strncpy(error, "Bad address", (size_t)errLen); break; case BADBSY: strncpy(error, "Motor still busy", (size_t)errLen); break; case BADCMD: snprintf(error, (size_t)errLen, "Bad command: '%s'\ndmcError: ", self->lastCmd); strncat(error, self->dmc2280Error, (size_t)errLen); break; case BADPAR: strncpy(error, "Bad parameter", (size_t)errLen); break; case BADUNKNOWN: strncpy(error, "Unknown error condition", (size_t)errLen); break; case BADSTP: strncpy(error, "Motor is stopped", (size_t)errLen); break; case BADEMERG: strncpy(error, "Emergency stop is engaged", (size_t)errLen); break; case BGFAIL: strncpy(error, "Begin not possible due to limit switch", (size_t)errLen); break; case RVRSLIM: strncpy(error, "Crashed into limit switch", (size_t)errLen); break; case FWDLIM: strncpy(error, "Crashed into limit switch", (size_t)errLen); break; case POSFAULT: strncpy(error, "Positioning fault detected", (size_t)errLen); break; case BADCUSHION: strncpy(error, "Air cushion problem", (size_t)errLen); break; case ERRORLIM: strncpy(error, "Axis error exceeds error limit", (size_t)errLen); break; case IMPOSSIBLE_LIM_SW: strncpy(error, "Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen); break; case BLOCKED: strncpy(error, "STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen); break; case MOTIONCONTROLOFF: strncpy(error, "MOTION CONTROL SEEMS TO BE DISABLED", (size_t)errLen); break; case MOTIONCONTROLUNK: strncpy(error, "MOTION CONTROL SEEMS TO BE UNKNOWN", (size_t)errLen); break; 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 default: /* FIXME What's the default */ snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode); break; } strncpy(self->errorMsg, error, (size_t)errLen); } /** \brief Attempts to recover from an error. Implements the TryAndFixIt * method in the MotorDriver interface. * * \param *pData provides access to a motor's data * \param iCode error code returned by DMC2280Error * \param fValue unused, target position * \return A return code which informs the abstract motors next action. * - MOTREDO try to redo the last move. * - MOTFAIL move failed, give up. */ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; assert(self != NULL); switch(iCode){ case BADADR: return MOTFAIL; case BADCMD: case BADPAR: case BLOCKED: case MOTIONCONTROLOFF: case MOTIONCONTROLUNK: case MOTCMDTMO: case THREADZERO: #ifdef AMPERROR case AMPERROR: #endif return MOTFAIL; case POSFAULT: #if HAS_RS232 case BADSEND: case TIMEOUT: case BADMEMORY: /* Won't happen if MonConnect sets the send terminator */ case INCOMPLETE: return MOTREDO; case NOTCONNECTED: #endif return MOTREDO; break; case STATEERROR: /* TODO: recover state error */ if (self->has_fsm) { if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; change_state(self, DMCState_Unknown); /* Schedule a timer event as soon as possible */ NetWatchRegisterTimer(&self->state_timer, 0, state_tmr_callback, self); } 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_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,HARDLOWERLIM) == 0) { *fValue = self->fLower; return 1; } if(strcasecmp(name,HARDUPPERLIM) == 0) { *fValue = self->fUpper; return 1; } if(strcasecmp(name,SPEED) == 0) { *fValue = self->speed; return 1; } if(strcasecmp(name,MAXSPEED) == 0) { *fValue = self->maxSpeed; return 1; } if(strcasecmp(name,ACCEL) == 0) { *fValue = self->accel; return 1; } if(strcasecmp(name,MAXACCEL) == 0) { *fValue = self->maxAccel; return 1; } if(strcasecmp(name,DECEL) == 0) { *fValue = self->decel; return 1; } if(strcasecmp(name,MAXDECEL) == 0) { *fValue = self->maxDecel; return 1; } if(strcasecmp(name,MOTOFFDELAY) == 0) { *fValue = self->motOffDelay; return 1; } if(strcasecmp(name,"debug") == 0) { *fValue = self->debug; return 1; } if(strcasecmp(name,SETTLE) == 0) { *fValue = self->settle; return 1; } if(strcasecmp(name,AIRPADS) == 0) { *fValue = self->has_airpads; return 1; } if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { *fValue = self->blockage_ckInterval; return 1; } if(strcasecmp(name,"blockage_thresh") == 0) { *fValue = self->blockage_thresh; return 1; } if(strcasecmp(name,"blockage_ratio") == 0) { *fValue = self->blockage_ratio; return 1; } if(strcasecmp(name,"blockage_fail") == 0) { *fValue = self->blockage_fail; return 1; } if(strcasecmp(name,"backlash_offset") == 0) { *fValue = self->backlash_offset; return 1; } if (self->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 (self->abs_encoder != 0) { if (strcasecmp(name,"absenc") == 0) { if (readAbsEnc(self, fValue) == SUCCESS) return 1; else return 0; } if (strcasecmp(name,"absenchome") == 0) { *fValue = self->absEncHome; return 1; } if (self->has_fsm) { 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; /* XXX Maybe move this to a configuration parameter.*/ /* Set home, managers only. Users should set softposition */ if(strcasecmp(name,HOME) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { if ( (self->fLower - newValue) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower); SCWrite(pCon, pError, eError); return 1; } if ( (newValue - self->fUpper) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper); SCWrite(pCon, pError, eError); return 1; } self->fHome = newValue; return 1; } } 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) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->has_airpads = newValue; return 1; } } /* Set interval between blocked motor checks, * managers only */ if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_ckInterval = newValue; return 1; } } /* Set movement threshold for blocked motor checks, * managers only */ if(strcasecmp(name,"blockage_thresh") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_thresh = newValue; return 1; } } /* Set ratio for blocked motor checks, * managers only */ if(strcasecmp(name,"blockage_ratio") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_ratio = newValue; return 1; } } /* Set blocked motor checks failure mode, * managers only */ if(strcasecmp(name,"blockage_fail") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->blockage_fail = newValue; return 1; } } /* Set backlash offset, * managers only */ if(strcasecmp(name,"backlash_offset") == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { self->backlash_offset = newValue; return 1; } } if (self->abs_encoder) { /* If we DO have an absolute encoder */ if (self->has_fsm) { /* If we DO have a finite state machine */ /* 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 == DMC2280Send(self, cmd)) return 0; /* FIXME should signal a HWFault */ return 1; } /* Set Acceleration */ if(strcasecmp(name,ACCEL) == 0) { if ((0.0 - newValue) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, ACCEL, 0.0); SCWrite(pCon, pError, eError); return 1; } if ((newValue - self->maxAccel ) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, ACCEL, self->maxAccel); SCWrite(pCon, pError, eError); return 1; } self->accel = newValue; snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel)); if (FAILURE == DMC2280Send(self, cmd)) return 0; /* FIXME should signal a HWFault */ return 1; } /* Set Deceleration */ if(strcasecmp(name,DECEL) == 0) { if ((0.0 - newValue) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, DECEL, 0.0); SCWrite(pCon, pError, eError); return 1; } if ((newValue - self->maxDecel ) > FLT_EPSILON) { snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, DECEL, self->maxDecel); SCWrite(pCon, pError, eError); return 1; } self->decel = newValue; snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel)); if (FAILURE == DMC2280Send(self, cmd)) return 0; /* FIXME should signal a HWFault */ return 1; } 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; #ifdef POSIT_COUNTS self->posit_array[index - 1] = newValue; #endif 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); 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.AirPads = %d\n", name, self->has_airpads); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.absEnc = %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); if (self->has_fsm) { 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 */ if (self->has_fsm) { /* Reset the state machine */ if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; change_state(self, DMCState_Unknown); /* Schedule a timer event as soon as possible */ NetWatchRegisterTimer(&self->state_timer, 0, state_tmr_callback, self); } 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; } #ifdef POSIT_COUNTS if (self->posit_array) { free(self->posit_array); self->posit_array = NULL; } #endif 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)); } /* FSM: this driver uses the finite state machine model */ if ((pPtr=getParam(pCon, interp, params,"fsm",_OPTIONAL)) == NULL) pNew->has_fsm=1; else { sscanf(pPtr,"%d",&(pNew->has_fsm)); assert(pNew->has_fsm); /* DEPRECATED */ } 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,"stepsperx",_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); return NULL; } sscanf(pPtr,"%f",&(pNew->stepsPerX)); if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL) pNew->motorHome=0; else sscanf(pPtr,"%d",&(pNew->motorHome)); if ((pPtr=getParam(pCon, interp, params,"nopowersave",_OPTIONAL)) == NULL) pNew->noPowerSave=_SAVEPOWER; else sscanf(pPtr,"%d",&(pNew->noPowerSave)); if ((pPtr=getParam(pCon, interp, params,"motoffdelay",_OPTIONAL)) == NULL) pNew->motOffDelay=0; else sscanf(pPtr,"%d",&(pNew->motOffDelay)); /* 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) pNew->has_airpads=0; else { sscanf(pPtr,"%d",&(pNew->has_airpads)); } /* ABSENC: If the parameter requires an abs enc add it to the else block */ if ((pPtr=getParam(pCon, interp, params,"absenc",_OPTIONAL)) == NULL) pNew->abs_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,"%f",&(pNew->cntsPerX)); if (pNew->has_fsm) { /* 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; } } 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]; #ifdef POSIT_COUNTS pNew->posit_array = malloc(pNew->posit_count * sizeof(*pNew->posit_array)); #endif 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) { #ifdef POSIT_COUNTS pNew->posit_array[i] = -1; #endif pNew->positions[i] = 0.0; } else { #ifdef POSIT_COUNTS pNew->posit_array[i] = strtol(pPtr, NULL, 10); #endif 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; 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; } DMC2280SendReceive(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 = %f %s", self->name, 1.0 / self->stepsPerX, self->units); SCWrite(pCon, line, eValue); if (self->abs_encoder) { snprintf(line, 132, "%s.encoder_count = %f %s", self->name, 1.0 / self->cntsPerX, self->units); SCWrite(pCon, line, eValue); snprintf(line, 132, "%s.steps_per_count = %f 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, self->axisLabel); 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("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) { if (self->has_fsm) { /* Reset the state machine */ if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; change_state(self, DMCState_Unknown); /* Schedule a timer event as soon as possible */ NetWatchRegisterTimer(&self->state_timer, 0, state_tmr_callback, self); } /* Handle further in generic motor driver */ return MotorAction(pCon, pSics, pData, argc, argv); } else if(self->has_fsm && 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(self->has_fsm && 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("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; } #ifdef POSIT_COUNTS if (self->posit_array) { free(self->posit_array); self->posit_array = NULL; } #endif 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)); #ifdef POSIT_COUNTS self->posit_array = malloc(self->posit_count * sizeof(*self->posit_array)); #endif int i; for (i = 0; i < self->posit_count; ++i) { self->positions[i] = strtod(argv[i + 2], NULL); #ifdef POSIT_COUNTS self->posit_array[i] = unit2count(self, self->positions[i]); #endif } 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; } }