diff --git a/make_gen b/make_gen index 90e6e49..bffd092 100644 --- a/make_gen +++ b/make_gen @@ -4,7 +4,10 @@ # Mark Koennecke, June 2003 #------------------------------------------------------------------------- .SUFFIXES: -.SUFFIXES: .c .o .f +.SUFFIXES: .c .o .f .tc + +.tc.c: + tjxp $*.tc $*.c OBJ=psi.o buffer.o ruli.o dmc.o nxsans.o nextrics.o sps.o pimotor.o \ pipiezo.o sanswave.o faverage.o fowrite.o amor2t.o nxamor.o \ @@ -18,7 +21,10 @@ OBJ=psi.o buffer.o ruli.o dmc.o nxsans.o nextrics.o sps.o pimotor.o \ el737hpv2driv.o swmotor2.o tricssupport.o amorcomp.o \ $(MZOBJ) amordrive.o amorset.o tcpdornier.o sinqhttp.o\ dgrambroadcast.o sinq.o tabledrive.o tcpdocho.o julcho.o \ - ritastorage.o poldizug.o audinelib.o delcam.o el737hpdrivsps.o + ritastorage.o poldizug.o audinelib.o delcam.o el737hpdrivsps.o \ + rebin.o sanslirebin.o + +.SECONDARY.: sanslirebin.c MZOBJ=fsm.o logger.o sugar.o pardef.o ease.o strobj.o oxinst.o logreader.o \ ipsdriv.o ilmdriv.o itcdriv.o ighdriv.o euro2kdriv.o modbus.o arrobj.o \ diff --git a/poldizug.c b/poldizug.c index 4365ec7..14d93d8 100644 --- a/poldizug.c +++ b/poldizug.c @@ -7,22 +7,26 @@ * * copyright: see file COPYRIGHT * - * Mark Koennecke, October 2006 + * Mark Koennecke, October 2006(started) + * - May 2007(The device was actually available) */ #include #include -#include /*===============================================================*/ #define FORCE 0 #define ZUGPOS 1 +#define REGEL 10 +#define OFF 20 /*---------------------------------------------------------------*/ typedef struct { pObjectDescriptor pDes; pIDrivable pDriv; prs232 controller; int iMode; + int state; float maxForce, minForce; float maxPos, minPos; + int velocity; }PZR, *pPZR; /*--------------------------------------------------------------*/ static int PZRCommand(prs232 controller, SConnection *pCon, @@ -37,7 +41,14 @@ static int PZRCommand(prs232 controller, SConnection *pCon, for(i = 0; i < 3; i++){ status = transactRS232(controller,command, strlen(command), reply, replyLen); - if(status == 1){ + if(status > 0 ){ + if(strstr(reply,"ERR") != NULL){ + if(pCon != NULL){ + snprintf(buffer,511,"ERROR: device reported %s", reply); + SCWrite(pCon,buffer,eError); + return HWFault; + } + } return 1; } /* @@ -85,7 +96,7 @@ static int PZRHalt(void *data) { self = (pPZR)data; - PZRCommand(self->controller,NULL,"stop\n",buffer,79); + PZRCommand(self->controller,NULL,"stop\r\n",buffer,79); return OKOK; } @@ -131,18 +142,54 @@ static long PZRSetValue(void *data, SConnection *pCon, float val){ self = (pPZR)data; - if(self->iMode == FORCE){ - snprintf(command,79,"wr force %f\n", val); - } else { - snprintf(command,79,"wr vel %f\n", val); - } - status = PZRCommand(self->controller, pCon, command, reply, 79); - if(status == 1){ + switch(self->iMode){ + case FORCE: + if(self->state == OFF){ + snprintf(command,79,"wr vel %d\r\n", self->velocity); + status = PZRCommand(self->controller, pCon, command, reply, 79); + if(status != 1){ + return HWFault; + } + snprintf(command,79,"wr force %f\r\n", val); + status = PZRCommand(self->controller, pCon, command, reply, 79); + if(status != 1){ + return HWFault; + } + status = PZRCommand(self->controller, pCon, "wr mode 1\r\n", reply, 79); + if(status != 1){ + return HWFault; + } + self->state = REGEL; + return OKOK; + } else { + snprintf(command,79,"wr force %f\r\n", val); + status = PZRCommand(self->controller, pCon, command, reply, 79); + if(status != 1){ + return HWFault; + } + return OKOK; + } + break; + case ZUGPOS: + self->state = OFF; + snprintf(command,79,"wr vel %d\r\n", self->velocity); + status = PZRCommand(self->controller, pCon, command, reply, 79); + if(status != 1){ + return HWFault; + } + snprintf(command,79,"wr pos %f\r\n", val); + status = PZRCommand(self->controller, pCon, command, reply, 79); + if(status != 1){ + return HWFault; + } + status = PZRCommand(self->controller, pCon, "wr mode 2\r\n", reply, 79); + if(status != 1){ + return HWFault; + } return OKOK; - } else { - return HWFault; + break; } - return HWFault; + return OKOK; } /*---------------------------------------------------------------- Checks the status of a running motor. Possible return values @@ -154,42 +201,80 @@ static long PZRSetValue(void *data, SConnection *pCon, float val){ For real motors CheckStatus again shall try hard to fix any issues with the motor ------------------------------------------------------------------*/ +static int BitSet(char d, int n){ + unsigned char e; + + e = d & (1 << n); + if(e == 0){ + return 0; + } else { + return 1; + } +} +/*---------------------------------------------------------------*/ static int PZRCheckStatus(void *data, SConnection *pCon){ pPZR self = NULL; - int status, code; - char command[80], reply[80], bits[1]; + int status, code, i; + char command[80], reply[80], bits; self = (pPZR)data; - snprintf(command,79,"rd stat\n"); + snprintf(command,79,"rd stat\r\n"); status = PZRCommand(self->controller, pCon, command, reply, 79); if(status != 1){ return HWFault; } sscanf(reply,"%x", &code); - bits[0] = (char)code; + bits = (char)code; + + /* + memset(command,0,80); + for(i = 0; i < 8; i++){ + if(BitSet(bits,i)){ + command[i] = '1'; + } else { + command[i] = '0'; + } + } + snprintf(reply,79,"code = %d",code); + strcat(command,reply); + SCWrite(pCon,command,eValue); + */ + /* * check for errors */ - if(BITSET(bits,3)){ - SCWrite(pCon,"ERROR: BANG! Sample broken!",eError); - return HWFault; - } - if(BITSET(bits,4)){ + if(BitSet(bits,4)){ SCWrite(pCon,"ERROR: hit positive limit switch",eError); return HWFault; } - if(BITSET(bits,5)){ + if(BitSet(bits,5)){ SCWrite(pCon,"ERROR: hit negative limit switch",eError); return HWFault; } - if(BITSET(bits,7)){ - SCWrite(pCon,"ERROR: general and overall error",eError); + if(BitSet(bits,6)){ + SCWrite(pCon,"ERROR: max force exceed, manual reset REQUIRED!",eError); return HWFault; } - if(BITSET(bits,2)){ - return HWBusy; - } else { - return HWIdle; + if(BitSet(bits,7)){ + SCWrite(pCon,"ERROR: bit 7 error, ask Marcel what this means",eError); + return HWFault; + } + + switch(self->iMode){ + case FORCE: + if(BitSet(bits,2)){ + return HWIdle; + } else { + return HWBusy; + } + break; + case ZUGPOS: + if(BitSet(bits,3)){ + return HWIdle; + } else { + return HWBusy; + } + break; } return HWFault; @@ -200,15 +285,15 @@ static int PZRCheckStatus(void *data, SConnection *pCon){ ------------------------------------------------------------------*/ static float PZRGetValue(void *data, SConnection *pCon){ pPZR self = NULL; - float val = -99999999.99; + float val = -9999.99; int status; char command[80], reply[80]; self = (pPZR)data; if(self->iMode == FORCE){ - snprintf(command,79,"rd force\n"); + snprintf(command,79,"rd force\r\n"); } else { - snprintf(command,79,"rd vel\n"); + snprintf(command,79,"rd pos\r\n"); } status = PZRCommand(self->controller, pCon, command, reply, 79); if(status != 1){ @@ -217,6 +302,22 @@ static float PZRGetValue(void *data, SConnection *pCon){ sscanf(reply,"%f",&val); return val; } +/*---------------------------------------------------------------*/ +static int PZRConnect(pPZR self, SConnection *pCon){ + char error[132], buffer[356]; + int status; + + closeRS232(self->controller); + status = initRS232(self->controller); + if(status < 0){ + getRS232Error(status, error, 131); + snprintf(buffer,255,"ERROR: failed to reconnect with: %s", error); + SCWrite(pCon,buffer,eError); + return 0; + } + SCSendOK(pCon); + return 1; +} /*---------------------------------------------------------------- returns NULL on failure, a new datastructure else ------------------------------------------------------------------*/ @@ -237,7 +338,7 @@ static pPZR PZRMakeObject(char *host, int port){ initRS232(self->controller); setRS232SendTerminator(self->controller,"\n"); setRS232ReplyTerminator(self->controller,"\n"); - setRS232Timeout(self->controller,1000); + setRS232Timeout(self->controller,10000); setRS232Debug(self->controller,1); @@ -286,41 +387,81 @@ static int PoldiReissAction(SConnection *pCon, SicsInterp *pSics, } strtolower(argv[2]); if(strcmp(argv[2],"force") == 0) { - status = PZRCommand(self->controller, pCon, - "wr mode 1\n", reply,79); - if(status == 1){ - self->iMode == FORCE; - SCSendOK(pCon); - return 1; - } else { - return 0; - } + self->iMode = FORCE; + SCSendOK(pCon); + return 1; } else if(strcmp(argv[2],"pos") == 0) { - status = PZRCommand(self->controller, pCon, - "wr mode 0\n", reply,79); - if(status == 1){ - self->iMode == ZUGPOS; - SCSendOK(pCon); - return 1; - } else { - return 0; - } + self->iMode = ZUGPOS; + SCSendOK(pCon); + return 1; } else { SCWrite(pCon,"ERROR: zug mode unknown, allowed force, pos", eError); return 0; } - } - } else { - if(self->iMode == FORCE){ - snprintf(command,79,"%s.mode = force",argv[0]); } else { - snprintf(command,79,"%s.mode = pos",argv[0]); + if(self->iMode == FORCE){ + snprintf(command,79,"%s.mode = force",argv[0]); + } else { + snprintf(command,79,"%s.mode = pos",argv[0]); + } + SCWrite(pCon,command,eValue); + return 1; + } + } else if(strcmp(argv[1],"velocity") == 0) { + if(argc > 2){ + if(!SCMatchRights(pCon,usUser)){ + return 0; + } + self->velocity = atoi(argv[2]); + SCSendOK(pCon); + return 1; + } else { + snprintf(command,70,"%s.velocity = %d", argv[0], self->velocity); + SCWrite(pCon,command,eValue); + return 1; + } + } else if(strcmp(argv [1],"connect") == 0){ + return PZRConnect(self,pCon); + } else if(strcmp(argv[1],"off") == 0) { + status = PZRCommand(self->controller, pCon, "wr mode 0\r\n", reply, 79); + self->state = OFF; + if(status == 1) { + SCSendOK(pCon); + return 1; + } else { + return 0; + } + } else if(strcmp(argv[1],"poslimits") == 0){ + if(argc > 3){ + if(!SCMatchRights(pCon,usMugger)){ + return 0; + } + self->minPos = atof(argv[2]); + self->maxPos = atof(argv[3]); + SCSendOK(pCon); + return 1; + } else { + snprintf(command,80,"pzr.poslimits = %f,%f", self->minPos, self->maxPos); + SCWrite(pCon,command,eValue); + return 1; + } + } else if(strcmp(argv[1],"forcelimits") == 0){ + if(argc > 3){ + if(!SCMatchRights(pCon,usMugger)){ + return 0; + } + self->minForce = atof(argv[2]); + self->maxForce = atof(argv[3]); + SCSendOK(pCon); + return 1; + } else { + snprintf(command,80,"pzr.forcelimits = %f,%f", self->minForce, self->maxForce); + SCWrite(pCon,command,eValue); + return 1; } - SCWrite(pCon,command,eValue); - return 1; } - } + } val = self->pDriv->GetValue(self,pCon); snprintf(command,79,"%s = %f", argv[0], val); SCWrite(pCon,command,eValue); @@ -346,9 +487,12 @@ int MakePoldiReiss(SConnection *pCon, SicsInterp *pSics, self->minForce = atof(argv[4]); self->maxForce = atof(argv[5]); self->minPos = atof(argv[6]); - self->maxPos = atof(argv[7]); + self->maxPos = atof(argv[7]); + self->iMode = FORCE; + self->state = OFF; + self->velocity = 10; PZRCommand(self->controller, pCon, - "wr mode 1\n", reply,79); + "wr mode 0\r\n", reply,79); AddCommand(pSics,argv[1], PoldiReissAction, diff --git a/psi.c b/psi.c index ef5be59..6888b66 100644 --- a/psi.c +++ b/psi.c @@ -6,7 +6,7 @@ copyright: see file COPYRIGHT - Mark Koennecke, June 2003 + Mark Koennecke, June 2003 - May 2007 -----------------------------------------------------------------------*/ #include #include @@ -69,6 +69,9 @@ extern int JulChoFactory(SConnection *pCon, SicsInterp *pSics, void *pData, /* from ritastorage.c */ extern int MakeRitaFix(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]); +/* from sanslirebin.c */ +extern int MakeSansliRebin(SConnection *pCon, SicsInterp *pSics, void *pData, + int argc, char *argv[]); /*--------------------------------------------------------------------------*/ void SiteInit(void) { @@ -124,6 +127,7 @@ static void AddPsiCommands(SicsInterp *pInter){ AddCommand(pInter,"MakeJulCho",JulChoFactory,NULL,NULL); AddCommand(pInter,"MakeRitaFix",MakeRitaFix,NULL,NULL); AddCommand(pInter,"MakePoldiReiss",MakePoldiReiss,NULL,NULL); + AddCommand(pInter,"MakeSansliRebin",MakeSansliRebin,NULL,NULL); /* AddCommand(pInter,"MakeDifrac",MakeDifrac,NULL,NULL); */ @@ -147,6 +151,7 @@ static void RemovePsiCommands(SicsInterp *pSics){ RemoveCommand(pSics,"MakeJulCho"); RemoveCommand(pSics,"MakeRitaFix"); RemoveCommand(pSics,"MakePoldiReiss"); + RemoveCommand(pSics,"MakeSansliRebin"); /* RemoveCommand(pSics,"MakeDifrac"); */ diff --git a/rebin.c b/rebin.c new file mode 100644 index 0000000..1dc5145 --- /dev/null +++ b/rebin.c @@ -0,0 +1,142 @@ +/** + * This is a couple of functions to do rebinning. This is neeeded when + * you have a number of data points with random coordinates and you wish to + * get this back onto a regular grid for better handling. + * + * copyright: see file COPYRIGHT + * + * Mark Koennecke, May 2007 + */ + #include +#include "rebin.h" +/*---------------------------------------------------------------------------*/ +static int checkPoint(pNXDS target, double x, double y){ + int maxx, maxy; + + maxx = getNXDatasetDim(target,0); + maxy = getNXDatasetDim(target,1); + + if(x < .0 || y < .0 || x > maxx -1 || y > maxy -1) { + return 0; + } + return 1; +} +/*--------------------------------------------------------------------------*/ +static void addNXDatasetValue(pNXDS target, int iPos[], double value){ + double val; + + val = value + getNXDatasetValue(target,iPos); + putNXDatasetValue(target,iPos,val); +} +/*---------------------------------------------------------------------------*/ +static void roundPoint(pNXDS target, double x, double y, double value){ + int iDim[2]; + + iDim[0] = (int)floor(x + .5); + iDim[1] = (int)floor(y + 0.5); + + addNXDatasetValue(target,iDim,value); +} +/*---------------------------------------------------------------------------- + * I am using the squares here, no need to do the sqrt which would give the + * actual distance. + * ---------------------------------------------------------------------------*/ +static double dist(double x1, double y1, double x2, double y2){ + double dx, dy; + + dx = x1 - x2; + dy = y1 -y2; + return dx*dx + dy*dy; +} +/*----------------------------------------------------------------------------- + * This tries to distribute value over all of the four neighbouring points + * weighted with the distance to them. +-------------------------------------------------------------------------------*/ +static void distribute(pNXDS target, double x, double y, double value){ + double lldist, lrdist, uldist, urdist, totalDist, ix, iy, frac; + int iPos[2]; + + ix = floor(x); + iy = floor(y); + lldist = dist(x,y,ix,iy); + if(lldist < .01){ + iPos[0] = ix; + iPos[1] = iy; + addNXDatasetValue(target,iPos,value); + return; + } + + ix += 1; + lrdist = dist(x,y,ix,iy); + if(lrdist < .01){ + iPos[0] = ix; + iPos[1] = iy; + addNXDatasetValue(target,iPos,value); + return; + } + + iy += 1; + urdist = dist(x,y,ix,iy); + if(urdist < .01){ + iPos[0] = ix; + iPos[1] = iy; + addNXDatasetValue(target,iPos,value); + return; + } + + ix = floor(x); + uldist = dist(x,y,ix,iy); + if(uldist < .01){ + iPos[0] = ix; + iPos[1] = iy; + addNXDatasetValue(target,iPos,value); + return; + } + + lldist = 1./lldist; + lrdist = 1./lrdist; + urdist = 1./urdist; + uldist = 1./uldist; + totalDist = lldist + lrdist + urdist + uldist; + + if(totalDist < .01){ + iPos[0] = (int)floor(x); + iPos[1] = (int)floor(y); + addNXDatasetValue(target,iPos,value); + return; + } + + iPos[0] = (int)floor(x); + iPos[1] = (int)floor(y); + frac = (lldist/totalDist)*value; + addNXDatasetValue(target,iPos,frac); + + iPos[0] += 1; + frac = (lrdist/totalDist)*value; + addNXDatasetValue(target,iPos,frac); + + iPos[1] += 1; + frac = (urdist/totalDist)*value; + addNXDatasetValue(target,iPos,frac); + + iPos[0] = (int)floor(x); + frac = (uldist/totalDist)*value; + addNXDatasetValue(target,iPos,frac); +} +/*---------------------------------------------------------------------------*/ +int rebinPoint2D(pNXDS target, double x, double y, double value, int method){ + if(!checkPoint(target,x,y)){ + return 0; + } + switch(method){ + case TRILINEAR: + distribute(target,x,y,value); + break; + case ROUND: + roundPoint(target,x,y,value); + break; + default: + return 0; + } + return 1; +} diff --git a/rebin.h b/rebin.h new file mode 100644 index 0000000..8dca432 --- /dev/null +++ b/rebin.h @@ -0,0 +1,27 @@ +/** + * This is a couple of functions to do rebinning. This is neeeded when + * you have a number of data points with random coordinates and you wish to + * get this back onto a regular grid for better handling. + * + * copyright: see file COPYRIGHT + * + * Mark Koennecke, May 2007 + */ +#ifndef REBIN_H_ +#define REBIN_H_ +#include + +#define TRILINEAR 1 +#define ROUND 2 +/** + * rebin a point + * @param target The target dataset to rebin too + * @param x The x coordiante of the point + * @param y The y coordinate of the point + * @param value The value of the point + * @param method The method to use for interpolation + * @return 1 on success, 0 else + */ +int rebinPoint2D(pNXDS target, double x, double y, double value, int method); + +#endif /*REBIN_H_*/ diff --git a/sanslirebin.tc b/sanslirebin.tc new file mode 100644 index 0000000..026c4e7 --- /dev/null +++ b/sanslirebin.tc @@ -0,0 +1,71 @@ +/** + * A special module for doing SANSLI rebinning. This is a fudge to make + * the data from the new detector electronics look like the old one. + * + * copyright: see file COPYRIGHT + * + * Mark Koennecke, May 2007 + */ + <%! source ../sicstemplates.tcl %> +<% stdIncludes %> +#include +#include +#include "rebin.h" +/*---------------------------------------------------------------------------*/ +<% makeSicsFunc SansliRebin%>{ + pSICSData target = NULL, hmData = NULL; + pNXDS dataset = NULL; + int iDim[2], ix, iy, pos, ival; + double x, y, val; + + <% testNoPar 2 4 %> + + hmData = (pSICSData)FindCommandData(pSics,argv[1],"SICSData"); + if(hmData == NULL){ + SCWrite(pCon,"ERROR: histogram memory data not found",eError); + return 0; + } + target = (pSICSData)FindCommandData(pSics,argv[2],"SICSData"); + if(target == NULL){ + SCWrite(pCon,"ERROR: target sicsdata not found",eError); + return 0; + } + iDim[0] = 128; + iDim[1] = 128; + dataset = createNXDataset(2,NX_FLOAT64, iDim); + if(dataset == NULL){ + SCWrite(pCon,"ERROR: out of memory allocating temporary data",eError); + return 0; + } + + + for(ix = 113; ix < 415; ix++){ + for(iy = 153; iy < 363; iy++){ + x = ((double)ix - 113.)/2.359; + y = ((double)iy - 153.)/1.640; + getSICSDataInt(hmData,512*iy + ix,&ival); + val = (double)ival; + rebinPoint2D(dataset,x,y,val,TRILINEAR); + } + } + + pos = 0; + clearSICSData(target); + if(argc > 3) { + setSICSDataInt(target,0,128); + setSICSDataInt(target,1,128); + pos = 2; + } + for(ix = 0; ix < 128*128; ix++, pos++){ + setSICSDataInt(target,pos,(int)floor(dataset->u.dPtr[ix] +.5)); + } + dropNXDataset(dataset); + SCSendOK(pCon); + return 1; +} +/*---------------------------------------------------------------------------*/ +<% makeSicsFunc MakeSansliRebin%>{ + AddCommand(pSics,"sanslirebin", SansliRebin,NULL,NULL); + SCSendOK(pCon); + return 1; +} diff --git a/sinqhmdriv.c b/sinqhmdriv.c index 5875e52..3357a69 100644 --- a/sinqhmdriv.c +++ b/sinqhmdriv.c @@ -839,11 +839,12 @@ pCon); } /* stop DAQ, if still active */ + /* if(iDaq == 1) { SINQHMStopDAQ(pInternal->pMaster); } - + */ /* do only counter, histograms are read on demand */ if(pInternal->pCounter != NULL) diff --git a/slsmagnet.c b/slsmagnet.c index f03a812..47764bc 100644 --- a/slsmagnet.c +++ b/slsmagnet.c @@ -14,6 +14,19 @@ Adaptiert auf neue DSP Software version, Mark Koennecke, August 2005 + Fixed some bugs and added resetting of error conditions: + Mark Koennecke, May 2007 + + There is another scheme for resetting controller erros which might need + to be implemented: + 1) switch the controller off + 2) Write 0 to 0x21 + 3) switch on + 4) drive to target + This reboots the controller. Lukas Tanner does not recommend to do this + automatically. This is why it is not implemented yet. This reboot anyway + is not as hard as a full power cycle. + Copyright: see copyright.h ----------------------------------------------------------------------------*/ #include @@ -209,7 +222,7 @@ static int communicateSLS(mkChannel *pSock, char msg[6], char reply[6]) err = (int)reply[5]; if(err != 0){ pMe->iError = -7000 - err; - return pMe->iError; + return 0; } return 1; @@ -847,7 +860,7 @@ static void slsdspCodeToText(int code, char *text, int textlen){ { if(msg[i] != reply[i]) { - pMe->iError = BADECHO; + pMe->iError = BADECHO; return BADECHO; } } @@ -880,15 +893,42 @@ static void slsdspCodeToText(int code, char *text, int textlen){ static int SLSFix(pEVDriver self, int iError) { pSLSDriv pMe = NULL; - int iRet, i; + int iRet, i, ival; char buffer[80]; + char msg[6], reply[6]; + float fVal = .0; assert(self); pMe = (pSLSDriv )self->pPrivate; assert(pMe); if(iError < -7000){ - return DEVFAULT; + /* + * This logic tries to reset the error code. Then it reads the + * error back. Serious errors persist and must be fixed by magnet + * technicians. But some can be cleared.... + */ + msg[0] = DSPWRITE; + msg[1] = 0x29; + ival = double2DSPfloat((double)fVal); + memcpy(msg+2, &ival,4); + communicateSLS(pMe->pSock,msg,reply); + /* + * read again: if persists: this is really, really bad + */ + msg[0] = DSPREAD; + msg[1] = 0x29; + iRet = communicateSLS(pMe->pSock,msg,reply); + if(iRet < 0) + { + return DEVFAULT; + } + ival = (int)reply[5]; + if(ival != 0){ + return DEVFAULT; + } else { + return DEVREDO; + } } switch(iError) diff --git a/tabledrive.c b/tabledrive.c index 9dcb992..9ebbbd7 100644 --- a/tabledrive.c +++ b/tabledrive.c @@ -104,9 +104,9 @@ static int findOrientingMotor(pTableDrive self, ptdMotor moti){ } /*-------------------------------------------------------------------------*/ static float locatePosition(tdMotor moti, SConnection *pCon){ - int status; + int status, begin = 1; float value; - double diff; + double diff; tdEntry lower, upper; status = MotorGetSoftPosition(moti.pMot,pCon,&value); @@ -116,36 +116,43 @@ static float locatePosition(tdMotor moti, SConnection *pCon){ status = LLDnodePtr2First(moti.table); while(status != 0){ LLDnodeDataTo(moti.table,&lower); - status = LLDnodePtr2Next(moti.table); - if(status != 0) { - LLDnodeDataTo(moti.table,&upper); - } else { - /* - * end of table - */ - return lower.tablePos; - } - /** - * we have to deal with ascending and descending tables. This is - * why we have to have some funny logic here - */ - if(upper.position > lower.position){ - /* - * ascending table - */ - if(value >= lower.position && value < upper.position){ - diff = upper.position - lower.position; - return lower.tablePos + (value - lower.position)/diff; - } - } else { - /* - * descending table - */ - if(value <= lower.position && value > upper.position){ - diff = lower.position - upper.position; - return lower.tablePos + ABS(value - lower.position)/diff; - } - } + status = LLDnodePtr2Next(moti.table); + if(status != 0) { + LLDnodeDataTo(moti.table,&upper); + } else { + /* + * end of table + */ + return lower.tablePos; + } + /** + * we have to deal with ascending and descending tables. This is + * why we have to have some funny logic here + */ + if(upper.position > lower.position){ + /* + * ascending table + */ + if(value >= lower.position && value < upper.position){ + diff = upper.position - lower.position; + return lower.tablePos + (value - lower.position)/diff; + } + if(begin == 1 && value < lower.position) { + return lower.tablePos; + } + } else { + /* + * descending table + */ + if(value <= lower.position && value > upper.position){ + diff = lower.position - upper.position; + return lower.tablePos + ABS(value - lower.position)/diff; + } + if(begin == 1 && value > lower.position){ + return lower.tablePos; + } + } + begin = 0; } return -999.99; } @@ -339,10 +346,10 @@ static int runToNextStep(pTableDrive self, SConnection *pCon, while(status != 0){ LLDnodeDataTo(self->motorTable,&moti); target = findTarget(moti,value); - test = MotorRun(moti.pMot,pCon,target); - if(test != OKOK){ - return test; - } + test = MotorRun(moti.pMot,pCon,target); + if(test != OKOK){ + return test; + } status = LLDnodePtr2Next(self->motorTable); } return 1; @@ -384,8 +391,13 @@ static void showPositions(pTableDrive self, SConnection *pCon, status = LLDnodePtr2Next(self->motorTable); } } -/*-------------------------------------------------------------------------*/ -static int TableDriveCheckStatus(void *pData, SConnection *pCon){ +/*------------------------------------------------------------------------- + * This is the old scheme: drive table positions tablestep by tablestep. + * I leave this code here: may be it is useful in another scenario then + * MARS. Then some means must be found to select the table drive + * algorithm. + * -----------------------------------------------------------------------*/ +static int TableDriveCheckStatusOld(void *pData, SConnection *pCon){ pTableDrive self = (pTableDrive)pData; int status; float step; @@ -464,6 +476,179 @@ static int TableDriveCheckStatus(void *pData, SConnection *pCon){ } return HWFault; } +/*=========================================================================== + * This is code for the new scheme of driving MARS tables: + * 1) drive ds and as to 0 + * 2) drive all other motors to targets directly + * 3) drive ds and as to desired positions. + * =========================================================================*/ + #define WAITDSAS 110 + #define WAITOTHER 111 + #define ASDSTO0 200 + #define ASDSTOTARGET 201 +/*-------------------------------------------------------------------------*/ +static int startASDS(pTableDrive self, SConnection *pCon, int target){ + tdMotor moti; + int status; + char name[132]; + float targetValue; + + if(self == NULL || self->motorTable < 0){ + return 0; + } + + status = LLDnodePtr2First(self->motorTable); + while(status != 0){ + LLDnodeDataTo(self->motorTable,&moti); + strncpy(name,moti.motorName,131); + strtolower(name); + if(strstr(name,"ds") != NULL || strstr(name,"as") != NULL){ + if(target == ASDSTO0){ + targetValue = 0.0; + } else { + targetValue = findTarget(moti, self->targetPosition); + } + status = MotorRun(moti.pMot,pCon,targetValue); + if(status != OKOK){ + return status; + } + } + status = LLDnodePtr2Next(self->motorTable); + } + return OKOK; +} +/*-------------------------------------------------------------------------*/ +static int startOthers(pTableDrive self, SConnection *pCon){ + tdMotor moti; + int status; + char name[132]; + float targetValue; + + if(self == NULL || self->motorTable < 0){ + return 0; + } + + status = LLDnodePtr2First(self->motorTable); + while(status != 0){ + LLDnodeDataTo(self->motorTable,&moti); + strncpy(name,moti.motorName,131); + strtolower(name); + if(strstr(name,"ds") == NULL && strstr(name,"as") == NULL){ + targetValue = findTarget(moti, self->targetPosition); + status = MotorRun(moti.pMot,pCon,targetValue); + if(status != OKOK){ + return status; + } + } + status = LLDnodePtr2Next(self->motorTable); + } + return OKOK; +} +/*-------------------------------------------------------------------------*/ +static int TableDriveCheckStatus(void *pData, SConnection *pCon){ + pTableDrive self = (pTableDrive)pData; + int status; + float step; + char pBueffel[1024]; + + switch(self->state){ + case STARTING: + /* + * make sure that our current position is up-to-date + */ + TableDriveGetValue(self,pCon); + liberateMotors(self,pCon); + status = startASDS(self,pCon,ASDSTO0); + if(status != OKOK){ + TableDriveHalt(self); + return HWFault; + } + self->state = WAITDSAS; + return HWBusy; + break; + case WAITDSAS: + status = checkRunning(self,pCon); + switch(status){ + case HWIdle: + case OKOK: + if(self->debug > 0){ + showPositions(self,pCon,pBueffel,1023); + SCWrite(pCon,pBueffel,eValue); + } + status = startOthers(self,pCon); + if(status != OKOK){ + TableDriveHalt(self); + return HWFault; + } + self->state = WAITOTHER; + return HWBusy; + break; + case HWBusy: + return HWBusy; + break; + default: + TableDriveHalt(self); + self->state = WAITFINISH; + return HWBusy; + break; + } + break; + case WAITOTHER: + status = checkRunning(self,pCon); + switch(status){ + case HWIdle: + case OKOK: + if(self->debug > 0){ + showPositions(self,pCon,pBueffel,1023); + SCWrite(pCon,pBueffel,eValue); + } + status = startASDS(self,pCon,ASDSTOTARGET); + if(status != OKOK){ + TableDriveHalt(self); + return HWFault; + } + self->state = WAITFINISH; + return HWBusy; + break; + case HWBusy: + return HWBusy; + break; + default: + TableDriveHalt(self); + self->state = WAITFINISH; + return HWBusy; + break; + } + break; + case WAITFINISH: + status = checkRunning(self,pCon); + if(status != HWBusy){ + if(self->debug > 0){ + showPositions(self,pCon,pBueffel,1023); + SCWrite(pCon,pBueffel,eValue); + } + TableDriveGetValue(self,pCon); + closeMotors(self,pCon); + self->state = IDLE; + return status; + } else { + return HWBusy; + } + break; + case OUTOFSYNC: + SCWrite(pCon,"WARNING: tabledrive out of sync",eWarning); + self->state = IDLE; + return HWFault; + break; + default: + SCWrite(pCon, + "ERROR: programming error in tabledrive, invalid state", + eError); + return HWFault; + break; + } + return HWFault; +} /*================== live and death ========================================*/ static void *TableDriveInterface(void *pData, int ID){ pTableDrive self = (pTableDrive)pData;