From 5eec1a9cd55516fff45eadb1cf37583dc041429e Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Tue, 11 Mar 2014 17:10:27 +1100 Subject: [PATCH] Impliment motor "oscillate" and "status" commands and prefix "driver run" --- site_ansto/motor_dmc2280.c | 411 ++++++++++++++++++++++++++++++------- 1 file changed, 340 insertions(+), 71 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 17f3b8c9..0d438d52 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -271,6 +271,13 @@ struct __MoDriv { double S_r; bool doTracking; TrackEntryBuffer *trackHead, *trackTail; + bool doOscillate; /**< flag for motor oscillation */ + bool oscillate_up; /**< next oscillation move is to high position */ + bool oscillate_init; /**< initial move is not counted */ + int oscillate_count; /**< number of moves to make or -1 for continuous */ + int oscillate_counter; /**< number of moves made - half-oscillations */ + double oscillate_low; /**< soft position at low end */ + double oscillate_high; /**< soft position at high end */ }; int DMC2280MotionControl = 1; /* defaults to enabled */ @@ -361,6 +368,64 @@ static double getMotorParam(pDMC2280Driv self, char *param) { return fParam; } +static bool parseNumber(const char *str, double *pNumber) { + char *endPtr; + double fNumber; + if (!pNumber) + return false; + fNumber = strtod(str, &endPtr); + if (endPtr == str || errno == ERANGE) + return false; + /* Reject Infinity and Not a Number (NaN) */ + switch (fpclassify(fNumber)) { + case FP_ZERO: + case FP_NORMAL: + break; + default: + return false; + } + *pNumber = fNumber; + return true; +} + +static bool getHardFromSoft(pDMC2280Driv self, SConnection *pCon, double fSoft, double *pHard) { + float fLower, fUpper, fZero, fSign, fLim; + double fHard; + + MotorGetPar(self->pMot, "softlowerlim", &fLower); + MotorGetPar(self->pMot, "softupperlim", &fUpper); + MotorGetPar(self->pMot, "softzero", &fZero); + MotorGetPar(self->pMot, "sign", &fSign); + if (fSoft > fUpper) { + if (pCon) + SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s", + fSoft, fUpper, self->name); + return false; + } + if (fSoft < fLower) { + if (pCon) + SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s", + fSoft, fLower, self->name); + return false; + } + fHard = fSoft; + fHard += fZero; + fHard *= fSign; + if (fHard > self->fUpper) { + if (pCon) + SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s", + fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name); + return false; + } else if (fHard < self->fLower) { + if (pCon) + SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s", + fSoft, self->fLower * fSign + fZero, self->fLower, self->name); + return false; + } + *pHard = fHard; + return true; +} + static void TrackingRelease(pDMC2280Driv self) { while (self->trackHead) { TrackEntryBuffer *victim; @@ -1745,6 +1810,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event); static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event); static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event); static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event); +static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event); static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event); static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event); static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event); @@ -1763,6 +1829,7 @@ static char* state_name(StateFunc func) { if (func == DMCState_Moving) return "DMCState_Moving"; if (func == DMCState_StepMove) return "DMCState_StepMove"; if (func == DMCState_SimpleMove) return "DMCState_SimpleMove"; + if (func == DMCState_Oscillate) return "DMCState_Oscillate"; if (func == DMCState_BacklashStart) return "DMCState_BacklashStart"; if (func == DMCState_BacklashCont) return "DMCState_BacklashCont"; if (func == DMCState_CreepStart) return "DMCState_CreepStart"; @@ -2523,6 +2590,10 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { if (self->trackHead) TrackingRelease(self); self->myNextState = NULL; + if (self->doOscillate) { + change_state(self, DMCState_Oscillate); + return; + } if (self->backlash_offset != 0) { change_state(self, DMCState_BacklashStart); return; @@ -3072,6 +3143,87 @@ static void DMCState_CreepCont(pDMC2280Driv self, pEvtEvent event) { return; } +/* + * This state oscillates the motor between two positions + */ +static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event) { + double target; + int absolute; + + switch (event->event_type) { + case eStateEvent: + self->myNextState = NULL; + /* handle termination conditions (e.g. count) */ + if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count) + self->doOscillate = false; + if (!self->doOscillate) { + change_state(self, DMCState_OffTimer); /* TODO: check this is OK */ + return; + } + if (self->oscillate_up) { + double fHard; + if (getHardFromSoft(self, NULL, self->oscillate_high, &fHard)) + self->fTarget = fHard; + self->oscillate_up = false; + } else { + double fHard; + if (getHardFromSoft(self, NULL, self->oscillate_low, &fHard)) + self->fTarget = fHard; + self->oscillate_up = true; + } + /* TODO: set timer for optional pause */ + /* Note: fallthrough is intentional */ + case eTimerEvent: + target = self->fTarget; + self->preseek = 0; + absolute = motCreep(self, target); + self->doSettle = false; + cmdPosition(self, absolute); + return; + case eMessageEvent: + if (self->run_flag != 0) { + change_state(self, DMCState_MotorHalt); + return; + } + /* set the next state after this move */ + self->myNextState = DMCState_Oscillate; + if (!self->oscillate_init) + self->oscillate_counter++; + self->oscillate_init = false; + change_state(self, DMCState_StepMove); + return; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* TODO: FIXME RUN command while running */ + if (self->driver_status == HWIdle) + self->driver_status = HWBusy; + self->run_flag = 1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + case CMD_HALT: + /* handle halt command, send message */ + self->run_flag = -1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + } + break; + case eTimeoutEvent: + strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); + self->errorCode = MOTCMDTMO; + self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); + return; + } + unhandled_event(self, event); + self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; +} /* * This state moves the motor from its current position to the current destination * set by PA (or PR) by the caller in myNextState. @@ -3819,6 +3971,7 @@ static int DMC2280Run(void *pData,float fValue){ /* Only do it if it is within our hard limits */ if (fValue >= self->fLower && fValue <= self->fUpper) { self->fTarget = fValue; + self->doOscillate = false; state_cmd_execute(self, CMD_RUN); return 1; } @@ -5274,86 +5427,202 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, } return 1; } - else if(strcasecmp("run", argv[1]) == 0) { + else if(strcasecmp("oscillate_count", argv[1]) == 0) { + if (argc > 2) + self->oscillate_count = atoi(argv[2]); + else + SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count); + return 1; + } + else if (strcasecmp("oscillate_high", argv[1]) == 0) { + double fSoft = 0.0; + double fHard = 0.0; if (argc > 2) { - float fLower, fUpper, fZero, fSign, fFixed, fLim; - double fSoft; - char *endPtr; - errno = 0; - fSoft = strtod(argv[2], &endPtr); - if (endPtr == argv[2] || errno == ERANGE) { - self->errorCode = BADPAR; - SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]); + if (parseNumber(argv[2], &fSoft) && + getHardFromSoft(self, pCon, fSoft, &fHard)) + self->oscillate_high = fSoft; + } + else + SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high); + return 1; + } + else if (strcasecmp("oscillate_low", argv[1]) == 0) { + double fSoft = 0.0; + double fHard = 0.0; + if (argc > 2) { + if (parseNumber(argv[2], &fSoft) && + getHardFromSoft(self, pCon, fSoft, &fHard)) + self->oscillate_low = fSoft; + } + else + SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low); + return 1; + } + else if (strcasecmp("oscillate_counter", argv[1]) == 0) { + SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter); + } + else if(strcasecmp("osc", argv[1]) == 0 || strcasecmp("oscillate", argv[1]) == 0) { + if (argc > 2) { + if (strcasecmp("list", argv[2]) == 0) { + /* TODO: list the osc params */ + SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count); + SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter); + SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low); + SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high); return 1; } - /* Reject Infinity and Not a Number (NaN) */ - switch (fpclassify(fSoft)) { - case FP_ZERO: - case FP_NORMAL: - break; - default: - self->errorCode = BADPAR; - SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]); + if (strcasecmp("start", argv[2]) == 0) { + float fFixed; + double fHard; + MotorGetPar(self->pMot, "fixed", &fFixed); + if (fFixed >= 0) { + SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name); return 1; + } + if (!getHardFromSoft(self, pCon, self->oscillate_low, &fHard)) { + SCPrintf(pCon, eWarning, "Oscillate low point is in error"); + return 1; + } + if (!getHardFromSoft(self, pCon, self->oscillate_high, &fHard)) { + SCPrintf(pCon, eWarning, "Oscillate high point is in error"); + return 1; + } + if (fabs(self->oscillate_high - motPosit(self)) < fabs(self->oscillate_low - motPosit(self))) + self->oscillate_up = true; + else + self->oscillate_up = false; + self->doOscillate = true; + self->oscillate_init = true; + self->oscillate_counter = 0; + state_cmd_execute(self, CMD_RUN); + return 1; } - MotorGetPar(self->pMot, "softlowerlim", &fLower); - MotorGetPar(self->pMot, "softupperlim", &fUpper); - MotorGetPar(self->pMot, "softzero", &fZero); - MotorGetPar(self->pMot, "sign", &fSign); - MotorGetPar(self->pMot, "fixed", &fFixed); - if (fFixed >= 0) { - SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name); - } else if (fSoft > fUpper) { - SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s", - fSoft, fUpper, self->name); - } else if (fSoft < fLower) { - SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s", - fSoft, fLower, self->name); - } else { - double fHard = fSoft; - fHard += fZero; - fHard *= fSign; - if (fHard > self->fUpper) { - SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s", - fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name); - } else if (fHard < self->fLower) { - SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s", - fSoft, self->fLower * fSign + fZero, self->fLower, self->name); + if (strcasecmp("stop", argv[2]) == 0) { + self->doOscillate = false; + return 1; + } + if (strcasecmp("halt", argv[2]) == 0) { + self->doOscillate = false; + state_cmd_execute(self, CMD_HALT); + return 1; + } + } else { + SCPrintf(pCon, eStatus, "%s.oscillation = %s", self->name, self->doOscillate ? "on" : "off"); + return 1; + } + } + else if(strcasecmp("driver", argv[1]) == 0) { + if(strcasecmp("run", argv[2]) == 0) { + if (argc > 2) { + float fLower, fUpper, fZero, fSign, fFixed, fLim; + double fSoft; + char *endPtr; + errno = 0; + fSoft = strtod(argv[3], &endPtr); + if (endPtr == argv[3] || errno == ERANGE) { + self->errorCode = BADPAR; + SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]); + return 1; + } + /* Reject Infinity and Not a Number (NaN) */ + switch (fpclassify(fSoft)) { + case FP_ZERO: + case FP_NORMAL: + break; + default: + self->errorCode = BADPAR; + SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]); + return 1; + } + MotorGetPar(self->pMot, "softlowerlim", &fLower); + MotorGetPar(self->pMot, "softupperlim", &fUpper); + MotorGetPar(self->pMot, "softzero", &fZero); + MotorGetPar(self->pMot, "sign", &fSign); + MotorGetPar(self->pMot, "fixed", &fFixed); + if (fFixed >= 0) { + SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name); + } else if (fSoft > fUpper) { + SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s", + fSoft, fUpper, self->name); + } else if (fSoft < fLower) { + SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s", + fSoft, fLower, self->name); } else { - self->fTarget = fHard; - SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard); - state_cmd_execute(self, CMD_RUN); + double fHard = fSoft; + fHard += fZero; + fHard *= fSign; + if (fHard > self->fUpper) { + SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s", + fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name); + } else if (fHard < self->fLower) { + SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s", + fSoft, self->fLower * fSign + fZero, self->fLower, self->name); + } else { + self->fTarget = fHard; + SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard); + self->doOscillate = false; + state_cmd_execute(self, CMD_RUN); + } } } + else { + char *txt = NULL; + if (OKOK == self->driver_status) + txt = "OKOK"; + else if (HWIdle == self->driver_status) + txt = "HWIdle"; + else if (HWBusy == self->driver_status) + txt = "HWBusy"; + else if (HWFault == self->driver_status) + txt = "HWFault"; + else if (HWPosFault == self->driver_status) + txt = "HWPosFault"; + else if (HWCrash == self->driver_status) + txt = "HWCrash"; + else if (NOMEMORY == self->driver_status) + txt = "NOMEMORY"; + else if (HWNoBeam == self->driver_status) + txt = "HWNoBeam"; + else if (HWPause == self->driver_status) + txt = "HWPause"; + else if (HWWarn == self->driver_status) + txt = "HWWarn"; + else if (HWRedo == self->driver_status) + txt = "HWRedo"; + else + txt = "HWUnknown"; + SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt); + } + return 1; } - else { - char *txt = NULL; - if (OKOK == self->driver_status) - txt = "OKOK"; - else if (HWIdle == self->driver_status) - txt = "HWIdle"; - else if (HWBusy == self->driver_status) - txt = "HWBusy"; - else if (HWFault == self->driver_status) - txt = "HWFault"; - else if (HWPosFault == self->driver_status) - txt = "HWPosFault"; - else if (HWCrash == self->driver_status) - txt = "HWCrash"; - else if (NOMEMORY == self->driver_status) - txt = "NOMEMORY"; - else if (HWNoBeam == self->driver_status) - txt = "HWNoBeam"; - else if (HWPause == self->driver_status) - txt = "HWPause"; - else if (HWWarn == self->driver_status) - txt = "HWWarn"; - else if (HWRedo == self->driver_status) - txt = "HWRedo"; - else - txt = "HWUnknown"; - SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt); - } + } + else if(strcasecmp("status", argv[1]) == 0) { + char *txt = NULL; + if (OKOK == self->driver_status) + txt = "OKOK"; + else if (HWIdle == self->driver_status) + txt = "HWIdle"; + else if (HWBusy == self->driver_status) + txt = "HWBusy"; + else if (HWFault == self->driver_status) + txt = "HWFault"; + else if (HWPosFault == self->driver_status) + txt = "HWPosFault"; + else if (HWCrash == self->driver_status) + txt = "HWCrash"; + else if (NOMEMORY == self->driver_status) + txt = "NOMEMORY"; + else if (HWNoBeam == self->driver_status) + txt = "HWNoBeam"; + else if (HWPause == self->driver_status) + txt = "HWPause"; + else if (HWWarn == self->driver_status) + txt = "HWWarn"; + else if (HWRedo == self->driver_status) + txt = "HWRedo"; + else + txt = "HWUnknown"; + SCPrintf(pCon, eValue, "%s.status = %s", self->name, txt); return 1; } else if(strcasecmp("halt", argv[1]) == 0) {