#!/bin/bash
# 06_cia402_state_demo.sh - CiA402 伺服驱动状态机模拟
# 目标：模拟 CiA402 规定的伺服驱动状态机
#       Not Ready → Switch On Disabled → Ready → Switched On → Operation Enabled
# 使用 CANopenNode 的 0x6040 Controlword / 0x6041 Statusword

SOCKET1="/tmp/CO_command_socket_1"

cocc() {
    local socket=$1
    local node=$2
    local cmd=$3
    echo "  → [$node] $cmd"
    cocomm -s "$socket" "$node $cmd"
}

echo "================================================"
echo " CiA402 伺服驱动状态机模拟"
echo "================================================"
echo ""

echo "--- Step 1: 确保节点在 Operational ---"
cocc "$SOCKET1" 1 "start"
sleep 1
echo ""

echo "--- Step 2: 读取 CiA402 对象字典条目 ---"
echo "    0x6040 = Controlword（主站→从站的控制字）"
echo "    0x6041 = Statusword（从站→主站的状态字）"
echo "    0x6060 = Modes of Operation（运行模式）"
echo "    0x6061 = Modes of Operation Display（当前模式显示）"
echo ""
echo "    当前 Controlword 值："
cocc "$SOCKET1" 1 "read 0x6040 0 u16"
echo "    当前 Statusword 值："
cocc "$SOCKET1" 1 "read 0x6041 0 u16"
echo "    当前 Modes of Operation："
cocc "$SOCKET1" 1 "read 0x6060 0 u8"
echo ""

echo "--- Step 3: CiA402 状态机转换序列 ---"
echo ""
echo "    初始状态应为 Switch On Disabled"
echo ""

echo "    设置 Controlword = 0x0006: Shutdown → Ready to Switch On"
echo "    (Bit 0=0, Bit 1=1, Bit 2=1)"
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x0006"
sleep 1
echo "    读取 Statusword 确认："
cocc "$SOCKET1" 1 "read 0x6041 0 u16"
echo ""

echo "    设置 Controlword = 0x0007: Switch On → Switched On"
echo "    (Bit 0=1, Bit 1=1, Bit 2=1)"
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x0007"
sleep 1
echo "    读取 Statusword 确认："
cocc "$SOCKET1" 1 "read 0x6041 0 u16"
echo ""

echo "    设置 Controlword = 0x000F: Enable Operation"
echo "    (Bit 0=1, Bit 1=1, Bit 2=1, Bit 3=1)"
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x000F"
sleep 1
echo "    读取 Statusword 确认："
cocc "$SOCKET1" 1 "read 0x6041 0 u16"
echo ""

echo "    模拟 Fault：设置 Controlword = 0x0080 (Fault reset)"
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x0080"
sleep 1
echo "    读取 Statusword 确认 fault："
cocc "$SOCKET1" 1 "read 0x6041 0 u16"
echo ""

echo "    清除 Fault：Controlword = 0x0000 再置 0x0080"
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x0000"
sleep 1
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x0080"
sleep 1
echo "    读取 Statusword："
cocc "$SOCKET1" 1 "read 0x6041 0 u16"
echo ""

echo "    重新 Enable Operation："
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x0006"  # Shutdown
sleep 1
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x0007"  # Switch On
sleep 1
cocc "$SOCKET1" 1 "write 0x6040 0 u16 0x000F"  # Enable Operation
sleep 1
echo "    最终 Statusword："
cocc "$SOCKET1" 1 "read 0x6041 0 u16"
echo ""

echo "--- CiA402 状态字位定义 ---"
echo "  Bit 0: Ready to switch on"
echo "  Bit 1: Switched on"
echo "  Bit 2: Operation enabled"
echo "  Bit 3: Fault"
echo "  Bit 4: Voltage enabled"
echo "  Bit 5: Quick stop"
echo "  Bit 6: Switch on disabled"
echo "  Bit 7: Warning"
echo "  Bit 8-9: Manufacturer specific"
echo "  Bit 10: Remote"
echo "  Bit 11: Target reached"
echo "  Bit 12: Internal limit active"
echo "  Bit 13: Unknown"
echo "  Bit 14-15: Specific"
echo ""

echo "================================================"
echo " CiA402 状态机实验完成"
echo " 关键认知："
echo " - Controlword 0x6040 是主站控制入口"
echo " - Statusword 0x6041 反映从站实际状态"
echo " - 标准序列：Shutdown → Switch On → Enable Op"
echo " - Fault 时需 Fault Reset 才能恢复"
echo " - 在 STM32 伺服驱动中，这是电机使能的控制入口"
echo "================================================"