lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented Homing on the EuroMove driver

Browse Source
This commit is contained in:
koennecke
2020-02-10 10:00:56 +01:00
parent 6d823e2265
commit 12249d5471
4 changed files with 210 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
iocBoot/iocsinqEPICS/euromovetest.cmd
View File

@ -4,12 +4,9 @@ require sinq,koennecke
epicsEnvSet("TOP","/afs/psi.ch/project/sinqdev/sinqepicsapp/iocBoot/iocsinqEPICS")
epicsEnvSet("BASE","/afs/psi.ch/project/sinqdev/sinqepicsapp")
epicsEnvSet("dbPATH","${EPICS_BASE}/dbd:${ASYN}/dbd:${MOTOR}/dbd")
cd ${TOP}
## Register all support components
dbLoadDatabase "../../dbd/sinqEPICS.dbd"
drvAsynIPPortConfigure("serial1", "amor-ts:3002",0,0,0)

11
sinqEPICSApp/Db/motorMessage.db
View File

@ -6,4 +6,13 @@ record(waveform, "$(P)$(M)-MsgTxt") {
field(FTVL, "CHAR")
field(NELM, "80")
field(SCAN, "I/O Intr")
}
}
record(ao,"$(P)m$(N)-Resolution"){
field(DESC,"m$(N) Resolution")
field(DOL,"$(P)m$(N).MRES CP MS")
field(OMSL,"closed_loop")
field(DTYP,"asynFloat64")
field(OUT,"@asyn($(PORT),$(N))MOTOR_RESOLUTION")
field(PREC,"3")
}

225
sinqEPICSApp/src/EuroMoveDriver.cpp
View File

@ -14,6 +14,7 @@ GPIB or USB. The controller is fairly simple. It can control quite a number of m
#include <math.h>
#include <ctype.h>
#include <time.h>
#include <unistd.h>
#include <iocsh.h>
#include <epicsThread.h>
@ -192,6 +193,44 @@ asynStatus EuroMoveController::transactController(int axisNo,char command[COMLEN
return status;
}
asynStatus EuroMoveController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
EuroMoveAxis *pAxis = NULL;
static const char *functionName = "EuroMoveController::writeFloat64";
pAxis = (EuroMoveAxis *)this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}
/* Set the parameter and readback in the parameter library. */
status = pAxis->setDoubleParam(function, value);
errlogPrintf("%s, reason %d, value %f\n", functionName, function, value);
// TODO: somethign is really shitty here: lowLimit and highLimit cannot be on the command
if (function == motorResolution_) {
if(value > .0){
pAxis->resolution = (int)(1./value);
} else {
pAxis->resolution = 1;
}
}
//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeFloat64(pasynUser, value);
return status;
}
// These are the EuroMoveAxis methods
/** Creates a new EuroMoveAxis object.
@ -236,7 +275,7 @@ asynStatus EuroMoveAxis::move(double position, int relative, double minVelocity,
if (relative) {
position += this->position;
}
homing = 0;
homingStatus = HomeIdle;
sprintf(command,"G%d=%d", motNo, (int)position);
status = pC_->transactController(motNo,command,reply);
next_poll = -1;
@ -247,10 +286,29 @@ asynStatus EuroMoveAxis::home(double minVelocity, double maxVelocity, double acc
{
asynStatus status = asynSuccess;
//static const char *functionName = "EuroMoveAxis::home";
char command[COMLEN], reply[COMLEN];
/*
I do not know if this controller actually can do homing
The EuroMove controller does not have a built in homing mechanism. We simulate it by:
1). Running the motor in the appropriate end swicth at high speed
2) Stepping back three units
3) Run the motor again in the end switch at low speed for better accuracy
4) Set the limit position with the <I> command
The state of this operation is maintained in the homingStatus variable
*/
homingDirection = forwards;
homingStatus = HomeFastRun;
if(homingDirection){
sprintf(command,"H%d=03", motNo);
} else {
sprintf(command,"H%d=07", motNo);
}
status = pC_->transactController(motNo,command,reply);
next_poll = -1;
return status;
}
@ -339,6 +397,7 @@ asynStatus EuroMoveAxis::poll(bool *moving)
asynStatus comStatus = asynSuccess;
char command[COMLEN], reply[COMLEN];
unsigned int axStatus;
double backStep, backPos, limit;
// protect against excessive polling
if(time(NULL) < next_poll){
@ -371,47 +430,135 @@ asynStatus EuroMoveAxis::poll(bool *moving)
/* errlogPrintf("Axis %d, status reply %s, position %d\n", axisNo_, reply, position); */
sscanf(reply, "%x", &axStatus);
if((axStatus & 128) > 0) { // test bit 8
*moving = true;
if(homingStatus == HomeIdle){
if((axStatus & 128) > 0) { // test bit 8
*moving = true;
next_poll = -1;
setIntegerParam(pC_->motorStatusDone_, false);
} else {
*moving = false;
next_poll = time(NULL)+IDLEPOLL;
setIntegerParam(pC_->motorStatusDone_, true);
}
/* Testing limit switches */
if((axStatus & 2) > 0){ // bit 2
setIntegerParam(pC_->motorStatusHighLimit_, true);
errlogPrintf("HighLimit detected on %d\n", motNo);
sendStop();
} else {
setIntegerParam(pC_->motorStatusHighLimit_, false);
}
if((axStatus & 1) > 0){ // bit 1
setIntegerParam(pC_->motorStatusLowLimit_, true);
errlogPrintf("LowLimit detected on %d\n", motNo);
sendStop();
} else {
setIntegerParam(pC_->motorStatusLowLimit_, false);
}
/* there could be errors reported in the motor status */
if((axStatus & 8) > 0){ // bit 4
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Internal timeout detected");
errlogPrintf("Internal timeout detected on %d\n", motNo);
comStatus = asynError;
}
if((axStatus & 4) > 0){ // bit 3
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Encoding anomaly");
errlogPrintf("Encoding anomaly on %d\n", motNo);
comStatus = asynError;
}
} else {
next_poll = -1;
setIntegerParam(pC_->motorStatusDone_, false);
} else {
*moving = false;
next_poll = time(NULL)+IDLEPOLL;
setIntegerParam(pC_->motorStatusDone_, true);
*moving = true;
switch(homingStatus){
case HomeIdle:
// handled above: this is here to silence the compiler
break;
case HomeFastRun:
// errlogPrintf("HomeFastRun\n");
if(axStatus & 2 || axStatus & 1){
sendStop();
homingStatus = HomeFastStop;
}
break;
case HomeFastStop:
// errlogPrintf("HomeFastStop\n");
if((axStatus & 128) == 0){
backStep = 3 * resolution;
if(homingDirection){
backPos = position - backStep;
} else {
backPos = position + backStep;
}
sprintf(command,"G%d=%d", motNo, (int)backPos);
comStatus = pC_->transactController(motNo,command,reply);
if(comStatus == asynError){
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Homing problem when back stepping");
errlogPrintf("Homing problem when back stepping: %s not accepted\n", command);
*moving = false;
setIntegerParam(pC_->motorStatusAtHome_, true);
homingStatus = HomeIdle;
setIntegerParam(pC_->motorStatusDone_, true);
goto skip;
}
usleep(200);
homingStatus = HomeBackStep;
}
break;
case HomeBackStep:
//errlogPrintf("HomeBackStep\n");
if((axStatus & 128) == 0){
homingStatus = HomeSlowRun;
if(homingDirection){
sprintf(command,"H%d=01", motNo);
} else {
sprintf(command,"H%d=05", motNo);
}
comStatus = pC_->transactController(motNo,command,reply);
if(comStatus == asynError){
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Homing problem slow running");
errlogPrintf("Homing problem when slow running: %s not accepted\n", command);
*moving = false;
setIntegerParam(pC_->motorStatusAtHome_, true);
homingStatus = HomeIdle;
setIntegerParam(pC_->motorStatusDone_, true);
goto skip;
}
}
break;
case HomeSlowRun:
//errlogPrintf("HomeSlowRun\n");
if(axStatus & 2 || axStatus & 1){
sendStop();
homingStatus = HomeSlowStop;
}
break;
case HomeSlowStop:
//errlogPrintf("HomeSlowStop\n");
if((axStatus & 128) == 0) {
if(homingDirection) {
pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&limit);
} else {
pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&limit);
}
setPosition(limit);
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
next_poll = time(NULL)+IDLEPOLL;
setIntegerParam(pC_->motorStatusDone_, true);
setIntegerParam(pC_->motorStatusAtHome_, true);
homingStatus = HomeIdle;
setDoubleParam(pC_->motorPosition_,(double)limit);
}
}
}
/* Testing limit switches */
if((axStatus & 2) > 0){ // bit 2
setIntegerParam(pC_->motorStatusHighLimit_, true);
errlogPrintf("HighLimit detected on %d\n", motNo);
sendStop();
} else {
setIntegerParam(pC_->motorStatusHighLimit_, false);
}
if((axStatus & 1) > 0){ // bit 1
setIntegerParam(pC_->motorStatusLowLimit_, true);
errlogPrintf("LowLimit detected on %d\n", motNo);
sendStop();
} else {
setIntegerParam(pC_->motorStatusLowLimit_, false);
}
/* there could be errors reported in the motor status */
if((axStatus & 8) > 0){ // bit 4
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Internal timeout detected");
errlogPrintf("Internal timeout detected on %d\n", motNo);
comStatus = asynError;
}
if((axStatus & 4) > 0){ // bit 3
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Encoding anomaly");
errlogPrintf("Encoding anomaly on %d\n", motNo);
comStatus = asynError;
}
skip:
callParamCallbacks();
return comStatus;

16
sinqEPICSApp/src/EuroMoveDriver.h
View File

@ -12,6 +12,15 @@ January 2020
#define COMLEN 80
typedef enum {
HomeIdle,
HomeFastRun,
HomeFastStop,
HomeBackStep,
HomeSlowRun,
HomeSlowStop,
} HomingStatus;
class EuroMoveAxis : public SINQAxis
{
public:
@ -35,9 +44,11 @@ private:
time_t next_poll;
int motNo; // motor number in the EuroMove controller
int position;
int homing;
HomingStatus homingStatus;
int homingDirection; // 1 = forward, 0 backwards
asynStatus sendStop();
friend class EuroMoveController;
int resolution;
friend class EuroMoveController;
};
class EuroMoveController : public SINQController {
@ -47,6 +58,7 @@ public:
void report(FILE *fp, int level);
EuroMoveAxis* getAxis(asynUser *pasynUser);
EuroMoveAxis* getAxis(int axisNo);
asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);
friend class EuroMoveAxis;
private: