program PLL
%%#include <math.h>

int COUNT1;
assign COUNT1 to "FBM:SR830:COUNT1";
monitor COUNT1;
int COUNTP;

double SNAP[10];
assign SNAP to "FBM:SR830:SNAPREAD";

double FREQ_SET;
assign FREQ_SET to "FBM:SR830:FREQ:SET";

double FREQ_NOW;

double FREQ_LOWLIM;
double FREQ_UPLIM;
double ACCFAC;

assign FREQ_LOWLIM to "FBM:SR830:FREQ:LOWLIM";
assign FREQ_UPLIM to "FBM:SR830:FREQ:UPLIM";
assign ACCFAC to "FBM:SR830:ACCFAC";

double SLOPE1;
double SLOPE2;

int i;

ss state_pll
{
	state init
	{
		when()
		{
		pvGet(COUNT1);
		pvGet(SNAP);
		FREQ_SET = SNAP[4];
		COUNTP = COUNT1;
		pvGet(FREQ_LOWLIM);
		pvGet(FREQ_UPLIM);
		pvGet(ACCFAC);
		SLOPE1 = 0.016911031*1000;
		SLOPE2 = 0.03*1000;

		}state lock
	}
	state lock
	{
		when(COUNT1 != COUNTP)
		{
		COUNTP=COUNT1;
		pvGet(SNAP);
		pvGet(FREQ_LOWLIM);
		pvGet(FREQ_UPLIM);
		pvGet(ACCFAC);


		if (SNAP[3] >80)
		 FREQ_SET = FREQ_SET + floor(SLOPE2*ACCFAC*SNAP[3]);
		else if (SNAP[3] >0)
		 FREQ_SET = FREQ_SET +floor(SLOPE1*ACCFAC*SNAP[3]);
                else if (SNAP[3]> -80)
		 FREQ_SET = FREQ_SET + ceil(SLOPE1*ACCFAC*SNAP[3]);
		else 
                 FREQ_SET = FREQ_SET +ceil(SLOPE2*ACCFAC*SNAP[3]);

		if (FREQ_SET > FREQ_UPLIM)
		 FREQ_SET = FREQ_UPLIM;
		if (FREQ_SET <FREQ_LOWLIM)
		 FREQ_SET = FREQ_LOWLIM;

		 pvPut(FREQ_SET); 		 
		 
		} state lock
	}
}