program sncCompCalMotion ( "C=COMCAL,P=hd:comcal,R1=x:motor,R2=y:motor,CSV_File='CompCal-maps.csv' ") option +r; /* Reentrant code */ option -a; /* Asynchronous pvGet */ option +d; /* Turn on debug message */ %% #include %% #include %% #include %% #include %% #include %{ int testRead(char *filename); float X_com[12][12]; float Y_com[12][12]; }% short errorX; short errorY; short nnx; short nny; short Ncol; short Nrow; short moveto_cristal; short moveto_retract; assign moveto_retract to "{C}:SNL:moveToRetract"; monitor moveto_retract; short moveto_center; assign moveto_center to "{C}:SNL:moveToCenter"; monitor moveto_center; float absOffset; float dxx; float dyy; float x_o; float y_o; float x_n; float y_n; float phi; char* pMacro; char* r1Macro; char* r2Macro; char* fileMacro; float moveInX; assign moveInX to "{C}:SNL:moveInX"; monitor moveInX; float moveInY; assign moveInY to "{C}:SNL:moveInY"; monitor moveInY; /* delta(X) and delta(Y) step size of tuning*/ float dX; assign dX to "{C}:SNL:dX"; monitor dX; float dY; assign dY to "{C}:SNL:dY"; monitor dY; /* max movement in(X) or (Y) for the tuning (< 1cm) */ float maxdX; assign maxdX to "{C}:SNL:maxdX"; monitor maxdX; float maxdY; assign maxdY to "{C}:SNL:maxdY"; monitor maxdY; /* Coordinates of central window (marked as x) */ float center; assign center to "{C}:SNL:center"; monitor center; short centered; assign centered to "{C}:SNL:centered"; monitor centered; /* Coordinates of retract position */ float retractX; assign retractX to "{C}:SNL:retractX"; monitor retractX; float retractY; assign retractY to "{C}:SNL:retractY"; monitor retractY; /* Coordinates of in Beam position (coordinates of the center ) */ float pivotX; assign pivotX to "{C}:SNL:pivotX"; monitor pivotX; float pivotY; assign pivotY to "{C}:SNL:pivotY"; monitor pivotY; /*coordinate ofcristal in ComCal coord system*/ float comcalX; assign comcalX to "{C}:SNL:comcalX"; monitor comcalX; float comcalY; assign comcalY to "{C}:SNL:comcalY"; monitor comcalY; /* distance betwen two pads) */ float stepX; assign stepX to "{C}:SNL:stepX"; monitor stepX; float stepY; assign stepY to "{C}:SNL:stepY"; monitor stepY; float offsetX; assign offsetX to "{C}:SNL:offsetX"; monitor offsetX; float saveOffsetX; assign saveOffsetX to "{C}:SNL:saveOffsetX"; monitor saveOffsetX; float offsetY; assign offsetY to "{C}:SNL:offsetY"; monitor offsetY; float saveOffsetY; assign saveOffsetY to "{C}:SNL:saveOffsetY"; monitor saveOffsetY; short n_y; assign n_y to "{C}:SNL:N_Y"; monitor n_y; short n_x; assign n_x to "{C}:SNL:N_X"; monitor n_x; short n_xc; assign n_xc to "{C}:SNL:N_Xc"; monitor n_xc; short n_yc; assign n_yc to "{C}:SNL:N_Yc"; monitor n_yc; short n_xtarget; assign n_xtarget to "{C}:SNL:N_Xtarget"; monitor n_xtarget; short n_ytarget; assign n_ytarget to "{C}:SNL:N_Ytarget"; monitor n_ytarget; short saveOffsets; assign saveOffsets to "{C}:SNL:saveOffsets"; monitor saveOffsets; short moveDone; assign moveDone to "{C}:SNL:moveDone"; monitor moveDone; short start; assign start to "{C}:SNL:start"; monitor start; short abort; assign abort to "{C}:SNL:abort"; monitor abort; short retract; assign retract to "{C}:SNL:retract"; monitor retract; short retracted; assign retracted to "{C}:SNL:retracted"; monitor retracted; short moveXR; assign moveXR to "{C}:SNL:moveXRight"; monitor moveXR; short moveXL; assign moveXL to "{C}:SNL:moveXLeft"; monitor moveXL; short moveYU; assign moveYU to "{C}:SNL:moveYUp"; monitor moveYU; short moveYD; assign moveYD to "{C}:SNL:moveYDown"; monitor moveYD; short runMove; assign runMove to "{C}:SNL:runMove"; monitor runMove; string stateText; assign stateText to "{C}:SNL:stateText"; monitor stateText; string errorMesage; assign errorMesage to "{C}:SNL:ERR_MESAGE"; monitor errorMesage; short error; assign error to "{C}:SNL:error"; monitor error; /* motor X */ float valX; assign valX to "{P}:{R1}.VAL"; monitor valX; short dmovX; assign dmovX to "{P}:{R1}.DMOV"; monitor dmovX; short stopX; assign stopX to "{P}:{R1}.STOP"; float RbvX; assign RbvX to "{P}:{R1}.RBV"; monitor RbvX; float RlvX; assign RlvX to "{P}:{R1}.RLV"; monitor RlvX; /* motor Y */ float valY; assign valY to "{P}:{R2}.VAL"; monitor valY; short dmovY; assign dmovY to "{P}:{R2}.DMOV"; monitor dmovY; short stopY; assign stopY to "{P}:{R2}.STOP"; monitor stopY; float RbvY; assign RbvY to "{P}:{R2}.RBV"; monitor RbvY; float RlvY; assign RlvY to "{P}:{R2}.RLV"; monitor RlvY; ss ss1 { /* Initialization */ state INIT { entry { sprintf(stateText,"INIT"); pvPut(stateText, SYNC); dX=0.025; pvPut( dX, SYNC); dY=0.01; pvPut( dY, SYNC); maxdX=10.; pvPut( maxdX, SYNC); maxdY=10.; pvPut( maxdY, SYNC); stepX=20.9; pvPut(stepX, SYNC); stepY=20.9; pvPut(stepY, SYNC); comcalX=0; pvPut(comcalX, SYNC); comcalY=0; pvPut(comcalY, SYNC); pivotX=251.20; pvPut(pivotX, SYNC); pivotY=134.63; pvPut(pivotY, SYNC); retractX=0.; pvPut(retractX, SYNC); retractY=pivotY; pvPut(retractY, SYNC); abort=0; pvPut(abort, SYNC); offsetX=0; pvPut(offsetX, SYNC); offsetY=0; pvPut(offsetY, SYNC); start=0; pvPut(start, SYNC); runMove=0; pvPut(runMove, SYNC); error=0; pvPut(error, SYNC); saveOffsetX=0.; pvPut(saveOffsetX, SYNC); saveOffsetY=0.; pvPut(saveOffsetY, SYNC); moveDone=0; pvPut(moveDone, SYNC); n_xc=0; pvPut(n_xc, SYNC); n_yc=0; pvPut(n_yc, SYNC); sprintf(errorMesage," "); pvPut(errorMesage, SYNC); fileMacro = macValueGet("CSV_File"); /*printf(" Start \n");*/ testRead(fileMacro); } when(delay(1.) ) { /* Go to STANDBY state without doing anything */ r1Macro = macValueGet( "R1" ); r2Macro = macValueGet( "R2" ); pMacro = macValueGet( "P" ); } state STANDBY } /*--------------STANDBY */ /* Standby waiting for requests for Movements */ state STANDBY { entry { n_xtarget=100; pvPut(n_xtarget, SYNC); n_ytarget=100; pvPut(n_ytarget, SYNC); sprintf(stateText,"STANDBY"); pvPut(stateText, SYNC); moveto_retract=0; pvPut(moveto_retract, SYNC); moveto_center=0; pvPut(moveto_center, SYNC); moveto_cristal=0; n_x=100; pvPut(n_x, SYNC); n_y=100; pvPut(n_y, SYNC); } when (abort == 1 ) { abort=0; pvPut(abort, SYNC); sprintf(errorMesage,"Nothing to ABORT "); pvPut(errorMesage, SYNC); } state STANDBY /*------------ Move X right/left in dX ----------*/ when( moveXR == 1 || moveXL == 1) { } state MOVE_RIGHT_LEFT /*------------ Move Y Up/Down in dY ----------*/ when( moveYU == 1 || moveYD == 1) { } state MOVE_Up_Down /*------------ Save OffsetX and OffsetY ----------*/ when( saveOffsets == 1 ) { } state SAVE_OFFSETS /*------------Move to Retract ----------*/ when( retract == 1 ) { sprintf(stateText,"RETRACT"); pvPut(stateText, SYNC); moveInX=retractX; moveInY=retractY; moveto_retract=1; pvPut(moveto_retract, SYNC); retract=0; pvPut(retract, SYNC); /*printf(" ***Move to Retract position X= %6.3f Y= %6.3f \n",moveInX,moveInY); */ } state PREPAR /*------------Move to Center ----------*/ when( center == 1 ) { sprintf(stateText,"IN BEAM"); pvPut(stateText, SYNC); moveInX=pivotX; moveInY=pivotY; moveto_center=1; pvPut(moveto_center, SYNC); center=0; pvPut(center, SYNC); /*printf(" ***Move to Centeral position X= %6.3f Y= %6.3f \n",moveInX,moveInY); */ } state PREPAR /*------------ Move to the cell (N_X: N_Y) ----------*/ when( runMove == 1 ) { /* start Moving */ centered=0; pvPut(centered, SYNC); retracted=0; pvPut(retracted, SYNC); moveDone=0; pvPut(moveDone, SYNC); n_xc=-100; pvPut(n_xc, SYNC); n_yc=-100; pvPut(n_yc, SYNC); moveto_cristal=1; } state MOVING } /*-----------*/ state PREPAR { entry { moveDone=0; pvPut(moveDone, SYNC); n_xc=-100; pvPut(n_xc, SYNC); n_yc=-100; pvPut(n_yc, SYNC); sprintf(errorMesage," "); pvPut(errorMesage, SYNC); } when( delay(0.5) ) { pvPut(moveInX, SYNC); pvPut(moveInY, SYNC); centered=0; pvPut(centered, SYNC); retracted=0; pvPut(retracted, SYNC); moveDone=0; pvPut(moveInY, SYNC); start=0; pvPut(start, SYNC); offsetX=0.; pvPut(offsetX, SYNC); offsetY=0.; pvPut(offsetY, SYNC); comcalX=0.; pvPut(comcalX, SYNC); comcalY=0.; pvPut(comcalY, SYNC); } state MOVE_X } /*-------- Save Offsets-------*/ state SAVE_OFFSETS { entry { sprintf(stateText,"SAVE OFFSETS"); pvPut(stateText, SYNC); saveOffsets=0., pvPut(saveOffsets, SYNC); } when( delay(0.5) ) { /* printf(" Save OffsetX: %6.3f OffsetY: %6.3f \n",offsetX,offsetY); */ saveOffsetX=offsetX; pvPut(saveOffsetX, SYNC); saveOffsetY=offsetY; pvPut(saveOffsetY, SYNC); } state STANDBY } /*-------- MOVING-------*/ state MOVING { entry { sprintf(stateText,"START MOVING"); pvPut(stateText, SYNC); runMove=0; pvPut(runMove, SYNC); error=0; pvPut(error, SYNC); sprintf(errorMesage," "); pvPut(errorMesage, SYNC); start=1; pvPut(start, SYNC); n_xc=100; pvPut(n_xc, SYNC); n_yc=100; pvPut(n_yc, SYNC); } when ( delay(0.5)&& (n_x>6|| n_x<-6 || n_y>6 || n_y<-6 )) { sprintf(errorMesage,"Bad N_X or N_Y "); pvPut(errorMesage, SYNC); printf("**** BadNx,Ny >6 or <-6 n_X= %d n_Y= %d \n",n_x,n_y ); } state STANDBY when (delay(0.5)&& (n_x>-2 && n_x<2) && (n_y==1|| n_y==-1) ) { sprintf(errorMesage,"Bad N_X or N_Y "); pvPut(errorMesage, SYNC); printf("**** N_Y=1:-1 and -2=2||n_y<=-2){ Ncol=6-n_x; if (n_x<0){ Ncol=6-n_x-1; } } if(n_y==1||n_y==-1){ Ncol=6-n_x; if (n_x<0){ Ncol=6-n_x-3; } } x_o=pivotX+nnx*stepX+stepX/2.+saveOffsetX; y_o=pivotY-nny*stepY-stepY/2.+saveOffsetY; phi=acos(-1.); moveInX=pivotX-(X_com[Nrow][Ncol]*cos(phi)-Y_com[Nrow][Ncol]*sin(phi))+saveOffsetX; moveInY=pivotY+(X_com[Nrow][Ncol]*sin(phi)+Y_com[Nrow][Ncol]*cos(phi))+saveOffsetY; pvPut(moveInX, SYNC); pvPut(moveInY, SYNC); /*x_n=pivotX+X_com[Nrow][Ncol]+saveOffsetX; y_n=pivotY-Y_com[Nrow][Ncol]+saveOffsetY;*/ comcalX=X_com[Nrow][Ncol]; pvPut(comcalX, SYNC); comcalY=Y_com[Nrow][Ncol]; pvPut(comcalY, SYNC); /* printf(" Move to %d:%d \n",n_x,n_y ); printf(" * n_row= %d Nrow= %d \n",Nrow,Ncol ); printf(" * in ComCal coord system: X= %6.2f Y= %6.2f \n",X_com[Nrow][Ncol],Y_com[Nrow][Ncol] ); printf(" nnx= %d nny= %d \n",nnx,nny ); printf(" Stage coord system(calc): X= %6.2f Y= %6.2f \n",x_o,y_o ); printf(" * in Stage coord system: X= %6.2f Y= %6.2f \n",moveInX,moveInY ); printf(" -------------- \n" ); */ offsetX=0.; pvPut(offsetX, SYNC); offsetY=0.; pvPut(offsetY, SYNC); } state MOVE_X } /*-------- MOVE_X-------*/ state MOVE_X { entry { valX=moveInX; pvPut(valX); } when( delay(0.1) ) { /*printf(" Move in X: %6.3f \n",moveInX); printf(" ************************************************ \n"); */ } state MOVINGX_STATUS } state MOVINGX_STATUS{ entry { sprintf(stateText,"MOVING X"); pvPut(stateText, SYNC); } when (abort == 1 ) { stopX=1; pvPut(stopX, SYNC); /* printf(" ***********ABORT******** abort=%d stopX=%d \n",abort,stopX); */ /* ABORT go to ABORTED state */ } state ABORTED /* wait for end of movement */ when(delay(0.2)&& dmovX == 1 ) { if((moveInX-RbvX)>=0.05 || (moveInX-RbvX)<=-0.05){ error=1; pvPut(error, SYNC); errorX=1; /* printf(" ********* wrong position in X moveInX %6.3f RbvX= %6.3f \n",moveInX,RbvX); */ } else{ errorX=0; } }state MOVE_X_DONE } /*-------- MOVE_X DONE-------*/ state MOVE_X_DONE { entry { sprintf(stateText,"MOVE X DONE"); pvPut(stateText, SYNC); } when( delay(0.2) ) { if(errorX==0){ /* printf("++++++++ MOVE_X_DONE +++++++++\n"); */ } } state MOVE_Y when( delay(0.5) ) { if(errorX==1){ sprintf(errorMesage,"Motor X problem: Controller ERROR ? "); pvPut(errorMesage, SYNC); /* printf("++++++++ MOVE_X ERROR +++++++++\n"); */ } } state STANDBY } /*-------- MOVE_Y-------*/ state MOVE_Y { entry { valY=moveInY; pvPut(valY); } when( delay(0.5) ) { /* printf(" Move in Y: %6.3f \n",moveInY); printf(" ************************************************ \n");*/ } state MOVINGY_STATUS } state MOVINGY_STATUS{ entry { sprintf(stateText,"MOVING Y "); pvPut(stateText, SYNC); } when (abort == 1 ) { stopY=1; pvPut(stopY, SYNC); /* printf(" ***********ABORT******** abort=%d stopY=%d \n",abort,stopY); */ /* ABORT go to ABORTED state */ } state ABORTED /* wait for end of movement */ when(delay(0.2)&& dmovY == 1 ) { if((moveInY-RbvY)>=0.05 || (moveInY-RbvY)<=-0.05){ error=1; pvPut(error, SYNC); errorY=1; } else{ errorY=0; } }state MOVE_Y_DONE } /*-------- MOVE_Y DONE-------*/ state MOVE_Y_DONE { entry { sprintf(stateText,"MOVE Y DONE"); pvPut(stateText, SYNC); } when( delay(0.2) ) { if(errorY==0){ /* printf("++++++++ MOVE_Y_DONE +++++++++\n"); */ } } state MOVE_DONE when( delay(0.5) ) { if(errorY==1){ sprintf(errorMesage,"Motor Y problem: Controller ERROR ? "); pvPut(errorMesage, SYNC); /* printf("++++++++ MOVE_Y ERROR +++++++++\n"); */ } } state STANDBY } /*-------- MOVE DONE-CRISTAL------*/ state MOVE_DONE { entry { moveDone=1; pvPut(moveDone, SYNC); sprintf(stateText,"MOVE DONE"); pvPut(stateText, SYNC); /* printf("++++++++ MOVE_DONE +++++++++\n"); */ } when( delay(0.5)&& moveto_retract==1) { moveto_retract=0; retracted=1; pvPut(retracted, SYNC); } state STANDBY when( delay(0.5)&& moveto_center==1) { moveto_center=0; pvPut(moveto_center, SYNC); centered=1; pvPut(centered, SYNC); } state STANDBY when( delay(0.2)&& moveto_cristal==1) { moveto_cristal=0; n_xc=n_x; pvPut(n_xc, SYNC); n_yc=n_y; pvPut(n_yc, SYNC); } state STANDBY } /*---------ABORT------*/ state ABORTED { entry { sprintf(stateText,"ABORTED"); pvPut(stateText, SYNC); start=0; pvPut(start, SYNC); retract=0; pvPut(retract, SYNC); runMove=0; pvPut(runMove, SYNC); offsetX=0.; pvPut(offsetX, SYNC); offsetY=0.; pvPut(offsetY, SYNC); center = 0; pvPut(center, SYNC); abort = 0; pvPut(abort, SYNC); sprintf(errorMesage,"ABORTED "); pvPut(errorMesage, SYNC); } when( delay(0.2) ) { /* printf("* ****** ABORT****** %d \n",stopX); */ } state STANDBY } /*------------ STATE Move X right/left in dX ----------*/ state MOVE_RIGHT_LEFT { entry { if (moveXR==1){ sprintf(stateText,"MOVE Right"); pvPut(stateText, SYNC); moveXR=0; pvPut(moveXR, SYNC); dxx=dX; } if (moveXL==1){ sprintf(stateText,"MOVE Left"); pvPut(stateText, SYNC); dxx=-dX; moveXL=0; pvPut(moveXL, SYNC); } } when( delay(0.5) ) { /* start Moving X */ error=0; pvPut(error, SYNC); sprintf(errorMesage," "); pvPut(errorMesage, SYNC); if((offsetX)<0){ absOffset=-offsetX-dxx; } else{ absOffset=offsetX+dxx; } if(absOffset>maxdX) { error=1; pvPut(error, SYNC); sprintf(errorMesage,"BIG Offset in X "); pvPut(errorMesage, SYNC); dxx=0; /* printf(" ********* BIG OFFSET in X %6.3f absOffsetX= %6.3f \n",offsetX,absOffset); */ } offsetX=offsetX+dxx; pvPut(offsetX, SYNC); /* printf(" \n"); printf(" * XPS: %s Motor: %s Moving dX %6.3f \n",pMacro,r1Macro,dxx); */ } state MOVE_X_REL } /*-------- MOVE_X-------*/ state MOVE_X_REL { entry { sprintf(stateText,"MOVE RELATIVE X"); pvPut(stateText, SYNC); } when( delay(0.5) ) { /* printf(" Move Relative in X: %6.3f \n",dxx); printf(" ************************************************ \n"); */ RlvX=dxx; pvPut(RlvX, SYNC); } state STATUS_X } state STATUS_X{ entry { sprintf(stateText,"MOVE RELATIVE X"); pvPut(stateText, SYNC); error=0; moveInX=moveInX+dxx; } when( dmovX == 1 ) { if((moveInX-RbvX)>=0.05 || (moveInX-RbvX)<=-0.05){ error=1; pvPut(error, SYNC); sprintf(errorMesage,"Motor X problem: Controller ERROR "); pvPut(errorMesage, SYNC); errorX=1; /* printf(" ********* wrong position in X moveInX %6.3f RbvX= %6.3f \n",moveInX,RbvX); */ } else{ errorX=0; } }state STANDBY } /*------------ STATE Move Y Up/Down in dY ----------*/ state MOVE_Up_Down { entry { if (moveYU==1){ sprintf(stateText,"MOVE Up"); pvPut(stateText, SYNC); moveYU=0; pvPut(moveYU, SYNC); dyy=dY; } if (moveYD==1){ sprintf(stateText,"MOVE Down"); pvPut(stateText, SYNC); moveYD=0; pvPut(moveYD, SYNC); dyy=-dY; } } when( delay(0.5) ) { /* start Moving Y */ error=0; pvPut(error, SYNC); sprintf(errorMesage," "); pvPut(errorMesage, SYNC); if((offsetY)<0){ absOffset=-offsetY-dyy; } else{ absOffset=offsetY+dyy; } if(absOffset>maxdY) { error=1; pvPut(error, SYNC); sprintf(errorMesage,"BIG Offset in Y "); pvPut(errorMesage, SYNC); dyy=0; /* printf(" ********* BIG OFFSET in Y %6.3f absOffsetY= %6.3f \n",offsetY,absOffset); */ } offsetY=offsetY+dyy; pvPut(offsetY, SYNC); /* printf(" \n"); printf(" * XPS: %s Motor: %s Moving dY %6.3f \n",pMacro,r2Macro,dyy); */ } state MOVE_Y_REL } /*-------- MOVE_Y Relative------*/ state MOVE_Y_REL { entry { sprintf(stateText,"MOVE RELATIVE Y"); pvPut(stateText, SYNC); } when( delay(0.5) ) { /* printf(" Move Relative in Y: %6.3f \n",dyy); printf(" ************************************************ \n"); */ RlvY=dyy; pvPut(RlvY, SYNC); } state STATUS_Y } state STATUS_Y{ entry { sprintf(stateText,"MOVE RELATIVE Y"); pvPut(stateText, SYNC); error=0; moveInY=moveInY+dyy; } when( dmovY == 1 ) { if((moveInY-RbvY)>=0.05 || (moveInY-RbvY)<=-0.05){ error=1; pvPut(error, SYNC); sprintf(errorMesage,"Motor Y problem: Controller ERROR "); pvPut(errorMesage, SYNC); errorY=1; /* printf(" ********* wrong position in Y moveInY %6.3f RbvY= %6.3f \n",moveInY,RbvY); */ } else{ errorY=0; } }state STANDBY } } ss ss2 { /* Initialization */ state INIT { entry { } when(delay(0.2) ) { } state POSITIONS } /*-----------POSITIONS */ state POSITIONS { entry { retracted=0; centered=0; pvPut(centered, SYNC); pvPut(retracted, SYNC); } when(delay(2.)&& dmovX == 1 && dmovY == 1) { if(((retractX-RbvX)>=-0.05 && (retractX-RbvX)<=0.05)&&((retractY-RbvY)>=-0.05 && (retractY-RbvY)<=0.05)){ retracted=1; pvPut(retracted, SYNC); /*printf(" ****POSITIONS***** RbvX= %6.3f RbvY= %6.3f retracted= %3d \n",RbvX,RbvY,retracted); */ } if(((pivotX-RbvX)>=-0.05 && (pivotX-RbvX)<=0.05)&&((pivotY-RbvY)>=-0.05 && (pivotY-RbvY)<=0.05)){ centered=1; /*printf(" ****POSITIONS***** RbvX= %6.3f RbvY= %6.3f centered= %3d \n",RbvX,RbvY,centered); */ pvPut(centered, SYNC); } } state POSITIONS } } %{ int testRead(char *filename) { FILE* fp; int col,row, i,j,k; float a, b, c; // printf(" ***** testRead **** \n " ); fp = fopen( filename, "r" ); if(fp == NULL) { printf("Error! Can't open CompCal-maps.csv file"); exit(1); } k=12; for (j=0;j<12;j++){ if(j==5 || j==6){ k=10;} else { k=12;} for (i=0;i