Arm Controller (arm_controller.py)

flow diagram

graph TD
    A[Instantiate ArmController] --> B[__init__ Method];
    B --> C[Log Device Name];
    C --> D[Initialize Callbacks and State Variables];
    D --> E[Open Serial Port];
    E --> F[Create Thread Locks];
    F --> G[Create Quit Event];
    G --> H[Create Receive Thread];
    H --> I[Start Receive Thread];
    I --> J[Receive Thread Runs];
    J --> K[Continuously Read Serial Data];
    K --> L{Is Header Byte?};
    L -- No --> M{Is Newline/Carriage Return?};
    M -- Yes --> N{Databuf Looks Like Debug?};
    N -- Yes --> O[Parse Debug Data];
    O --> P{Debug Callback Registered?};
    P -- Yes --> Q[Call Debug Callback];
    Q --> R[Reset Databuf];
    P -- No --> R;
    N -- No --> R;
    M -- No --> S[Append Data to Databuf];
    S --> T[Echo Data to Stdout];
    T --> K;
    R --> K;
    L -- Yes --> U[Read Remaining Packet Bytes];
    U --> V{Packet Length Correct?};
    V -- No --> K;
    V -- Yes --> W{Checksum Valid?};
    W -- No --> X[Log Checksum Error];
    X --> K;
    W -- Yes --> Y{Packet Type?};
    Y --> |COMM_TYPE_PONG| Z{Pong Callback Registered?};
    Z -- Yes --> AA[Call Pong Callback];
    AA --> K;
    Z -- No --> K;
    Y --> |COMM_TYPE_FEEDBACK| AB[Unpack Raw Feedback];
    AB --> AC[Convert Raw to SI Units];
    AC --> AD[Record Current Time];
    AD --> AE[Acquire State Lock];
    AE --> AF{First Update?};
    AF -- Yes --> AG[Initialize Last State and Time];
    AG --> AH[Release State Lock];
    AH --> AI{State Callback Registered?};
    AI -- Yes --> AJ[Call State Callback with State];
    AJ --> K;
    AI -- No --> K;
    AF -- No --> AK[Calculate Velocity and Effort];
    AK --> AL[Update Last State and Time];
    AL --> AH;
    Y --> |COMM_TYPE_CONFIG_FEEDBACK| AM{Config Callback Registered?};
    AM -- Yes --> AN[Call Config Callback with Data];
    AN --> K;
    AM -- No --> K;
    I --> BA[set_torque 1];
    BA --> BB[set_pid];
    BB --> BC[Wait for First Feedback];
    BC --> BD[Controller Ready];

    BD --> BE[Call set_pos method];
    BE --> BF{Position within Limits?};
    BF -- No --> BG[Log Limit Error];
    BG --> BH{Error Callback Registered?};
    BH -- Yes --> BI[Call Error Callback];
    BI --> BD;
    BH -- No --> BD;
    BF -- Yes --> BJ[Convert SI to Raw Position];
    BJ --> BK[Construct Control Packet];
    BK --> BL[Call write method];

    BD --> BM[Call set_torque method];
    BM --> BN[Construct Torque Packet];
    BN --> BL;

    BL --> BO[Acquire Write Lock];
    BO --> BP[Write Data to Serial];
    BP --> BQ[Release Write Lock];
    BQ --> BD;

    BD --> BR[Call stop method];
    BR --> BS[Set Quit Event];
    BS --> BT[Receive Thread Exits Loop];
    BT --> BU[Disable Torque];
    BU --> BV[Flush Serial Output];
    BV --> BW[Close Serial Port];
    BW --> BX[Controller Stopped];

    BY[Object Garbage Collected] --> BZ[__del__ Method];
    BZ --> BR;

    J -- Quit Event Set --> BT;

for thread sequence

sequenceDiagram
    participant Main Thread
    participant Recv Thread
    participant Serial Port

    Main Thread->>ArmController: ArmController 생성
    ArmController-->>Recv Thread: 스레드 시작 (recv_thread)
    loop 수신 대기
        Recv Thread->>Serial Port: 데이터 읽기 (ser.read)
        Serial Port-->>Recv Thread: 데이터 반환
        Recv Thread->>Recv Thread: 패킷 처리 및 검증
        alt 피드백 데이터
            Recv Thread->>ArmController: 상태 데이터 갱신 (with lock)
            Recv Thread->>Main Thread: 콜백 함수 호출 (state_cb)
        else 퐁 응답
            Recv Thread->>Main Thread: 콜백 함수 호출 (pong_cb)
        else 설정 피드백
            Recv Thread->>Main Thread: 콜백 함수 호출 (config_cb) (with lock)
        end
    end
    Main Thread->>ArmController: 로봇 팔 제어 명령
    Main Thread->>ArmController: 토크 제어
    Main Thread->>ArmController: PID 파라미터 설정
    ArmController->>Serial Port: 데이터 쓰기 (with write_lock)
    Main Thread->>ArmController: ArmController 종료
    ArmController-->>Recv Thread: 스레드 종료 신호
    Recv Thread-->>Serial Port: 남은 데이터 처리 및 포트 닫기