FUNCTION FC500 : VOID

// This function is used for bridle speed calculation. This speed is used for elongation correction elaboration

TITLE = 'AEC_BS_Speed_Cal'
VERSION : '0.0'
//KNOW_HOW_PROTECT
AUTHOR : BenBen
NAME : BS_Speed
FAMILY : Common

VAR_INPUT
// Input
Pulse_Nb : INT; // Pulse number
Turn_Nb : INT; // Turn number
// Parameter
Cycle_Time : INT; // Cycle time in ms
Gear_Ratio : REAL;
BS_Diam : REAL; // Bridle roll diameter in mm
END_VAR


VAR_IN_OUT
BS_Pos_Last : REAL; // Last position (last cycle)
Turn_Nb_Last : INT; // Last turn number (last cycle)
END_VAR


VAR_TEMP
BS_Pos : REAL; // Bridle roll position in degree
Angle : REAL; // Angle executed during cycle time
Turn_Nb_Cycle : REAL; // Turn number during cycle time
Deg_Rad_Val : REAL; // If degree is selected parameter = 360 else parameter = 2*3.1456
END_VAR


VAR_OUTPUT
BS_Speed_Rad : REAL; // Bridle speed in Rad/s
BS_Speed_Lin : REAL; // Bridle linear speed in mm/s
END_VAR





BEGIN

// Bridle roll position
IF Pulse_Nb > 0
THEN BS_Pos := Pulse_Nb / 32767 * 180 ;
ELSE BS_Pos := (-Pulse_Nb / 32767 * 180) + 180;
END_IF;


// Turn number during one cycle time
Turn_Nb_Cycle := Turn_Nb - Turn_Nb_Last;
IF Turn_Nb_Cycle < -10000
THEN Turn_Nb_Cycle := Turn_Nb_Cycle + 32767;
ELSIF Turn_Nb_Cycle > 10000
THEN Turn_Nb_Cycle := Turn_Nb_Cycle - 32768;
END_IF;


// Angle executed during cycle time
Angle := Turn_Nb_Cycle * 360 + BS_Pos - BS_Pos_Last;


// Bridle speed in Rad/s
BS_Speed_Rad := (Angle * 1000 * 2 * 3.14) / (Cycle_Time * Gear_Ratio * 360) ;
// Bridle linear speed in mm/s
BS_Speed_Lin := BS_Speed_Rad * BS_Diam / 2;


// Bridle position and turn number memorizing
BS_Pos_Last:= BS_Pos;
Turn_Nb_Last := Turn_Nb;


END_FUNCTION