PROGRAM WinderControl
VAR
unwind_servo : SERVO_DRIVE;
rewind_servo : SERVO_DRIVE;
unwind_speed : REAL := 0.0;
rewind_speed : REAL := 0.0;
speed_ratio : REAL := 1.0;
dancer_pos : REAL;
dancer_pos_filtered : REAL := 0.5;
dancer_setpoint : REAL := 0.5;
tension_PID : PID_CONTROLLER;
tension_error : REAL;
tension_avg : REAL := 0.0;
tension_var : REAL := 0.0;
system_state : (IDLE, RUNNING, FAULT) := IDLE;
emergency_stop : BOOL := FALSE;
start_command : BOOL := FALSE;
reset_command : BOOL := FALSE;
line_speed : REAL := 1.0;
material_thickness : REAL := 0.001;
max_acceleration : REAL := 0.1;
max_tension : REAL := 100.0;
min_tension : REAL := 10.0;
max_tension_var : REAL := 50.0;
material_break_threshold : REAL := 0.01;
END_VAR
IF FIRST_SCAN THEN
tension_PID.Kp := 1.0;
tension_PID.Ki := 0.1;
tension_PID.Kd := 0.01;
tension_PID.Ts := 0.01;
system_state := IDLE;
END_IF
CASE system_state OF
IDLE:
IF start_command THEN
system_state := RUNNING;
END_IF
RUNNING:
RunControlLogic();
IF emergency_stop OR
tension_error > max_tension OR
tension_error < min_tension OR
tension_var > max_tension_var OR
ABS(dancer_pos_filtered - dancer_setpoint) > material_break_threshold THEN
system_state := FAULT;
END_IF
FAULT:
EmergencyStop();
IF reset_command THEN
system_state := IDLE;
END_IF
END_CASE
METHOD RunControlLogic : BOOL
VAR_INPUT
END_VAR
VAR
speed_cmd : REAL;
accel_time : REAL;
END_VAR
speed_cmd := line_speed / (2 * 3.14159 * unwind_servo.radius);
unwind_speed := speed_cmd;
rewind_speed := speed_cmd * speed_ratio;
IF ABS(unwind_speed - rewind_speed) > 0.01 THEN
speed_ratio := unwind_speed / rewind_speed;
END_IF
dancer_pos_filtered := 0.8 * dancer_pos_filtered + 0.2 * dancer_pos;
tension_error := dancer_setpoint - dancer_pos_filtered;
tension_PID.PV := dancer_pos_filtered;
tension_PID.SP := dancer_setpoint;
IF ABS(tension_error) > 0.1 THEN
tension_PID.Kp := 2.0;
tension_PID.Ki := 0.2;
ELSIF ABS(tension_error) > 0.05 THEN
tension_PID.Kp := 1.0;
tension_PID.Ki := 0.1;
ELSE
tension_PID.Kp := 0.5;
tension_PID.Ki := 0.05;
END_IF
tension_PID.CYCLE();
tension_avg := 0.9 * tension_avg + 0.1 * tension_error;
tension_var := 0.9 * tension_var + 0.1 * (tension_error - tension_avg) * (tension_error - tension_avg);
unwind_speed := unwind_speed + tension_PID.CV;
rewind_speed := rewind_speed - tension_PID.CV;
unwind_speed := LIMIT(unwind_speed, 0.0, 10.0);
rewind_speed := LIMIT(rewind_speed, 0.0, 10.0);
unwind_servo.SetSpeed(unwind_speed);
rewind_servo.SetSpeed(rewind_speed);
RunControlLogic := TRUE;
END_METHOD
METHOD EmergencyStop : BOOL
VAR_INPUT
END_VAR
unwind_servo.Stop();
rewind_servo.Stop();
emergency_stop := TRUE;
start_command := FALSE;
EmergencyStop := TRUE;
END_METHOD