CiA402 State Machine 説明
2022/5/1 加古川篤
static EC_T_WORD fnChgMotionGoState(EC_T_WORD woStWd, EC_T_WORD *pCrWd)の関数を使って，状態遷移を実施する．
woStWd は Slave の現在の状態（Statusword）を，*pCrWd は状態遷移指令値（Controlword）を示している．0b は 2 進数を，0x は 16 進数を表しており，数字の前に何も記号がないもの
を 10 進数とする．
例：
while(1) {if (woStatusWord != 0x27) {/* 現在のｻｰﾎﾞﾄﾞﾗｲﾊﾞ状態取得 */pwVal = (EC_T_WORD*)&pbyPDIn[MySlave.dwPdOffsIn+2];
woStatusWord = *pwVal;/* 現在のｻｰﾎﾞﾄﾞﾗｲﾊﾞ状態から、次のｻｰﾎﾞﾄﾞﾗｲﾊﾞ状態へ遷移させる関数ｺｰﾙ */woStatusWord = fnChgMotionGoState(woStatusWord, &woControlWord);
if (woStatusWord == 0x27) break;/* 次のｻｰﾎﾞﾄﾞﾗｲﾊﾞ遷移値を書き込み */pwVal = (EC_T_WORD*)&pbyPDOut[MySlave.dwPdOffsOut];*pwVal = woControlWord;}};

woStatusWord（int）の値が 0x27（=39 = 0b00100111）でない限り，状態遷移を繰り返すル

ープ．0x27 のとき，Slave は「Operation enable（サーボオン）」の状態にあり，モータ駆動可能となる．つまり，Slave の woStatusWord の値を 0x27 まで遷移させないとモータを制御できない．

State Machine の状態遷移図を以下に示す．
https://doc.synapticon.com/software/44/canopen_over_ecat/index.html#canopen-overecat

The function that are available in each state are given in the table below:
Function

FSA States
Not
readytoswitchon

Switchondisabled

Readytoswitchon

Switchedon

Operation
enabledQuick
StopactiveFault
reaction
activeFault

Brake applied,
if present

Yes

Yes

Yes

Yes

No

No

No

Yes

Drive function
enabled

No

No

No

No

Yes

Yes

Yes

No

Configuration
allowed

Yes

Yes

Yes

Yes

No

No

No

Yes

0x6041 Statusword
The status of the PDS FSA. Only bits 0-6 are supported.
1514ms1312oms

11109876543210ilatrrm

msw

sod

qsvefoeso

rtso

MSB

LSB

LEGEND: ms = manufacturer-specific; oms = operation mode specific; ila = internal limit
active; tr = target reached; rm = remote; w = warning; sod = switch on disabled; qs = quickstop; ve = voltage enabled; f = fault; oe = operation enabled; so = switched on; rtso = readyto switch on
Statusword

PDS FSA statexxxx xxxx x0xx 0000

Not ready to switch onxxxx xxxx x1xx 0000

bSwitch on disabledxxxx xxxx x01x 0001

Ready to switch onxxxx xxxx x01x 0011

Switched onxxxx xxxx x01x 0111

Operation enabledxxxx xxxx x00x 0111b

Quick stop active

xxxx xxxx x0xx 1111

bFault reaction active

xxxx xxxx x0xx 1000

Faultbbbbb

If bit 4 (voltage enabled) of the statusword is 1, this indicates that high voltage isapplied to the PDS.
If bit 5 (quick stop) of the statusword is 0, this indicates that the PDS is reacting ona quick stop request.
Operation mode specific bits of Statusword ("oms")
Bits

Cyclicsynchronous
positioncontrol

Cyclicsynchronous
velocitycontrol

bit 13(0x2000)

Followingerror bit

not used

bit 12(0x1000)

Targetposition
ignoredTargetvelocity
ignoredCyclicsynchronous
torquecontrol

TargettorqueignoredProfileposition
mode

Profilevelocitymode

Homingmode

Followingerror bitnotusedHomingerror
bit

Set-point
acknowledge
bit

Speed
bit

Homingattainedbit

0x6040 Controlword
The received command controlling the PDS FSA.
15

1110987r

oms

hfr

ms

6

432

10oms

eo

qs

ev

soMSB

LSB

LEGEND: ms = manufacturer-specific; r = reserved; oms = operation mode specific; h =halt; fr = fault reset; eo = enable operation; qs = quick stop; ev = enable voltage; so = switchon
Bits of the controlword

CommandTransitions

Bit 7

Bit 3

Bit 2

Bit 1

Bit 0

Shutdown

0X

1102, 6, 8

Switch on

0011

1

3

Switch on + enable operation

01

1

1

1

3 + 4 (NOTE)Disable voltage

0X

X

0X

7, 9, 10, 12

Quick stop

0X

01

X

7, 10, 11

Disable operation

00

1

1

1

5

Enable operation

011

1

14, 16

Fault reset

1XXX

X

15NOTE Automatic transition to Enable operation state after executing SWITCHED ON
state functionality.
Bits 9, 6, 5, and 4 of the controlword are operation mode specific. The halt function (bit 8)
behaviour is operation mode specific. If the bit is 1, the commanded motion shall beinterrupted, the PDS shall behave as defined in the halt option code. After releasing the halt
function, the commanded motion shall be continued if possible.
The bit 10 is reserved for further use; it shall be set to 0. The bits 11, 12, 13, 14, and 15 aremanufacturer-specific.
NameIndex:SubTypeBit
SizeControlword0x6040:0UINT

16Min
Data

Max
Data

Default
Data

0Unit

AccessPDO
Mapping

readwrite

Receive
PDO(Outputs)

State Machine の状態遷移分岐を以下に示す．
例：/* Description : CiA402 ｻｰﾎﾞﾄﾞﾗｲﾊﾞ状態遷移処理 */static EC_T_WORD fnChgMotionGoState(EC_T_WORD woStWd, EC_T_WORD *pCrWd){switch( (0x6F & woStWd) ) {case 0: *pCrWd = (0xFF70 & *pCrWd) | 0; break;/* 未初期化状態 -> 初期化要求 */case 0x40: /* (0x60 と同じ) */case 0x60: *pCrWd = (0xFF70 & *pCrWd) | 0x06; break;/* 初期化完了状態 -> 主回路電源OFF 要求 */case 0x21: *pCrWd = (0xFF70 & *pCrWd) | 0x07; break;/* 主回路電源OFF 状態 -> ｻｰﾎﾞﾚﾃﾞｨ要求 */case 0x23: *pCrWd = (0xFF70 & *pCrWd) | 0x0F; break;/* ｻｰﾎﾞﾚﾃﾞｨ状態 -> ｻｰﾎﾞON 要求 */case 0x27: break; /* ｻｰﾎﾞON 状態 (運転開始OK 状態) */case 0x2F: break; /* 異常処理動作中状態 */case 0x28: *pCrWd = (0xFF70 & *pCrWd) | 0x80; break;/* 異常状態 -> 初期化要求 */}return (0x6F & woStWd);}

0x6F & woStWdの値を調べて，現在の状態に応じて*pCrWd の値を変更する．0x6F = 111 = 0b0110 1111．
①woStWd = 0 pCrWd =0 のとき，0x6F & woStWd = 0 となる．この状態は Not ready to switch onであり，このとき，*pCrWd = (0xFF70 & *pCrWd) | 0 を返す．
0xFF70 =0b 1111 1111 0111 0000

*pCrWd =0b 0000 0000 0000 0000

0=0b 0000 0000 0000 0000

→ (0xFF70 & *pCrWd) | 0 = 0b 0000 0000 0000 0000

その結果，*pCrWd =0 が返される．

②Statusword = Not ready to switch on のとき，自動的に Statusword = Switch on disabledに移る．このとき，woStWd = 0b 0100 0000 (= 0x40 = 64) であり，0x6F & woStWdの値は，以下のように 0x40 となる．
0x6F =

0b 0110 1111

woStWd =0b 0100 0000 (= 0x40)

0x6F & woStWd =0b 0100 0000 (= 0x40)

このとき，*pCrWd = (0xFF70 & *pCrWd) | 0x06 を返す．
0xFF70 =0b 1111 1111 0111 0000

*pCrWd =0b 0000 0000 0000 0000

0x06 =

0b 0000 0000 0000 0110

→ (0xFF70 & *pCrWd) | 0x06 =0b 0000 0000 0000 0110

その結果，*pCrWd = 0x06 (= 0b 0110)が返される．これは Controlword = Shutdown の指令に相当し，Ready to switch on へ移行する．
③Statusword = Ready to switch on のとき，woStWd = 0b 0010 0001 (= 0x21 = 33)であり，0x6F & woStWdの値は，以下のように 0x21 となる．

0x6F =

0b 0110 1111

woStWd =0b 0010 0001 (= 0x21)

0x6F & woStWd =0b 0010 0001 (= 0x21)

このとき，*pCrWd = (0xFF70 & *pCrWd) | 0x07 を返す．
0xFF70 =0b 1111 1111 0111 0000

*pCrWd =0b 0000 0000 0000 0110

0x07 =

0b 0000 0000 0000 0111

→ (0xFF70 & *pCrWd) | 0x07 =0b 0000 0000 0000 0111

その結果，*pCrWd = 0x07 (= 0b 0111)が返される．これは Controlword = Switch on の指令に相当し，Switch on へ移行する．
④Statusword = Switch on のとき，woStWd = 0b 0010 0011 (= 0x23= 35)であり，0x6F & woStWdの値は，以下のように 0x23 となる．
0x6F =0b 0110 1111

woStWd =0b 0010 0011 (= 0x23)

0x6F & woStWd =0b 0010 0011 (= 0x23)このとき，*pCrWd = (0xFF70 & *pCrWd) | 0x0F を返す．
0xFF70 =0b 1111 1111 0111 0000

*pCrWd =0b 0000 0000 0000 0111

0x0F =0b 0000 0000 0000 1111

→ (0xFF70 & *pCrWd) | 0x0F =0b 0000 0000 0000 1111

その結果，*pCrWd = 0x0F (= 0b 1111)が返される．これは Controlword = Switch on + enable

operation の指令に相当し，Operation enabled へ移行する．

異常状態のとき⑤Statusword = Fault reaction active のとき，woStWd = 0b 0010 1111 (= 0x2F = 47)であり，0x6F & woStWdの値は，以下のように 0x23 となる．

0x6F =

0b 0110 1111

woStWd =0b 0010 1111 (= 0x2F)

0x6F & woStWd =0b 0010 1111 (= 0x2F)

このとき，Fault reaction active となり，ドライバは停止する．
⑥Statusword = Switch on のとき，woStWd = 0b 0010 1000 (= 0x28= 40)であり，0x6F & woStWdの値は，以下のように 0x23 となる．
0x6F =

0b 0110 1111

woStWd =0b 0010 1000 (= 0x28)

0x6F & woStWd =0b 0010 1000 (= 0x28)

このとき，*pCrWd = (0xFF70 & *pCrWd) | 0x80 を返す．
0xFF70 =0b 1111 1111 0111 0000

*pCrWd =0b 0000 0000 X000 0000

0x80 =

0b 0000 0000 1000 0000

→ (0xFF70 & *pCrWd) | 0x80 =0b 0000 0000 X000 0000

その結果，*pCrWd = 0x80 か 0x00 が返され，これは初期化要求に相当する．