Comunicación Profibus entre un Beckhoff CX9010 y un servomotor Festo MTR-DCI (parte 2ª)

Teniendo el servomotor configurado y comunicando en la primera parte, vamos ahora con la programación.

En el CD que acompaña al servomotor viene un ejemplo de programación para Step7 y no dice nada de Beckhoff (ni de ningún otro fabricante). Explorando un poco el ejemplo veo que el control se hace a través de un FB escrito en lista de instrucciones (AWL), que afortunadamente no está protegido y puedo analizarlo: lo que se hace es leer 8 bytes de entrada a través de Profibus, se interpretan según el modo de funcionamiento del servomotor, se codifican los posibles comandos al servomotor en 8 bytes de salida y finalmente se transmiten por Profibus. Lo que voy a hacer es 'traducir' el código en AWL de Step7 a lenguaje ST para el CX9010.

ADVERTENCIA: El código que presento a continuación lo he probado únicamente utilizando el servomotor en posicionamiento absoluto. Para los demás modos de funcionamiento debería funcionar, he procurado ser meticuloso a la hora de transcribir el código, pero puede haber errores. Si usas este código y te funciona, si le haces modificaciones o si encuentras un error te agradecería que me lo comuniques.

Para empezar voy a declarar los tipos de datos necesarios. He intentado respetar al máximo la programación hecha en Step7 por Festo, los nombres de las variables son un calco y su funcionalidad idéntica.

Primero vamos a estructurar los datos de entrada/salida Profibus. En el apartado 'Data types' del TwinCAT PLC Control creo, para la recepción de datos, MTR_DCI_INPUT_DATA:

(******************************************************************************
Se usa como variable interna del FB BECKHOFF_MTR_DCI_CTRL
******************************************************************************)

TYPE MTR_DCI_INPUT_DATA :
STRUCT

SCON AT %IB0: BYTE;
SPOS AT %IB1: BYTE;
BYTE_3 AT %IB2: BYTE;
BYTE_4 AT %IB3: SINT;
Actual_Value_2 AT %ID4: DINT;

END_STRUCT
END_TYPE

Y para el envío de datos MTR_DCI_OUTPUT_DATA:

(******************************************************************************
Se usa como variable interna del FB BECKHOFF_MTR_DCI_CTRL
******************************************************************************)

TYPE MTR_DCI_OUTPUT_DATA :
STRUCT

CCON AT %QB0: BYTE;
CPOS AT %QB1: BYTE;
BYTE_3 AT %QB2: BYTE;
Byte_4 AT %QB3: BYTE;
Set_Value_2 AT %QD4: DINT;

END_STRUCT
END_TYPE

El avezado lector se habrá dado cuenta de que especifico un área de entradas y salidas, respectivamente, que coincide con el área que le había asignado como entrada/salida Profibus del servomotor: son los 8 primeros bytes de entradas y salidas, pero en diferente formato (ver la primera parte).

En el apartado 'Data types' también voy a declarar una estructura que voy a usar para dar las órdenes al servomotor y leer su estado. Será mi interfaz desde el programa hacia el servomotor.

(* Estructura donde se interactuará desde programa para controlar el servomotor *)

TYPE DATA_MTR_DCI :
STRUCT

HMI_Access_locked: BOOL;
Reset_Fault: BOOL;
Halt: BOOL;
Stop: BOOL;
Enable_Drive: BOOL;
Start_Homing: BOOL;
Start_Task: BOOL;
Operation_Mode: BOOL;
Control_Mode: BOOL;
Deactivate_Stroke_Limit: BOOL;
reserved: BOOL;
Stroke_Limit_Reached: BOOL;
Velocity_Limit_Reached: BOOL;
Absolute_Relative: BOOL;
Clear_Rem_Position: BOOL;
Jog_neg: BOOL;
Jog_pos: BOOL;
Teach_Actual_Value: BOOL;
Ack_Start: BOOL;
Actual_Working_Mode: BOOL;
Halt_Not_Active: BOOL;
Control_FCT_HMI: BOOL;
Drag_Error: BOOL;
Drive_Enabled: BOOL;
Fault: BOOL;
MC: BOOL;
Ready: BOOL;
Standstill_Control: BOOL;
Supply_Voltage_Ok: BOOL;
Drive_is_Referenced: BOOL;
Warning: BOOL;
State_Operation_Mode: BOOL;
Ack_Teach: BOOL;
Drive_is_Moving: BOOL;
State_Control_Mode: BOOL;
Actual_Velocity: INT;
Set_Value_Velocity: INT;
Set_Value_Force: INT;
Actual_Force: INT;
Record_No: INT;
Actual_Record_No: INT;
RET_VALUE: INT;
Set_Value_Position: DINT;
Actual_Position:DINT;

END_STRUCT
END_TYPE

Hasta aquí los tipos de datos. Ahora, en el apartado de variables globales voy a añadir la declaración de esta última estructura, que se añadirá a las tablas (arrays) que ya había creado en la primera parte.

VAR_GLOBAL

(* Variables para controlar el servomotor *)
MTR_DCI_E AT %IB0: ARRAY[0..7] OF USINT;
MTR_DCI_A AT %QB0: ARRAY[0..7] OF USINT;
SERVO: DATA_MTR_DCI;

END_VAR

A continuación vamos con el código del bloque de función, al que he llamado BECKHOFF_MTR_DCI_CTRL, que va a controlar el servomotor; primero la declaración de variables:

FUNCTION_BLOCK BECKHOFF_MTR_DCI_CTRL

(******************************************************************************
Adaptación del FB de Festo en Step7 para controlar un servomotor MTR-DCI
en un PLC Beckhoff a través de Profibus

Adaptado por GR - notasdeautomatizacion.blogspot.com
******************************************************************************)

VAR_INPUT

I_ADDRESS: WORD;
O_ADDRESS: WORD;

Pos_Factor_numerator: DINT;
Pos_Factor_denumerator: DINT;

HMI_Access_Locked: BOOL; (* SPS Control == 1 / HMI Control == 0 *)
Reset_Fault: BOOL;
Halt: BOOL; (* Halt aktiv == 0 / Halt not aktiv == 1 *)
Stop: BOOL;
Enable_Drive: BOOL;
Start_Homing: BOOL;
Start_Task: BOOL;
Operation_Mode: BOOL; (* Recordmode == 0 / Direktmode == 1 *)
Control_Mode: BOOL; (* Position control == 0 / Force control == 1 *)
Deactivate_Stroke_Limit: BOOL;
Absolute_Relativ: BOOL; (* Absolute == 0 / Relativ == 1 *)
Clear_Remaining_Position: BOOL;
Jog_pos: BOOL;
Jog_neg: BOOL;
Teach_Actual_Value: BOOL;
Record_No: INT;
Set_Value_Velocity: INT;
Set_Value_Force: INT;
Set_Value_Position: DINT;

END_VAR

VAR_OUTPUT

Drive_Control_FCT_HMI: BOOL; (* 0 == Access MMI, 1 == SPS *)
Drive_Enabled: BOOL; (* 0 == Disable, 1 == Enable *)
Supply_Voltage_Present: BOOL; (* 0 == not present, 1 == present *)
Warning: BOOL;
Fault: BOOL;
Ready: BOOL;
State_Operating_Mode: BOOL; (* 0 == Recordmode, 1 == Direktmode *)
State_Control_Mode: BOOL; (* Position control == 0 / Force control == 1 *)
Ack_Start: BOOL;
Ack_Teach: BOOL;
MC: BOOL;
Drive_is_moving: BOOL;
Halt_Not_Active: BOOL;
Drag_Error: BOOL;
Standstill_control: BOOL;
Drive_is_referenced: BOOL;
Stroke_Limit_Reached: BOOL;
Velocity_Limit_Reached: BOOL;
Actual_Record_No: INT;
Actual_Velocity: INT;
Actual_Force: INT;
Actual_Position: DINT;
RET_VALUE: INT;

END_VAR

VAR

INPUT_DATA: MTR_DCI_INPUT_DATA;
OUTPUT_DATA: MTR_DCI_OUTPUT_DATA;

Save_AR1: DWORD; (* NO USADO *)
SFC14_15_RET_VAL: INT; (* NO USADO *)
position_factor: LREAL;

(* Variables para reordenar bytes *)
Aux_DInt AT %MD0: ARRAY [0..1] OF DINT;
Aux_Int AT %MW0: ARRAY [0..3] OF INT;
Aux_Byte AT %MB0: ARRAY[0..7] OF BYTE;

END_VAR

ADVERTENCIA: En la zona de marcas reservo los primeros 8 bytes para reordenar bytes de datos, así que debemos tener cuidado de no escribir ahí desde ninguna otra parte del programa.

El código propiamente dicho del FB es este:

(* Se comprueban los parámetros *)
IF Pos_Factor_numerator <= 0 OR Pos_Factor_denumerator <= 0 THEN
RET_VALUE := -1;
RETURN;
END_IF;

(* Calculation of the position tolerance factor *)
position_factor := DINT_TO_LREAL(Pos_Factor_numerator) / DINT_TO_LREAL(Pos_Factor_denumerator);

(* Step7: Read consistent data from a standard DP slave *)

(******************************************************************************
No existe una función para leer datos de un esclavo DP
Se asigna una zona de entradas en el TwinCAT System Manager
Entradas desde el byte 0 al 7, variable global MTR_DCI_E
Se mapea en la misma zona de memoria la estructura MTR_DCI_INPUT_DATA
******************************************************************************)

(* Asignación de las variables de salida del servomotor (Byte 1 - SCON) *)
State_Operating_Mode := (INPUT_DATA.SCON AND 2#0100_0000) > 0; (* Bit 6 del primer byte *)
Drive_Control_FCT_HMI := (INPUT_DATA.SCON AND 2#0010_0000) > 0; (* Bit 5 del primer byte *)
Supply_Voltage_Present := (INPUT_DATA.SCON AND 2#0001_0000) > 0; (* Bit 4 del primer byte *)
Fault := (INPUT_DATA.SCON AND 2#0000_1000) > 0; (* Bit 3 del primer byte *)
Warning := (INPUT_DATA.SCON AND 2#0000_0100) > 0; (* Bit 2 del primer byte *)
Ready := (INPUT_DATA.SCON AND 2#0000_0010) > 0; (* Bit 1 del primer byte *)
Drive_Enabled := (INPUT_DATA.SCON AND 2#0000_0001) > 0; (* Bit 0 del primer byte *)

(* Asignación de las variables de salida (Byte 2 - SPOS) *)
Drive_is_referenced := (INPUT_DATA.SPOS AND 2#1000_0000) > 0; (* Bit 7 del segundo byte *)
Standstill_control := (INPUT_DATA.SPOS AND 2#0100_0000) > 0; (* Bit 6 del segundo byte *)
Drag_Error := (INPUT_DATA.SPOS AND 2#0010_0000) > 0; (* Bit 5 del segundo byte *)
Drive_is_moving := (INPUT_DATA.SPOS AND 2#0001_0000) > 0; (* Bit 4 del segundo byte *)
Ack_Teach := (INPUT_DATA.SPOS AND 2#0000_1000) > 0; (* Bit 3 del segundo byte *)
MC := (INPUT_DATA.SPOS AND 2#0000_0100) > 0; (* Bit 2 del segundo byte *)
Ack_Start := (INPUT_DATA.SPOS AND 2#0000_0010) > 0; (* Bit 1 del segundo byte *)
Halt_Not_Active := (INPUT_DATA.SPOS AND 2#0000_0001) > 0; (* Bit 0 del segundo byte *)

(* Dependiendo del modo de operación, los bytes 3 y 4 cambian de contenido *)
IF State_Operating_Mode THEN
(* Direktmode *)

(* Asignación de las variables de salida (Byte 3 - SDIR) *)
Stroke_Limit_Reached := (INPUT_DATA.BYTE_3 AND 2#0010_0000) > 0; (* Bit 5 del tercer byte *)
Velocity_Limit_Reached := (INPUT_DATA.BYTE_3 AND 2#0001_0000) > 0; (* Bit 4 del tercer byte *)
State_Control_Mode := (INPUT_DATA.BYTE_3 AND 2#0000_0010) > 0; (* Bit 1 del tercer byte *)
Actual_Record_No := 0; (* No hay 'Recordnumber' en 'Direktmode' *)

(* Asignación de las variables de salida (Byte 4) *)
(* Dependiendo del modo de control, se lee velocidad o fuerza *)
IF State_Control_Mode THEN
(* Force control *)
Actual_Force := SINT_TO_INT(INPUT_DATA.BYTE_4);
Actual_Velocity := 0;
ELSE
(* Position control *)
Actual_Velocity := SINT_TO_INT(INPUT_DATA.BYTE_4);
Actual_Force := 0;
END_IF;

ELSE
(* Recordmode *)
Actual_Record_No := BYTE_TO_INT(INPUT_DATA.BYTE_3);
Actual_Velocity := 0; (* No se transmite velocidad en 'Recordmode' *)
END_IF;

(* Lectura del valor de posición *)
(* INPUT_DATA.Actual_Value_2 no se puede leer directamente, el orden de los bytes es incorrecto *)
Aux_Byte[0] := MTR_DCI_E[7];
Aux_Byte[1] := MTR_DCI_E[6];
Aux_Byte[2] := MTR_DCI_E[5];
Aux_Byte[3] := MTR_DCI_E[4];
Actual_Position := LREAL_TO_DINT(DINT_TO_LREAL(Aux_DInt[0]) / position_factor);

(*****************************************************************************)

(* Asignación de las variables de entrada (Byte 1 - CCON) *)
(* Bit 2 (Open Break), Bit 4 (Reservado), y Bit 7 (OPM2) a cero *)
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON AND 2#0110_1011;

IF Enable_Drive THEN
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON OR 2#0000_0001;
ELSE
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON AND 2#1111_1110;
END_IF;

IF Stop THEN
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON OR 2#0000_0010;
ELSE
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON AND 2#1111_1101;
END_IF;

IF Reset_Fault THEN
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON OR 2#0000_1000;
ELSE
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON AND 2#1111_0111;
END_IF;

IF HMI_Access_Locked THEN
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON OR 2#0010_0000;
ELSE
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON AND 2#1101_1111;
END_IF;

IF Operation_Mode THEN
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON OR 2#0100_0000;
ELSE
OUTPUT_DATA.CCON := OUTPUT_DATA.CCON AND 2#1011_1111;
END_IF;

(* Asignación de las variables de entrada (Byte 2 - CPOS) *)
(* Bit 7 (Reservado) a cero *)
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#0111_1111;

IF Clear_Remaining_Position THEN
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS OR 2#0100_0000;
ELSE
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#1011_1111;
END_IF;

IF Teach_Actual_Value THEN
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS OR 2#0010_0000;
ELSE
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#1101_1111;
END_IF;

IF Jog_neg THEN
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS OR 2#0001_0000;
ELSE
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#1110_1111;
END_IF;

IF Jog_pos THEN
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS OR 2#0000_1000;
ELSE
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#1111_0111;
END_IF;

IF Start_Homing THEN
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS OR 2#0000_0100;
ELSE
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#1111_1011;
END_IF;

IF Start_Task THEN
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS OR 2#0000_0010;
ELSE
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#1111_1101;
END_IF;

IF Halt THEN
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS OR 2#0000_0001;
ELSE
OUTPUT_DATA.CPOS := OUTPUT_DATA.CPOS AND 2#1111_1110;
END_IF;

(* Dependiendo del modo de operación, los bytes 3 y 4 cambian de contenido *)
IF State_Operating_Mode THEN
(* Direktmode *)

(* Asignación de las variables de entrada (Byte 3 - CDIR) *)
OUTPUT_DATA.BYTE_3 := 0; (* Se pone a cero el byte 3 *)

IF Deactivate_Stroke_Limit THEN
OUTPUT_DATA.BYTE_3 := OUTPUT_DATA.BYTE_3 OR 2#0010_0000;
ELSE
OUTPUT_DATA.BYTE_3 := OUTPUT_DATA.BYTE_3 AND 2#1101_1111;
END_IF;

IF Control_Mode THEN
OUTPUT_DATA.BYTE_3 := OUTPUT_DATA.BYTE_3 OR 2#0000_0010;
ELSE
OUTPUT_DATA.BYTE_3 := OUTPUT_DATA.BYTE_3 AND 2#1111_1101;
END_IF;

IF Absolute_Relativ THEN
OUTPUT_DATA.BYTE_3 := OUTPUT_DATA.BYTE_3 OR 2#0000_0001;
ELSE
OUTPUT_DATA.BYTE_3 := OUTPUT_DATA.BYTE_3 AND 2#1111_1110;
END_IF;

(* Asignación de las variables de entrada (Byte 4) *)
(* Velocidad en porcentaje - entre 0% y 100% *)
IF Set_Value_Velocity > 100 THEN
OUTPUT_DATA.Byte_4 := 100;
ELSIF Set_Value_Velocity < 0 THEN
OUTPUT_DATA.Byte_4 := 0;
ELSE
OUTPUT_DATA.Byte_4 := INT_TO_BYTE(Set_Value_Velocity);
END_IF;

(* Asignación de las variables de entrada (Bytes 5-8) *)
IF Operation_Mode AND Control_Mode THEN
(* Force control *)
(* Fuerza en porcentaje - entre -100% y 100% *)
IF Set_Value_Force > 100 THEN
OUTPUT_DATA.Set_Value_2 := 100;
ELSIF Set_Value_Force < -100 THEN
OUTPUT_DATA.Set_Value_2 := -100;
ELSE
OUTPUT_DATA.Set_Value_2 := INT_TO_DINT(Set_Value_Force);
END_IF;
ELSE
(* Position control *)
(* Hay que cambiar el orden de los bytes *)
Aux_DInt[1] := LREAL_TO_DINT(DINT_TO_LREAL(Set_Value_Position) * position_factor);
Aux_Byte[0] := Aux_Byte[7];
Aux_Byte[1] := Aux_Byte[6];
Aux_Byte[2] := Aux_Byte[5];
Aux_Byte[3] := Aux_Byte[4];
OUTPUT_DATA.Set_Value_2 := Aux_DInt[0];
END_IF;

ELSE
(* Recordmode *)

OUTPUT_DATA.BYTE_3 := INT_TO_BYTE(Record_No);
OUTPUT_DATA.BYTE_4 := 0; (* Reservado *)
OUTPUT_DATA.Set_Value_2 := 0;

END_IF;

(* Step7: Write consistent data to a standard DP slave *)

(******************************************************************************
No existe una función para escribir datos de un esclavo DP
Se asigna una zona de salidas en el TwinCAT System Manager
Salidas desde el byte 0 al 7, variable global MTR_DCI_A
Se mapea en el mismo área la estructura MTR_DCI_OUTPUT_DATA
******************************************************************************)

RET_VALUE := 0;

Antes de mostrar la llamada al FB hay que explicar un par de variables: SERVOMOTOR_CTRL.Pos_Factor_numerator y SERVOMOTOR_CTRL.Pos_Factor_denumerator. Del servomotor se leen pulsos del encóder (1200 pulsos por vuelta), que debemos convertir a las unidades que nos interesen. Esta conversión suele ser una fración y podemos especificarle el numerador y el denominador para que nos haga la conversión. Si no nos interesa le escribimos sendos unos y tenemos el valor real de pulsos. Aclarado este punto, vamos con la llamada al FB desde el programa principal, sería como sigue:

PROGRAM PRINCIPAL

VAR
(* Instancia del FB BECKHOFF_MTR_DCI_CTRL *)
SERVOMOTOR_CTRL: BECKHOFF_MTR_DCI_CTRL;
END_VAR

(* Control del servomotor *)
(* Llamada al FB que lee y escribe en el servomotor a través de Profibus *)

(* Parámetros de entrada del FB *)
SERVOMOTOR_CTRL.I_ADDRESS := 0; (* NO USADO *)
SERVOMOTOR_CTRL.O_ADDRESS := 0; (* NO USADO *)
SERVOMOTOR_CTRL.Pos_Factor_numerator := 1323000; (* 3969 * 100 * 1200 * 1000 (milésimas de grado) / 360 *)
SERVOMOTOR_CTRL.Pos_Factor_denumerator := 289000; (* 289 * 1 * 360 / 360 *)
SERVOMOTOR_CTRL.HMI_Access_Locked := SERVO.HMI_Access_locked;
SERVOMOTOR_CTRL.Reset_Fault := SERVO.Reset_Fault;
SERVOMOTOR_CTRL.Halt := SERVO.Halt;
SERVOMOTOR_CTRL.Stop := SERVO.Stop;
SERVOMOTOR_CTRL.Enable_Drive := SERVO.Enable_Drive;
SERVOMOTOR_CTRL.Start_Homing := SERVO.Start_Homing;
SERVOMOTOR_CTRL.Start_Task := SERVO.Start_Task;
SERVOMOTOR_CTRL.Operation_Mode := SERVO.Operation_Mode;
SERVOMOTOR_CTRL.Control_Mode := SERVO.Control_Mode;
SERVOMOTOR_CTRL.Deactivate_Stroke_Limit := SERVO.Deactivate_Stroke_Limit;
SERVOMOTOR_CTRL.Absolute_Relativ := SERVO.Absolute_Relative;
SERVOMOTOR_CTRL.Clear_Remaining_Position := SERVO.Clear_Rem_Position;
SERVOMOTOR_CTRL.Jog_pos := SERVO.Jog_pos;
SERVOMOTOR_CTRL.Jog_neg := SERVO.Jog_neg;
SERVOMOTOR_CTRL.Teach_Actual_Value := SERVO.Teach_Actual_Value;
SERVOMOTOR_CTRL.Record_No := SERVO.Record_No;
SERVOMOTOR_CTRL.Set_Value_Velocity := SERVO.Set_Value_Velocity;
SERVOMOTOR_CTRL.Set_Value_Force := SERVO.Set_Value_Force;
SERVOMOTOR_CTRL.Set_Value_Position := SERVO.Set_Value_Position;

(* Llamada al FB *)
SERVOMOTOR_CTRL();

(* Parámetros de salida del FB *)
SERVO.Control_FCT_HMI := SERVOMOTOR_CTRL.Drive_Control_FCT_HMI;
SERVO.Drive_Enabled := SERVOMOTOR_CTRL.Drive_Enabled;
SERVO.Supply_Voltage_Ok := SERVOMOTOR_CTRL.Supply_Voltage_Present;
SERVO.Warning := SERVOMOTOR_CTRL.Warning;
SERVO.Fault := SERVOMOTOR_CTRL.Fault;
SERVO.Ready := SERVOMOTOR_CTRL.Ready;
SERVO.State_Operation_Mode := SERVOMOTOR_CTRL.State_Operating_Mode;
SERVO.State_Control_Mode := SERVOMOTOR_CTRL.State_Control_Mode;
SERVO.Ack_Start := SERVOMOTOR_CTRL.Ack_Start;
SERVO.Ack_Teach := SERVOMOTOR_CTRL.Ack_Teach;
SERVO.MC := SERVOMOTOR_CTRL.MC;
SERVO.Drive_is_Moving := SERVOMOTOR_CTRL.Drive_is_moving;
SERVO.Halt_Not_Active := SERVOMOTOR_CTRL.Halt_Not_Active;
SERVO.Drag_Error := SERVOMOTOR_CTRL.Drag_Error;
SERVO.Standstill_Control := SERVOMOTOR_CTRL.Standstill_control;
SERVO.Drive_is_Referenced := SERVOMOTOR_CTRL.Drive_is_referenced;
SERVO.Stroke_Limit_Reached := SERVOMOTOR_CTRL.Stroke_Limit_Reached;
SERVO.Velocity_Limit_Reached := SERVOMOTOR_CTRL.Velocity_Limit_Reached;
SERVO.Actual_Record_No := SERVOMOTOR_CTRL.Actual_Record_No;
SERVO.Actual_Velocity := SERVOMOTOR_CTRL.Actual_Velocity;
SERVO.Actual_Force := SERVOMOTOR_CTRL.Actual_Force;
SERVO.Actual_Position := SERVOMOTOR_CTRL.Actual_Position;
SERVO.RET_VALUE := SERVOMOTOR_CTRL.RET_VALUE;
(* Fin de la llamada al FB que lee y escribe en el servomotor a través de Profibus *)

Para finalizar, en el siguiente código he puesto los comandos mínimos para poner el servomotor operativo. Voy a suponer que el servomotor tiene una configuración similar a la mostrada en esta entrada.

(* Control del servomotor *)
SERVO.HMI_Access_locked := FALSE;
SERVO.Operation_Mode := TRUE;
SERVO.Control_Mode := FALSE;
SERVO.Deactivate_Stroke_Limit := FALSE;
SERVO.Absolute_Relative := FALSE;
SERVO.Clear_Rem_Position := FALSE;
SERVO.Teach_Actual_Value := FALSE;
SERVO.Record_No := 0; (* NO USADO *)
SERVO.Set_Value_Force := 0; (* NO USADO *)
SERVO.Halt := TRUE;
SERVO.Stop := TRUE;
SERVO.Enable_Drive := TRUE;

Para empezar a mover el servomotor lo más sencillo es forzar los valores SERVO.Jog_pos o SERVO.Jog_neg desde el entorno del TwinCAT. También puedes, si tienes un detector conectado al servomotor hacer un referenciado (homing) con SERVO.Start_Homing y a partir de ahí hacer posicionado absoluto, escribiendo una posición destino en SERVO.Set_Value_Position, una velocidad en porcentaje en SERVO.Actual_Velocity y dando la orden de arranque de movimiento con SERVO.Start_Task. Si apareciese algún error se puede acusar con SERVO.Reset_Fault.

Y esto es todo lo necesario para poner a andar el servomotor; sorprendentemente, para ser la primera vez que monto un componente Profibus sin Siemens de por medio, me funcionó bien a la primera. El entorno de Beckhoff me ha parecido bastante intuitivo.

Como siempre, agradeceré cualquier comentario, corrección o sugerencia.