//Deklarace proměnných

FUNCTION_BLOCK Homing
VAR_INPUT
END_VAR
VAR_OUTPUT
END_VAR
VAR
	// Variables from input cards
	FirstAxisSensorRawValue : BYTE; 
	SecondAxisSensorRawValue : BYTE;

	// Processed values
	FirstAxisRawPos : BYTE :=0;
	SecondAxisRawPos : BYTE :=0;	
	
	ThirdAxisHomeSensor : BOOL :=0; //copy variable
	FourthAxisHomeSensor : BOOL :=0; //copy variable
	
	FirstAxisPrevPos : INT :=0;
	SecondAxisPrevPos : INT :=0;	
	
	FirstAxisActualPos : INT :=0;
	SecondAxisActualPos : INT :=0;	
	
	FirstAxisNextPos : INT :=0;
	SecondAxisNextPos : INT :=0;
	
	FirstAxisDetectedPos : INT :=0;
	SecondAxisDetectedPos : INT :=0;					  
	
	HomingDoneAxis1 : BOOL := 0; //Home procedure completion indicator
	HomingDoneAxis2 : BOOL := 0; //Home procedure completion indicator
	HomingDoneAxis3 : BOOL := 0; //Home procedure completion indicator
	HomingDoneAxis4 : BOOL := 0; //Home procedure completion indicator

	SpeedMultiplierPrevValue : REAL := 10;
	
	firstAxisRAF :  WORD := 0; //Case selector variable
	secondAxisRAF :  WORD := 0; //Case selector variable
	thirdAxisRAF :  WORD := 0; //Case selector variable
	fourthAxisRAF :  WORD := 0; //Case selector variable
		  
	decodeError : BOOL :=0; // Error value when decoding goes bad
	decodeError1 : BOOL :=0; // Error value when decoding goes bad

	LookUpTable : ARRAY[0..63] OF INT := [ 0, 30, 6, 60, -6, 36, 108, 90, 18, 24, 12, 66, 5000, 138, 114, 120, -54, 48, -48, 54, -108, 42, 102, 96, -96
							  , 5000, 5000, 5000, -102, 144, 5000, 5000, -30, -24, -36, 78, -12, -18, 5000, 84, -84, -78, -138, 72, -126, 132, -132, 126
							  , -60, -66, -42, 5000, -114, 5000, 5000, 5000, -90, -72, -144, 5000, -120, 5000, -150, 5000]; 
	// 5000 = unfilled position = error
	
	//timer section
	Timer : TON :=(PT := T#1000MS);
	TimerFirstAxis : TON :=(PT := T#1000MS);
	TimerSecondAxis : TON :=(PT := T#1000MS);
	TimerThirdAxis : TON :=(PT := T#1000MS);
	TimerThirdAxisTimeout : TON :=(PT := T#1000MS);
	TimerFourthAxis : TON :=(PT := T#1000MS);
	
	
END_VAR


//Programová část


// powering on homing circuit after powering up of all axes and turning off after all axes are homed
gvl.OutputCard[0] := (NOT(HomingDoneAxis1 AND HomingDoneAxis2 AND HomingDoneAxis3 AND HomingDoneAxis4))AND(GVL.stAxis1ControlSignals.bReady AND GVL.stAxis2ControlSignals.bReady AND GVL.stAxis3ControlSignals.bReady AND GVL.stAxis4ControlSignals.bReady); //AND(NOT(decodeError));

//convering data from bool arrays to byte
FirstAxisSensorRawValue := 0;
IF GVL.FirstCard[0] THEN FirstAxisSensorRawValue := FirstAxisSensorRawValue + 1; END_IF;
IF GVL.FirstCard[1] THEN FirstAxisSensorRawValue := FirstAxisSensorRawValue + 2; END_IF;
IF GVL.FirstCard[2] THEN FirstAxisSensorRawValue := FirstAxisSensorRawValue + 4; END_IF;
IF GVL.FirstCard[3] THEN FirstAxisSensorRawValue := FirstAxisSensorRawValue + 8; END_IF;
IF GVL.FirstCard[4] THEN FirstAxisSensorRawValue := FirstAxisSensorRawValue + 16; END_IF;
IF GVL.FirstCard[5] THEN FirstAxisSensorRawValue := FirstAxisSensorRawValue + 32; END_IF;

SecondAxisSensorRawValue := 0;
IF GVL.SecondCard[0] THEN SecondAxisSensorRawValue := SecondAxisSensorRawValue + 1; END_IF;
IF GVL.SecondCard[1] THEN SecondAxisSensorRawValue := SecondAxisSensorRawValue + 2; END_IF;
IF GVL.SecondCard[2] THEN SecondAxisSensorRawValue := SecondAxisSensorRawValue + 4; END_IF;
IF GVL.SecondCard[3] THEN SecondAxisSensorRawValue := SecondAxisSensorRawValue + 8; END_IF;
IF GVL.SecondCard[4] THEN SecondAxisSensorRawValue := SecondAxisSensorRawValue + 16; END_IF;
IF GVL.SecondCard[5] THEN SecondAxisSensorRawValue := SecondAxisSensorRawValue + 32; END_IF;

//processing input data of first axis

FirstAxisActualPos := LookUpTable[FirstAxisSensorRawValue];
	
IF FirstAxisActualPos = 5000 THEN
	decodeError1:=1;// nějaký alarm stav protože tohle nemůže nastat = velký problém
END_IF

//processing input data of second axis

SecondAxisActualPos := LookUpTable[SecondAxisSensorRawValue];

IF SecondAxisActualPos = 5000 THEN
	decodeError1:=1;
END_IF

IF  decodeError THEN
	//firstAxisRAF := 1;
	//secondAxisRAF := 1;
	thirdAxisRAF := 1;
	fourthAxisRAF := 1;
	
	GVL.stAxis1ControlSignals.bJogForward:= 0;
	GVL.stAxis1ControlSignals.bJogBackward:= 0;
	
	GVL.stAxis2ControlSignals.bJogForward:= 0;
	GVL.stAxis2ControlSignals.bJogBackward:= 0;
	
	GVL.stAxis3ControlSignals.bJogForward:= 0;
	GVL.stAxis3ControlSignals.bJogBackward:= 0;
	
	GVL.stAxis4ControlSignals.bJogForward:= 0;
	GVL.stAxis4ControlSignals.bJogBackward:= 0;
	
END_IF



ThirdAxisHomeSensor := GVL.FirstCard[7];

FourthAxisHomeSensor := GVL.SecondCard[7];


Timer(IN:=gvl.OutputCard[0]);

// homing procedure of first axis

TimerFirstAxis(IN:=(firstAxisRAF=200));

CASE firstAxisRAF OF 
	0:	IF	GVL.stAxis1ControlSignals.bReady AND Timer.Q THEN
			SpeedMultiplierPrevValue := gvl.SpeedMultiplier;
			GVL.SpeedMultiplier:=0.000001; // 0.000001% of max speed
			firstAxisRAF:=10;
		END_IF

	10: IF FirstAxisActualPos > -1 THEN
			
			FirstAxisNextPos:= FirstAxisActualPos - 6;
			FirstAxisPrevPos := FirstAxisActualPos;
			
			GVL.stAxis1ControlSignals.bJogBackward:= 1;						
			// motor se otáčí malou rychlostí po směru hodinových ručiček při pohledu shora na prvni ose
			
			firstAxisRAF:= 20;
		ELSE
			FirstAxisNextPos:= FirstAxisActualPos + 6;
			FirstAxisPrevPos := FirstAxisActualPos;				
			// motor se otáčí malou rychlostí PROTI směru hodinových ručiček při pohledu zvrchu na prvni ose	
			
			GVL.stAxis1ControlSignals.bJogForward :=1;
			
			firstAxisRAF:= 100;			
		END_IF
	
	20:	IF (FirstAxisActualPos = FirstAxisNextPos) OR (FirstAxisActualPos <> FirstAxisPrevPos) THEN
					
			GVL.stAxis1ControlSignals.bJogBackward:= 0;
			
		IF FirstAxisActualPos = FirstAxisNextPos THEN
			FirstAxisDetectedPos := FirstAxisNextPos + 6;
			
			//GVL.SpeedMultiplier:=0.0000000001;
			GVL.SpeedMultiplier:=1;
			GVL.stAxis1ControlSignals.bJogForward:= 1;
			firstAxisRAF:= 30;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF
		END_IF
	
	30: IF (FirstAxisActualPos = FirstAxisPrevPos) OR (FirstAxisActualPos <> FirstAxisNextPos) THEN
		GVL.stAxis1ControlSignals.bJogForward:= 0;
	
		IF FirstAxisActualPos = FirstAxisPrevPos THEN
			firstAxisRAF:= 200;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF	
		END_IF
	
	100: IF (FirstAxisActualPos = FirstAxisNextPos) OR (FirstAxisActualPos <> FirstAxisPrevPos) THEN
			GVL.stAxis1ControlSignals.bJogForward := 0;
			
		IF FirstAxisActualPos = FirstAxisNextPos THEN
			FirstAxisDetectedPos := FirstAxisNextPos;
			
			GVL.SpeedMultiplier:=0.0000000001;
			GVL.stAxis1ControlSignals.bJogBackward:= 1;
			
			firstAxisRAF:= 110;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF
	END_IF
	
	110: IF (FirstAxisActualPos = FirstAxisPrevPos) OR (FirstAxisActualPos <> FirstAxisNextPos) THEN
		GVL.stAxis1ControlSignals.bJogBackward:= 0;
	
		IF FirstAxisActualPos = FirstAxisPrevPos THEN
			GVL.stAxis1ControlSignals.bJogForward:= 1;
			firstAxisRAF:= 120;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF	
		END_IF
		
	120: IF (FirstAxisActualPos = FirstAxisNextPos) OR (FirstAxisActualPos <> FirstAxisPrevPos) THEN
		GVL.stAxis1ControlSignals.bJogForward:= 0;
	
		IF FirstAxisActualPos = FirstAxisNextPos THEN
			
			firstAxisRAF:= 200;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF	
		END_IF	
	
	200: IF NOT(HomingDoneAxis1)AND TimerFirstAxis.Q THEN
		
		GVL.stAxis1ControlSignals.fDetectedPossition := INT_TO_LREAL(FirstAxisDetectedPos+90);	// +90° protože má Martin nulu když je rameno natočené vpravo
		
		GVL.stAxis1ControlSignals.bResetAxisHome :=1;
		
		GVL.SpeedMultiplier := SpeedMultiplierPrevValue;
		
		HomingDoneAxis1 := 1;
	END_IF		
END_CASE

// homing procedure of second axis

TimerSecondAxis(IN:=(secondAxisRAF=200));

CASE secondAxisRAF OF 
	0:	IF	GVL.stAxis2ControlSignals.bReady AND HomingDoneAxis1 THEN
			SpeedMultiplierPrevValue := gvl.SpeedMultiplier;
			GVL.SpeedMultiplier:=0.0000000001; // 0.0000000001% of max speed
			secondAxisRAF:=10;
		END_IF

	10: IF SecondAxisActualPos > -1 THEN
			
			SecondAxisNextPos:= SecondAxisActualPos - 6;
			SecondAxisPrevPos := SecondAxisActualPos;
			
			GVL.stAxis2ControlSignals.bJogBackward:= 1;						
			// motor se otáčí malou rychlostí po směru hodinových ručiček při pohledu shora na prvni ose
			
			secondAxisRAF:= 20;
		ELSE
			SecondAxisNextPos:= SecondAxisActualPos + 6;
			SecondAxisPrevPos := SecondAxisActualPos;				
			// motor se otáčí malou rychlostí PROTI směru hodinových ručiček při pohledu zvrchu na prvni ose	
			
			GVL.stAxis2ControlSignals.bJogForward :=1;
			
			secondAxisRAF:= 100;			
		END_IF
	
	20:	IF (SecondAxisActualPos = SecondAxisNextPos) OR (SecondAxisActualPos <> SecondAxisPrevPos) THEN
					
			GVL.stAxis2ControlSignals.bJogBackward:= 0;
			
		IF SecondAxisActualPos = SecondAxisNextPos THEN
			SecondAxisDetectedPos := SecondAxisNextPos + 6;
			
			GVL.SpeedMultiplier:=0.000000000001;
			GVL.stAxis2ControlSignals.bJogForward:= 1;
			secondAxisRAF:= 30;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF
		END_IF
	
	30: IF (SecondAxisActualPos = SecondAxisPrevPos) OR (SecondAxisActualPos <> SecondAxisNextPos) THEN
		GVL.stAxis2ControlSignals.bJogForward:= 0;
	
		IF SecondAxisActualPos = SecondAxisPrevPos THEN
			secondAxisRAF:= 200;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF	
		END_IF
	
	100: IF (SecondAxisActualPos = SecondAxisNextPos) OR (SecondAxisActualPos <> SecondAxisPrevPos) THEN
			GVL.stAxis2ControlSignals.bJogForward := 0;
			
		IF SecondAxisActualPos = SecondAxisNextPos THEN
			SecondAxisDetectedPos := SecondAxisNextPos;
			
			GVL.SpeedMultiplier:=0.000000000001;
			GVL.stAxis2ControlSignals.bJogBackward:= 1;
			
			secondAxisRAF:= 110;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF
	END_IF
	
	110: IF (SecondAxisActualPos = SecondAxisPrevPos) OR (SecondAxisActualPos <> SecondAxisNextPos) THEN
		GVL.stAxis2ControlSignals.bJogBackward:= 0;
	
		IF SecondAxisActualPos = SecondAxisPrevPos THEN
			GVL.stAxis2ControlSignals.bJogForward:= 1;
			secondAxisRAF:= 120;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF	
		END_IF
	
	120: IF (SecondAxisActualPos = SecondAxisNextPos) OR (SecondAxisActualPos <> SecondAxisPrevPos) THEN
		GVL.stAxis2ControlSignals.bJogForward:= 0;
	
		IF SecondAxisActualPos = SecondAxisNextPos THEN
			secondAxisRAF:= 200;
		ELSE 
			decodeError:=1; //error bad decoding
		END_IF	
		END_IF	
		
	200: IF NOT(HomingDoneAxis2) AND TimerSecondAxis.Q  THEN
		
		GVL.stAxis2ControlSignals.fDetectedPossition := INT_TO_LREAL(SecondAxisDetectedPos);
		
		GVL.stAxis2ControlSignals.bResetAxisHome :=1;
		
		GVL.SpeedMultiplier := SpeedMultiplierPrevValue;
		
		HomingDoneAxis2 := 1;
	END_IF		
END_CASE


// homing procedure of third axis

TimerThirdAxis(IN:=(thirdAxisRAF=200));
TimerThirdAxisTimeout(IN:=(thirdAxisRAF=10));

CASE thirdAxisRAF OF 
	0:	IF	GVL.stAxis3ControlSignals.bReady AND HomingDoneAxis4 THEN
			SpeedMultiplierPrevValue := gvl.SpeedMultiplier;
			GVL.SpeedMultiplier:=0.1; // 0.1% of max speed
			thirdAxisRAF:=10;
		END_IF

	10: IF  TimerThirdAxisTimeout.Q THEN
		IF ThirdAxisHomeSensor  THEN
			GVL.SpeedMultiplier:=0.00001; // 0.00001% of max speed
			GVL.stAxis3ControlSignals.bJogBackward:= 1;
			thirdAxisRAF:= 30;
		ELSE
			GVL.stAxis3ControlSignals.bJogForward :=1;
			thirdAxisRAF:= 20;
		END_IF
		END_IF

	20:	IF ThirdAxisHomeSensor THEN
			GVL.stAxis3ControlSignals.bJogForward:= 0;
			GVL.SpeedMultiplier:=0.00001; // 0.00001% of max speed
			GVL.stAxis3ControlSignals.bJogBackward:= 1;
			thirdAxisRAF:= 30;
		END_IF

	30: IF NOT(ThirdAxisHomeSensor) THEN
		GVL.stAxis3ControlSignals.bJogBackward:= 0;
		thirdAxisRAF:= 200;
		END_IF

	200: IF NOT(HomingDoneAxis3) AND TimerThirdAxis.Q THEN
		
		GVL.stAxis3ControlSignals.fDetectedPossition := 0;
		
		GVL.stAxis3ControlSignals.bResetAxisHome :=1;
		
		GVL.SpeedMultiplier := SpeedMultiplierPrevValue;
		
		HomingDoneAxis3 := 1;
	END_IF
END_CASE	

// homing procedure of fourth axis

TimerFourthAxis(IN:=(fourthAxisRAF=200));

CASE fourthAxisRAF OF 
	0:	IF	GVL.stAxis4ControlSignals.bReady AND HomingDoneAxis2 THEN
			SpeedMultiplierPrevValue := gvl.SpeedMultiplier;
			GVL.SpeedMultiplier:=50; // 50% of max speed
			fourthAxisRAF:=10;
		END_IF

	10: IF FourthAxisHomeSensor THEN
			GVL.stAxis4ControlSignals.bJogForward :=1;
			fourthAxisRAF:= 20;
		ELSE
			GVL.stAxis4ControlSignals.bJogBackward:= 1;
			fourthAxisRAF:= 100;
			
		END_IF

	20:	IF NOT(FourthAxisHomeSensor) THEN
			GVL.stAxis4ControlSignals.bJogForward:= 0;
			GVL.SpeedMultiplier:=0.0000001; // 0.0000001% of max speed
			GVL.stAxis4ControlSignals.bJogBackward:= 1;
			fourthAxisRAF:= 30;
		END_IF

	30: IF FourthAxisHomeSensor THEN
		GVL.stAxis4ControlSignals.bJogBackward:= 0;
		fourthAxisRAF:= 200;
		END_IF
	
	100: IF FourthAxisHomeSensor THEN
			GVL.stAxis4ControlSignals.bJogBackward:= 0;
			fourthAxisRAF:= 10;
		END_IF	

	200: IF NOT(HomingDoneAxis4) AND TimerFourthAxis.Q THEN
		
		GVL.stAxis4ControlSignals.fDetectedPossition := 0;
		
		GVL.stAxis4ControlSignals.bResetAxisHome :=1;
		
		//GVL.SpeedMultiplier := SpeedMultiplierPrevValue;
		
		HomingDoneAxis4 := 1;
		END_IF
END_CASE


gvl.Home := HomingDoneAxis1 AND HomingDoneAxis2 AND HomingDoneAxis3 AND HomingDoneAxis4;