Obsidean_VM/04-InLavoro/22 - 9.3841 - Sidel - Tilting/Software/FB626 - FB Calculate Diff Phase Pulses.md

#"Sensor Master Pulse" := #"Master Clock" AND NOT #Aux.OnsM;
#Aux.OnsM := #"Master Clock";

#"Sensor Slave Pulse" := #"Slave Clock" AND NOT #Aux.OnsS;
#Aux.OnsS := #"Slave Clock";

#ElapsedRuntime_s := RUNTIME(#Aux.TimeRecorded);
#Aux."Time Pass" := #Aux."Time Pass" + #ElapsedRuntime_s;

IF #Reset THEN
    #Aux."Time Pass" := 0;
END_IF;

IF #"Master b/h" = 0 THEN
    RETURN;
END_IF;

#"Estimated Master half Clock s" := ( 3600.0 / #"Master b/h" ) / 2.0;

IF #"Sensor Master Pulse" THEN
    #Aux."Time Pulse Sensor M" := #Aux."Time Pass";
END_IF;

IF #"Sensor Slave Pulse" THEN
    IF ((#Aux."Time Pass" - #Aux."Time Pulse Sensor M") > #"Estimated Master half Clock s") THEN
        #"Diff Phase Time" := (2 * #"Estimated Master half Clock s") - (#Aux."Time Pass" - #Aux."Time Pulse Sensor M");
        #PhaseAngle := (#"Diff Phase Time" / (2 * #"Estimated Master half Clock s")) * 3.14159; // Pi
        #"Phase Error" := -SIN(#PhaseAngle);
    ELSE
        #"Diff Phase Time" := #Aux."Time Pass" - #Aux."Time Pulse Sensor M";
        #PhaseAngle := (#"Diff Phase Time" / (2 * #"Estimated Master half Clock s")) * 3.14159; // Pi
        #"Phase Error" := SIN(#PhaseAngle);
        
    END_IF;
END_IF;

#"Diff Phase %" := 100.0 * #"Phase Error" ;