3.1, CANOpen communication
3.1.1 Object Dictionary
Since the underlying communication of EtherCAT over CANOpen has already been implemented, we only need to know that CANOpen communicates as a dictionary of objects. This simply means that each hexadecimal number represents a variable. If you you are familiar with C#, then the communication protocol can be understood as aDictionary<ushort, object>. If you are familiar with MODBUS, then you can understand that the hexadecimal number is the register address, and the variable is the content of the register, for example, the content of this register, 16#6040, is the control word, and the control word is already in effect when it is filled into this register, 16#6040.
3.1.2 Communication establishment
Power on the drive, adjust the parameters of the drive according to the manual, connect the drive and PLC with a network cable, find the drive manufacturer to get the xml device description file corresponding to the drive, create a new TwinCAT3 project, connect it to the PLC, switch the PLC to the Config mode, scan the device, and then, not surprisingly, you will get the drive.
Next, edit the PDO page of the driver to add all the object dictionaries we might use. The addition process is skipped (see the TwinCAT3 tutorial), and the following is the result of the addition.
Statusword, Modes of operation, Controlword, Target position and other objects are often used, and will be discussed below.
3.2, CiA402 servo state machine
The manual says that the servo drive must be guided according to the process specified in the CiA402 protocol for the servo drive to operate in the specified state. The objects associated with the servo state machine are the status word Statusword and the control word Controlword. details of the state machine transfer are given below:
| state machine transfer | Control word (16#6040) | Status word (16#6041) |
|---|---|---|
| 0 Power-up → Initialization | Natural transition without control commands | 0x0000 |
| 1 Initialization → No servo malfunction | Natural transition without control commands | 0x0250 (0010-0101-0000 Servo no fault) |
| 2 No servo malfunction → Servo ready | 0x06 (0000-0000-0110 Servo Ready) | 0x0231 (0010-0011-0001 servo ready) |
| 3 Servo ready → Wait to turn on servo enable | 0x07 (0000-0000-0111 wait for enable) | 0x0233 (0010-0011-0011 wait for servo enable) |
| 4 Waiting to turn on servo enable → servo operation | 0x0F (0000-0000-1111 Servo Run) | 0x0237 (0010-0011-0111 servo run) |
| 5 Servo operation → wait for servo enable to be turned on | 0x07 | 0x0233 |
| 6 Waiting to turn on servo enable → servo ready | 0x06 | 0x0231 |
| 7 Servo ready → No servo failure | 0x00 (0000-0000-0000 Servo no fault) | 0x0250 (0010-0101-0000 Servo no fault) |
| 8 Servo Run → Servo Ready | 0x06 | 0x0231 |
| 9 Servo operation → no servo malfunction | 0x00 | 0x0250 |
| 10 Waiting to turn on servo enable → no servo failure | 0x00 | 0x0250 |
| 11 Servo operation → quick stop | 0x02 (0000-0000-0010 Quick Stop) | 0x0217 (0010-0001-0111 Quick Stop) |
| 12 Quick stop → no servo failure | Natural transition without control commands | 0x0250 |
| 13 Any → Failure shutdown | Automatic switchover in the event of a fault | 0x021F (0010-0001-1111 Faulty shutdown) |
| 14 Failure stop → Failure | Natural transition without control commands | 0x0218 (0010-0001-1000 fault) |
| 15 Fault → servo no fault | 0x80 (0000-1000-0000 Fault Reset) | 0x0250（0010-0101-0000） |
| 16 Quick stop → servo run | 0x0F | 0x0237 |
Details of the control word Controlword and status word Statusword are as follows:
| Control word bit | 0 | 1 | 2 | 3 | 4-6 | 7 | 8 | 9-15 |
|---|---|---|---|---|---|---|---|---|
| name (of a thing) | Servo on | main circuit | Quick Stop | servo operation | Related to mode of operation | fault reset | pause (media player) | NA |
| descriptive | 0-No 1-Yes | 0-No 1-Yes | 0-yes 1-no | 0-No 1-Yes | Rising edge valid | 0-No 1-Yes |
| operating mode | Contour Position Mode | Zero Return Mode | Contour Speed Mode | Contour Torque Mode |
|---|---|---|---|---|
| Control word bit4 | Enable new position command (rising edge triggered) | Start zero return (rising edge trigger, must hold) | NA | NA |
| Control word bit5 | Position command update mode (0 - not immediately updated 1 - immediately updated) | NA | NA | NA |
| Control word bit6 | Position instruction type (0-absolute position instruction 1-relative position instruction) | NA | NA | NA |
| The bit of the status word | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12-15 |
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
| name (of a thing) | Servo ready. | The servo can run | servo operation | malfunctions | main circuit | Quick Stop | Servo inoperable | warnings | Manufacturer Customization | remote control | target arrival | Internal restrictions | Mode of operation related |
| operating mode | Contour Position Mode | Zero Return Mode | Contour Speed Mode | Contour Torque Mode |
|---|---|---|---|---|
| Status word bit12 | Capable of receiving position commands (0-capable 1-capable) | Home return to zero complete (0-No 1-Yes) | Zero speed signal (0-No 1-Yes) | NA |
| Status word bit13 | Position deviation status (0 - within threshold 1 - beyond threshold) | Home return error (0 - no 1 - yes) | NA | NA |
| Status word bit14 | NA | NA | NA | NA |
| Status word bit15 | NA | NA | NA | NA |
The current state machine is determined by the status word Statusword (if it enters theservo operation(The previous appearance of a status word that is not in the table is likely to be a drive power-up abnormality or an error report), and the transfer of the state machine is operated entirely by the control word Controlword. Generally speaking, after the drive is powered up, we have to give the control word based on the current state, with the ultimate goal of transferring the drive to theservo operationThis state machine. For simplicity, no matter what state we are currently in, we send a message to theservo operationGo in this direction.
To achieve the purpose, write the following code:
TYPE E_DriveStatus :
(
Init := 0, //initialization //not ready to switch on
NoFault := 1, //No servo faults //switch on disabled
Ready := 2, //Servo ready. //ready to switch on
WaitOn := 3, //Waiting to turn on servo enable //switched on
Operation := 4, //servo operation //operation enabled
QuickStop := 5, //Quick Stop //quick stop active
FaultStop := 6, //malfunction stoppage //fault reaction active
Fault := 7 //malfunctions //fault
);
END_TYPE
TYPE AXIS_REF .
STRUCT
//0x6040 Effective only in OP state Each bit of the control word is meaningless on its own and must be combined with other bits to form a control instruction.
ControlWord AT%Q*: UINT.
//0x6041 Update only in OP or Safe-OP Each bit of the status word is meaningless to read alone, it must be combined with other bits to form the current status.
StatusWord AT%I*: UINT; //0x6041 Update only under OP or Safe-OP.
END_STRUCT
END_TYPE
FUNCTION_BLOCK MC_Run
VAR_IN_OUT
Axis: AXIS_REF.
AXIS_REF; END_VAR
VAR
//----------
StatusWordBit9: UINT; //Used to determine the state machine, the state machine is only related to bit0-bit9
//----------
DriveStatus: E_DriveStatus; //Drive Status
END_VAR
//State machine transfer ----------
StatusWordBit9:= AND 2#1111111111; //Used to determine the state machine, the state machine is only related to bit0-bit9
IF StatusWordBit9 = 16#0 THEN //0
DriveStatus:= E_DriveStatus.
END_IF
IF StatusWordBit9 = 16#0250 THEN //1 //10 //7 //9 //12 //15
DriveStatus:= E_DriveStatus.NoFault;
END_IF
IF StatusWordBit9 = 16#0231 THEN //2 //6 //8
DriveStatus:= E_DriveStatus.
END_IF
IF StatusWordBit9 = 16#0233 THEN //3 //5
DriveStatus:= E_DriveStatus.WaitOn;
END_IF
IF StatusWordBit9 = 16#0237 THEN //4 //16
DriveStatus:= E_DriveStatus.
Operation; END_IF
IF StatusWordBit9 = 16#0217 THEN //11
DriveStatus:= E_DriveStatus.QuickStop;
END_IF
IF StatusWordBit9 = 16#021F THEN //13
DriveStatus:= E_DriveStatus.FaultStop;
END_IF
IF StatusWordBit9 = 16#0218 THEN //14
DriveStatus:= E_DriveStatus.Fault;
END_IF
//User command to run ----------
CASE DriveStatus OF
E_DriveStatus.NoFault.
:= 16#06; //go to E_DriveStatus.Ready, end at E_DriveStatus.Operation
E_DriveStatus.Ready.
:= 16#07; //Go to E_DriveStatus.WaitOn and end at E_DriveStatus.Operation
E_DriveStatus.WaitOn: := 16#0F; // Go to E_DriveStatus.
:= 16#0F; //go to E_DriveStatus.Operation
E_DriveStatus.Operation.
//drive's movement, press it first
E_DriveStatus.QuickStop.
:= 16#0F; //will go to E_DriveStatus.Operation when shutdown is complete (can't keep ControlWord at 0x02, will get stuck at E_DriveStatus.QuickStop)
E_DriveStatus.Fault.
:= 16#80; // switch to E_DriveStatus.NoFault
END_CASE
The device is powered up, the program runs, and according to the program's logic, as long as the drive is powered up properly and does not report errors, it will enter theservo operationState Machine.
3.3, Servo operation
existservo operationstate, you can control the drive to start moving.
3.3.1 Modes of operation
Most drives support 8 modes of operation:
- Cyclic Synchronous Position ModeCyclic Synchronous Position Mode, i.e. MC_MoveAbsolute and MC_MoveRelative
- Cyclic Synchronous Velocity ModeCyclic Synchronous Velocity Mode, i.e. MC_MoveVelocity
- Cyclic Synchronous Torque ModeCyclic Synchronous Torque Mode, i.e. MC_TorqueControl
- Profile Position Mode, similar to MC_MoveAbsolute and MC_MoveRelative
- Profile Velocity Mode, similar to MC_MoveVelocity
- Profile Torque ModeProfile Torque Mode, similar to MC_TorqueControl
- Homing Mode, similar to MC_Home
- Interpolation Position Mode, similar to MC_ExtSetPointGenFeed
The 8 models can be divided into 3 categories:
- Cycle Synchronization Mode: There are 3 modes, in this mode the PLC does the path planning internally, in each communication cycle with the drive, the PLC calculates the target position (speed/torque) and sends it to the drive. This part of the functionality is provided by the PLC supplier, as for Pefo, this part of the functionality is encapsulated in the TC1250 or TF5000 mentioned at the beginning.
- Contour mode: There are also three modes, the effect of using up with the cycle synchronization mode is similar, the difference is that in this mode the PLC only in the instruction given to the drive target position, target speed and acceleration and deceleration speed and other settings, and then by the drive itself to do the path planning, CANOpen communication control, the main is to use the contour mode.
- Back to zero and interpolation: back to zero is to find the origin, you can use or write your own. Interpolation is to manually do path planning, basically do not need.
The objects related to the operation modes are Modes of operation: (the SV660C manual misspells a lot of things, for example, the following table is the value of the operation mode, but the SV660C manual says it's a bit)
| The value of the operation mode | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 |
|---|---|---|---|---|---|---|---|---|
| descriptive | NA | Contour Position Mode | NA | Contour Speed Mode | Contour Torque Mode | NA | Zero Return Mode | interpolation mode |
3.3.2 Contour position mode
The objects related to the profile position mode are Modes of operation, Controlword, Target position, Profile velocity, Profile acceleration, Profile deceleration. The conversion of target position and profile velocity will be different for different drives, and the following is an example to illustrate the control flow with Delta ASDA-A2.
The control flow is as follows:
- Modes of operation=16#1
- Target position in pulses.
- Given profile velocity in pulses/sec.
- Given profile acceleration and profile deceleration, the number of milliseconds it takes to accelerate from 0 rpm to 3000 rpm.
- Rising edge of bit4 of control word Controlword enables the new position instruction. bit5 is set to 1 to use immediate update mode. bit6 is set to 0 to use absolute position mode.
- Determine that the status word StatusWord has received the instruction, and then reset bit4 of the control word Controlword to make it easier to give the rising edge next time
- Determine that the status word StatusWord has reached the target position and end the process
One of the conversions from the number of pulses to the actual distance also involves the ScalingFactorDenominator of the drive (i.e., how many pulses per revolution of the motor) and the reduction ratio of the gearbox.
In summary, write the following code:
TYPE AXIS_REF .
STRUCT
//0x6060
ModeOperation AT%Q*: SINT.
//0x6040 Effective only in OP state Each bit of the control word is meaningless on its own, and must be combined with other bits to form a control instruction.
ControlWord AT%Q*: UINT.
AT%Q*: UINT; //0x607A
TargetPosition AT%Q*: DINT.
//0x6081
ProfileVelocity AT%Q*: DINT.
//0x6083
ProfileAcc AT%Q*: DINT.
//0x6084
ProfileDec AT%Q*: DINT.
//0x6041 Update only under OP or Safe-OP Each bit of the status word is meaningless when read alone, it must be combined with other bits to form the current status.
StatusWord AT%I*: UINT.
END_STRUCT
END_TYPE
TYPE AXIS_INFO .
STRUCT
TargetPosition: REAL; //Target position mm
TargetVelocity: REAL; //Target velocity mm/s
ProfileVelocity: REAL; //Profile velocity mm/s
ProfileAcc: REAL; //Profile Acceleration mm/s
ProfileDec: REAL; //Profile Deceleration mm/s
END_STRUCT
END_TYPE
TYPE AXIS_PARA .
STRUCT
ScalingFactorNumerator: REAL; //numerator, fill in how many millimeters the motor has traveled in one revolution.
ScalingFactorDenominator: REAL; // Denominator, fill in the number of pulses per motor revolution, for example, ASDA driver is 1280000, SV660C-22bit driver is 4194304.
END_STRUCT
END_TYPE
FUNCTION_BLOCK MC_Run
VAR_IN_OUT
Axis: AXIS_REF.
Info: AXIS_INFO.
Para: AXIS_PARA.
END_VAR
VAR
//----------
DriveStatus: E_DriveStatus; //Drive Status
//----------
DriveMoveCommand: INT; //0:None 1:Contour Position Control Move 3:Contour Speed Control Move 6:Home Mode 20:Quick Stop 21:Fault Reset
AbsOrRel: INT; //Contour position control movement mode 0:Absolute movement 1:Relative movement
DriveCommandState: INT; //Command State Machine
ScalingFactor: REAL; //denominator/numerator = number of pulses/mm
END_VAR
//Out-----
ScalingFactor:= / MAX(, 0.001); //Command State Machine
//User command to run ----------
CASE DriveStatus OF
E_DriveStatus.NoFault.
:= 16#06; //go to E_DriveStatus.Ready, end at E_DriveStatus.Operation
E_DriveStatus.Ready.
:= 16#07; //Go to E_DriveStatus.WaitOn, ending at E_DriveStatus.Operation
E_DriveStatus.WaitOn: := 16#0F; // Go to E_DriveStatus.
:= 16#0F; //go to E_DriveStatus.Operation
E_DriveStatus.Operation.
IF DriveMoveCommand = 1 THEN //contour position control move
CASE DriveCommandState OF
0.
:= 1; //contour position control mode //0x6060
IF AbsOrRel = 0 THEN //Absolute
:= REAL_TO_DINT( * ScalingFactor); //0x607A //Unit:Pulse Number
ELSIF AbsOrRel = 1 THEN //Relative
:= + REAL_TO_DINT( * ScalingFactor); //Full_Rel = 1 THEN //Unit:Number of Pulses
END_IF
:= REAL_TO_DINT( * ScalingFactor); //0x6081 //Unit:pulses/second
:= LIMIT(1, REAL_TO_DINT(50000.0 * / MAX(, 0.001)), 65500); //0x6083 //milliseconds from 0rpm to 3000rpm //range 1-65500
:= LIMIT(1, REAL_TO_DINT(50000.0 * / MAX(, 0.001)), 65500); //0x6084
.4:= 0;
DriveCommandState:= 10;
10.
IF = 1 THEN
//control word 0x0F->0x3F
.4:= 1; //Rising edge: enable shift command
.5:= 1; //0:not update immediately 1:update immediately
.6:= 0; //Position instruction type 0: means 607A is absolute position 1: means 607A is relative position (set 1 can't work)
DriveCommandState:= 20; //DriveCommandState:= 20; //Position Command Type
END_IF
20.
IF .12 THEN //No new displacement command can be received, indicating that the servo has received a displacement command.
.4:= 0; //Reset displacement instruction 0x3F->0x2F
DriveCommandState:= 30;
END_IF
30.
IF .10 THEN //target reached
DriveMoveCommand:= 0; //End of story
DriveCommandState:= 0;
END_IF
END_CASE
END_CASE
E_DriveStatus.QuickStop.
:= 16#0F; //will go to E_DriveStatus.Operation when shutdown is complete (can't keep ControlWord at 0x02, it will get stuck in E_DriveStatus.QuickStop)
E_DriveStatus.Fault.
:= 16#80; // switch to E_DriveStatus.NoFault
END_CASE
According to the process to fill in the appropriate parameters, not surprisingly, the motor can be turned up.
3.3.3 Contour speed mode
The control flow is similar to the contour position mode as follows:
- The control word Controlword is held at 0x0F.
- Modes of operation=16#3
- Given target velocity in 0.1rpm
- Given profile acceleration and profile deceleration, the number of milliseconds it takes to accelerate from 0 rpm to 3000 rpm.
- Judge the status word StatusWord to reach the target speed and end the process
//User command to run ----------
CASE DriveStatus OF
E_DriveStatus.
IF DriveMoveCommand = 3 THEN //profile speed control mode
CASE DriveCommandState OF
0.
:= 16#0F; //control word remains at 0x0F
DriveCommandState:= 10; //Control word stays at 0x0F; //Control word stays at 0x0F.
10.
:= 3; //Profile speed control mode //0x6060
//Unit:0.1rpm //X(mm/s) = 60*X(mm/min) = 60*X/molecule(rpm) = 600*X/molecule(0.1rpm)
:= REAL_TO_DINT(600 * / ); //0x60FF
:= LIMIT(1, REAL_TO_DINT(50000.0 * / MAX(, 0.001)), 65500); //0x6083 // milliseconds from 0rpm to 3000rpm ms/3000rpm / / range 1-65500
:= LIMIT(1, REAL_TO_DINT(50000.0 * / MAX(, 0.001)), 65500); //0x6084 //milliseconds from 0rpm to 3000rpm
DriveCommandState:= 20;
20.
IF = 3 THEN
DriveCommandState:= 30;
END_IF
30: IF .10 THEN //Target reached.
IF .10 THEN //target reached
DriveMoveCommand:= 0; //Ended
DriveCommandState:= 0;
END_IF
END_CASE
END_CASE
END_CASE
Here only posted the key code, the same to fill in the appropriate parameters, not surprisingly, the motor can be turned up.
3.3.4 Other
Quick stops and fault resets are relatively simple and can be achieved by simply writing the control word Controlword and the mode of operation Modes of operation.
As for MC_SetPosition, I'm not sure how Peifu does it for the time being, but the same effect can be achieved by using the return-to-zero mode of the Modes of operation and combining it with the return-to-zero method Homing Method.
4. Use it.
I forgot to take a screenshot of the back, so I'll post a picture of the motor just being spun up in contour speed mode in the early stages.
