FUNCTION_BLOCK FB150
TITLE = 'friction control'
VERSION : '0.0'
VAR_INPUT
Enable:BOOL;
Speed_In:INT;//Dimension?Value
Torque_In:INT;//Dimension?Value
Clock:BOOL;//System?Clock
END_VAR
VAR_OUTPUT
Run_CMD:BOOL;
Error:BOOL;
Speed_Ref:INT;
Torque_Ref:INT;
Group1:ARRAY[0..20]OF REAL;
Group2:ARRAY[0..20]OF REAL;
Group3:ARRAY[0..20]OF REAL;
Group4:ARRAY[0..20]OF REAL;
Group5:ARRAY[0..20]OF REAL;
END_VAR
VAR
Index1:INT;
Index2:INT;
Index3:INT;
Index4:INT;
Index:INT;
Value1:ARRAY[1..100]OF REAL;
Value2:REAL;
Speed_Dimension:REAL;//this?value?depend?on?driver
Torque_Dimension:REAL;//this?value?depend?on?driver
speed:REAL;
torque:REAL;
X01:BOOL;
Speed_R:REAL;
END_VAR
Speed_Dimension:=20000.0;
Torque_Dimension:=10000.0;
Speed:=(INT_TO_REAL(Speed_In)/Speed_Dimension)*100.0;
Torque:=(INT_TO_REAL(Torque_In)/Torque_Dimension)*100.0;
IF Enable THEN
Run_CMD:=True;
Torque_Ref:=REAL_TO_INT(Torque_Dimension);
IF NOT X01 AND Clock THEN
Index2:=Index2+1;
END_IF;
IF (Index2=1) AND NOT Clock AND X01 THEN
FOR Index1:= 0 TO 20 BY 1 DO
Group5[Index1]:=Group4[Index1];
Group4[Index1]:=Group3[Index1];
Group3[Index1]:=Group2[Index1];
Group2[Index1]:=Group1[Index1];
END_FOR;
END_IF;
IF Index2=1 THEN
Speed_R:=INT_TO_REAL(Index2);
Speed_Ref:=REAL_TO_INT((Speed_R/100.0)*Speed_Dimension);
END_IF;
IF (Index2>2) AND (Index2<5) THEN
IF ABS(speed-Speed_R)>1.0 THEN
Error:=True;
ELSE
Value1[1]:=torque;
FOR Index3:= 1 TO 99 BY 1 DO
Value1[Index3+1]:=Value1[Index3];
END_FOR;
Value2:=0;
FOR Index4:= 1 TO 100 BY 1 DO
Value2:=Value2+Value1[Index4];
END_FOR;
Group1[0]:=Value2/100.0;
END_IF;
END_IF;
FOR Index:= 0 TO 95 BY 5 DO
IF Index2=Index THEN
Speed_R:=INT_TO_REAL(Index2);
Speed_Ref:=REAL_TO_INT(Speed_R/100.0*Speed_Dimension);
END_IF;
IF Index2>(Index+1) AND Index2<(Index+4) THEN
IF ABS(speed-Speed_R)>1.0 THEN
Error:=True;
ELSE
Value1[1]:=torque;
FOR Index3:= 1 TO 99 BY 1 DO
Value1[Index3+1]:=Value1[Index3];
END_FOR;
Value2:=0;
FOR Index4:= 1 TO 100 BY 1 DO
Value2:=Value2+Value1[Index4];
END_FOR;
Group1[Index/5]:=Value2/100.0;
END_IF;
END_IF;
END_FOR;
IF Index2=100 THEN
Speed_R:=INT_TO_REAL(Index2);
Speed_Ref:=REAL_TO_INT(Speed_R/100.0*Speed_Dimension);
END_IF;
IF Index2>102 AND Index2<105 THEN
IF ABS(speed-Speed_R)>1.0 THEN
Error:=True;
ELSE
Value1[1]:=torque;
FOR Index3:= 1 TO 99 BY 1 DO
Value1[Index3+1]:=Value1[Index3];
END_FOR;
Value2:=0;
FOR Index4:= 1 TO 100 BY 1 DO
Value2:=Value2+Value1[Index4];
END_FOR;
Group1[20]:=Value2/100.0;
END_IF;
END_IF;
IF Index2>=105 THEN
Run_CMD:=False;
Speed_Ref:=0;
Torque_Ref:=0;
END_IF;
END_IF;
IF NOT Enable THEN
Index:=0;
Index1:=0;
Index2:=0;
Index3:=0;
Index4:=0;
Run_CMD:=false;
Speed_Ref:=0;
Torque_Ref:=0;
END_IF;
X01:=Clock;
END_FUNCTION_BLOCK