EMU_MTR-HOME_AUTO.st

/*
#  C.E.A. IRFU/SIS/LDISC
#
*/

program homeMtrAuto

%%#include <stdio.h>
%%#include <string.h>
%%#include <stdlib.h>

/***** ARCHIVE ****/
short ARCHIVING;
assign ARCHIVING to "LNS-ISRC-010:PBI-EM:Archive";
monitor ARCHIVING;

/**** MTR PV ******/
short HOMCPTFLAG;
assign HOMCPTFLAG to "{PMACPREFIX}:{MOTOR_NAME}-HOMCPTFLAG";
monitor HOMCPTFLAG;

short statusMTRMov;
assign statusMTRMov to "{PMACPREFIX}:{MOTOR_NAME}.MOVN";
monitor statusMTRMov;

short mtrPlaced;
assign mtrPlaced to "{PMACPREFIX}:{MOTOR_NAME}-AXISSTS";
monitor mtrPlaced;
evflag mtrPlacedEvent;
sync mtrPlaced mtrPlacedEvent;

short mtrStop;
assign mtrStop to "{PMACPREFIX}:{MOTOR_NAME}.SPMG";
monitor mtrStop;

short homeCMD;
assign homeCMD to "{PMACPREFIX}:{MOTOR_NAME}-HOMCMD";
monitor homeCMD;

short mtrPosCons;
assign mtrPosCons to "{PMACPREFIX}:{MOTOR_NAME}";
monitor mtrPosCons;

short brakeMtrCMD;
assign brakeMtrCMD to "{PREFIX}:PBI-EM:VER-BRAKE_SET";
monitor brakeMtrCMD;

short brakeMtrGET;
assign brakeMtrGET to "{PREFIX}:PBI-EM:VER-BRAKE_GET";
monitor brakeMtrGET;
evflag brakeMtrGETEvent;
sync brakeMtrGET brakeMtrGETEvent;

/**** PS PV ******/
short scanPS_Sts;
assign scanPS_Sts to "{DEVICE}:PS.BUSY";
monitor scanPS_Sts;

/**** MTR PV ******/
short scanMTR_Sts;
assign scanMTR_Sts to "{DEVICE}:MTR.BUSY";
monitor scanMTR_Sts;


/**** PV to start procedure ******/
short homeSTART;
assign homeSTART to "{DEVICE}:PROC-AUTO-HOME";
monitor homeSTART;
evflag homeSTARTEvent;
sync homeSTART homeSTARTEvent;

/**** PV to read the PLIM status ******/
short plimSTATUS;
assign plimSTATUS to "{PMACPREFIX}:{MOTOR_NAME}-PENDLSTS";
monitor plimSTATUS;
evflag plimSTATUSEvent;
sync plimSTATUS plimSTATUSEvent;

ss ss1
{
	state init {
	    	      when (delay(1.0) && (pvConnectCount() == pvChannelCount())) 
					{
					homeSTART=0;
				    pvPut(homeSTART);
					printf("\nWAIT\n");
					} state waiting
		   }
	
	state waiting
		   {
		    when(efTestAndClear(homeSTARTEvent)&&(homeSTART==1)&&(plimSTATUS==0))
			    {
			    printf("\nSTART HOME PROCEDURE\n");
			    }state startHOME
		    }


	state startHOME
		 {	
			when((scanPS_Sts==0) && (scanMTR_Sts==0) && (statusMTRMov==0) && (delay(1.0))){
			printf("\n GO HOME\n");
			brakeMtrCMD=1;
			pvPut(brakeMtrCMD);
			delay(1);
			HOMCPTFLAG=1; // PLIM
			pvPut(HOMCPTFLAG);
			delay(1);
			homeCMD=1;
			pvPut(homeCMD);
			}state limitON
		 }

	state limitON
		   {
			when(efTestAndClear(plimSTATUSEvent)&&(plimSTATUS==1))
				{
				printf("\nLIMIT HIGH\n");
				HOMCPTFLAG=0; // HOME
				pvPut(HOMCPTFLAG);
				}state breakON
		   }

	state breakON
		   {
			when(delay(1.0))
				{
				printf("\nINIT POSITION\n");
				HOMCPTFLAG=0; // HOME
				pvPut(HOMCPTFLAG);
				delay(1);
				printf("\nBRAKE ON\n");
				homeCMD=1;
				pvPut(homeCMD);
				homeSTART=0;
				pvPut(homeSTART);
				brakeMtrCMD=0;
				pvPut(brakeMtrCMD);
				}state stopArchiving
           }
 
 	state stopArchiving
		   {
		   when(delay(2.0))
				{       
				printf("\nStop Archiving\n"); 
           		ARCHIVING=0;
				pvPut(ARCHIVING);
				}state init
           }
}