Motorcortex Core  version: 2.7.6
drive_sercos_fsm_data.h
1 /*
2  * Developer : Alexey Zakharov (alexey.zakharov@vectioneer.com)
3  * All rights reserved. Copyright (c) 2018 VECTIONEER.
4  */
5 
6 #ifndef MOTORCORTEX_CORE_DRIVE_SERCOS_FSM_DATA_H
7 #define MOTORCORTEX_CORE_DRIVE_SERCOS_FSM_DATA_H
8 
9 #include <utility>
10 
11 #include "drive_command.h"
12 #include "drive_state.h"
13 #include "drive_mode.h"
14 #include "drive_utils.h"
15 
16 namespace mcx {
17 
18 namespace drive {
19 
20 /* Sercos (over EtherCAT) ControlWord
21  *
22  * Bit Designation/function Comment
23  *
24  * 5-0 Control information for service channel -
25  *
26  * 7-6 Real-time control bits 1 and 2 -
27  *
28  * 11/9/8 Command operation mode 0_00: Primary operation mode
29  * 0_01: Secondary operation mode 1, etc.
30  * 1_11: Secondary operation mode 7
31  *
32  * 10 IPOSYNC, interpolator clock Toggles when new command values are to be transmitted
33  *
34  * 13 Drive Halt 1-0 change: Deceleration of drive while maintaining maximum acceleration (S-0-0138)
35  * (only possible if bits 14 and 15 = 1)
36  * 14 Drive Enable 1-0 change: Torque disable without delay (independent of bit 15 or 13)
37  * 15 Drive On 1-0 change: Best possible deceleration (only possible if bit 14 = 1)
38  * |
39  * |
40  * --> When changing from 1 to 0: the maximum drive off delay time (IDN S-0-0273) is started, drive is
41  * decelerated as best as possible according to the emergency stop deceleration (IDN S-0-0429),
42  * followed by disabling of the torque/force at n min, after the drive off delay time (IDN S-0-0207).
43  * The power stage can remain in an activated state (only possible when bit 14=1). After the maximum
44  * drive off delay time (IDN S-0-0273) is elapsed, the locking of brake is initiated and the torque/force
45  * is disabled.
46  * When changing from 0 to 1: drive follows the command values of the control unit after the drive on
47  * delay time (IDN S-0-0206).
48  */
49 
50 enum SercosCommandBits {
51  SERCOS_CMD_OPMODE_BIT1 = 8,
52  SERCOS_CMD_OPMODE_BIT2 = 9,
53  SERCOS_CMD_OPMODE_BIT3 = 11,
54 
55  SERCOS_CMD_DRIVE_HALT = 13,
56  SERCOS_CMD_DRIVE_ENABLE = 14,
57  SERCOS_CMD_DRIVE_ON = 15
58 };
59 
60 enum SercosCommandOpMode {
61 /*
62  * 11/9/8 Command operation mode 0_00: Primary operation mode
63  * 0_01: Secondary operation mode 1, etc.
64  * 1_11: Secondary operation mode 7
65  */
66 
67  SERCOS_CMD_OPMODE_PRIMARY = 0,
68  SERCOS_CMD_OPMODE_SECONDARY1 = 1,
69  SERCOS_CMD_OPMODE_SECONDARY2 = 2,
70  SERCOS_CMD_OPMODE_SECONDARY3 = 3,
71  SERCOS_CMD_OPMODE_SECONDARY4 = 8,
72  SERCOS_CMD_OPMODE_SECONDARY5 = 9,
73  SERCOS_CMD_OPMODE_SECONDARY6 = 10,
74  SERCOS_CMD_OPMODE_SECONDARY7 = 11
75 };
76 
77 /* Sercos (over EtherCAT) StatusWord
78  *
79  * Bit Designation/function Comment
80  *
81  * 2-0 Control information for service channel -
82  *
83  * 3 Status of command value processing -
84  *
85  * 4 Reserved -
86  *
87  * 5 Command change bit 1: Changed command status
88  * 0: Unchanged command status
89  *
90  * 7-6 Real-time status bits 1 and 2 -
91  *
92  * 10-8 Actual operation mode 000: Primary operation mode active
93  * 001: Secondary operation mode 1 active
94  * 010: Secondary operation mode 2, etc.
95  * 11 Class 3 diagnostics message (cf. S-0-0013) The bit is set if a class 3 diagnostics message is present.
96  *
97  * 12 Class 2 diagnostics warning (cf. S 0-0012) The bit is set if a class 2 diagnostics warning is present.
98  *
99  * 13 Class 1 diagnostics drive error (cf. S-0-0011) The bit is set if a class 1 diagnostics error is present (drivelock-out).
100  *
101  * 15-14 Ready for operation (P-0-0116, bit 15/14) 00: Not ready for power on (e.g., P2)
102  * 01: Ready for power on (bb)
103  * 10: Control section and power section ready for op. (Ab)
104  * 11: In operation, with torque (e.g. AF)
105  */
106 enum SercosStatusBits {
107  SERCOS_STATUS_OPMODE_BIT1 = 8,
108  SERCOS_STATUS_OPMODE_BIT2 = 9,
109  SERCOS_STATUS_OPMODE_BIT3 = 10,
110  SERCOS_STATUS_CLASS3_DIAG_MSG = 11,
111  SERCOS_STATUS_CLASS3_DIAG_WARNING = 12,
112  SERCOS_STATUS_CLASS3_DIAG_ERROR = 13,
113  SERCOS_STATUS_OPERATION_BIT1 = 14,
114  SERCOS_STATUS_OPERATION_BIT2 = 15,
115 };
116 
117 enum SercosStatusOpMode {
118  SERCOS_STATUS_OPMODE_PRIMARY = 0,
119  SERCOS_STATUS_OPMODE_SECONDARY1 = 1,
120  SERCOS_STATUS_OPMODE_SECONDARY2 = 2,
121  SERCOS_STATUS_OPMODE_SECONDARY3 = 3,
122  SERCOS_STATUS_OPMODE_SECONDARY4 = 4,
123  SERCOS_STATUS_OPMODE_SECONDARY5 = 5,
124  SERCOS_STATUS_OPMODE_SECONDARY6 = 6,
125  SERCOS_STATUS_OPMODE_SECONDARY7 = 7,
126 };
127 
128 enum SercosStatusOperation {
129  SERCOS_STATUS_NOT_READY_FOR_POWER_ON = 0,
130  SERCOS_STATUS_READY_FOR_POWER_ON = 1,
131  SERCOS_STATUS_READY_FOR_OP = 2,
132  SERCOS_STATUS_IN_OPERATION = 3
133 };
134 
135 struct SercosFsmData {
136  explicit SercosFsmData(std::string name) : fsm_name{std::move(name)} {};
137 
138  const std::string fsm_name;
139  DriveState drive_state{DriveState::DRIVE_STATE_OFF};
140  bool upload_error_code{};
141  unsigned int sercos_cmd{SERCOS_CMD_OPMODE_PRIMARY};
142  unsigned int sercos_mode{SERCOS_CMD_OPMODE_PRIMARY};
143  unsigned int sercos_state{};
144  struct {
145  double fault_ack{1};
146  double shutdown{1};
147  double switch_on{1};
148  double disable_voltage{1};
149  double quick_stop{1};
150  double disable_operation{1};
151  double enable_operation{1};
152  double upload_code{0.01};
153  } timeout_sec;
154 };
155 
156 inline SercosStatusOperation sercosStatusOperation(unsigned int sercos_state) {
157  /*
158  * 15-14 Ready for operation (P-0-0116, bit 15/14) 00: Not ready for power on (e.g., P2)
159  * 01: Ready for power on (bb)
160  * 10: Control section and power section ready for op. (Ab)
161  * 11: In operation, with torque (e.g. AF)
162  */
163  unsigned int status = ((sercos_state >> SERCOS_STATUS_OPERATION_BIT1) & 1u) +
164  2 * ((sercos_state >> SERCOS_STATUS_OPERATION_BIT2) & 1u);
165  return static_cast<SercosStatusOperation>(status);
166 }
167 
168 inline SercosStatusOpMode sercosStatusActualMode(unsigned int sercos_state) {
169  /*
170  * 10-8 Actual operation mode 000: Primary operation mode active
171  * 001: Secondary operation mode 1 active
172  * 010: Secondary operation mode 2, etc.
173  */
174  unsigned int mode = ((sercos_state >> SERCOS_STATUS_OPMODE_BIT1) & 1u) +
175  2 * ((sercos_state >> SERCOS_STATUS_OPMODE_BIT2) & 1u) +
176  4 * ((sercos_state >> SERCOS_STATUS_OPMODE_BIT3) & 1u);
177 
178  return static_cast<SercosStatusOpMode>(mode);
179 }
180 
181 inline unsigned int sercosSetOpMode(unsigned int sercos_cmd, SercosCommandOpMode op_mode) {
182  /*
183  * 11/9/8 Command operation mode 0_00: Primary operation mode
184  * 0_01: Secondary operation mode 1, etc.
185  * 1_11: Secondary operation mode 7
186  */
187  unsigned int mode = op_mode << SERCOS_CMD_OPMODE_BIT1;
188  sercos_cmd = clearBit(sercos_cmd, SERCOS_CMD_OPMODE_BIT1, SERCOS_CMD_OPMODE_BIT2, SERCOS_CMD_OPMODE_BIT3);
189  sercos_cmd |= mode;
190  return sercos_cmd;
191 }
192 
193 } // namespace drive
194 
195 } // namespace mcx
196 
197 #endif //MOTORCORTEX_CORE_DRIVE_SERCOS_FSM_DATA_H
mcx::drive::SercosFsmData
Definition: drive_sercos_fsm_data.h:135