Base Controller (base_controller.py)

flow diagram

graph TD
    A[Instantiate BaseController] --> B[__init__ Method];
    B --> C[Log Device Name];
    C --> D[Initialize Callbacks and State Variables];
    D --> E[Create Thread Locks];
    E --> F[Initialize Position Dictionaries];
    F --> G[Create Quit Event];
    G --> H[Create Left Motor Thread];
    G --> I[Create Right Motor Thread];
    H --> J[Start Left Motor Thread];
    I --> K[Start Right Motor Thread];
    J --> L[Left odrive_can_thread Runs];
    K --> M[Right odrive_can_thread Runs];
    L --> N[Initialize Left ODrive CAN];
    M --> O[Initialize Right ODrive CAN];
    N --> P[Start Left CAN Communication];
    O --> Q[Start Right CAN Communication];
    P --> R[Clear Left ODrive Errors];
    Q --> S[Clear Right ODrive Errors];
    R --> T[Reset Left Encoder];
    S --> U[Reset Right Encoder];
    T --> V[Set Left Control Mode and Gains];
    U --> W[Set Right Control Mode and Gains];
    V --> X[Enter Left Closed-Loop Control];
    W --> Y[Enter Right Closed-Loop Control];
    X --> Z{Left Motor in Closed-Loop?};
    Y --> AA{Right Motor in Closed-Loop?};
    Z -- No --> AB[Sleep 0.1s Left];
    AB --> Z;
    AA -- No --> AC[Sleep 0.1s Right];
    AC --> AA;
    Z -- Yes --> AD[Assign feedback_cb to Left ODrive];
    AA -- Yes --> AE[Assign feedback_cb to Right ODrive];
    AD --> AF[Left Control Loop Runs];
    AE --> AG[Right Control Loop Runs];

    AF --> AH{Quit Event Set Left?};
    AH -- No --> AI[Acquire Write Lock Left];
    AI --> AJ[Set Left ODrive Input Position];
    AJ --> AK[Check Left ODrive Errors];
    AK --> AL[Sleep 0.1s Left Control];
    AL --> AF;
    AI --> AN[Release Write Lock Left];
    AN --> AK;
    AH -- Yes --> AO[Raise Quit Exception Left];
    AO --> AP[Left ODrive Stop];

    AG --> AQ{Quit Event Set Right?};
    AQ -- No --> AR[Acquire Write Lock Right];
    AR --> AS[Set Right ODrive Input Position];
    AS --> AT[Check Right ODrive Errors];
    AT --> AU[Sleep 0.1s Right Control];
    AU --> AG;
    AR --> AV[Release Write Lock Right];
    AV --> AT;
    AQ -- Yes --> AW[Raise Quit Exception Right];
    AW --> AX[Right ODrive Stop];


    J --> BA[Wait for First Feedback];
    K --> BA;
    BA --> BB[Controller Ready];

    BB --> BC[feedback_cb Triggered by ODrive CAN];
    BC --> BD{Message is Encoder Estimates?};
    BD -- Yes --> BE[Extract Pos and Vel];
    BE --> BF[Log Debug Info];
    BF --> BG[Update curpos];
    BG --> BH{Debug Callback Registered?};
    BH -- Yes --> BI[Call Debug Callback curpos];
    BI --> BJ[Record Current Time];
    BH -- No --> BJ;
    BD -- No --> BJ;

    BJ --> BK[Acquire State Lock];
    BK --> BL{First Update?};
    BL -- Yes --> BM[Initialize Last Velocities and Time];
    BM --> BN[Release State Lock];
    BN --> BP{State Callback Registered?};
    BP -- Yes --> BQ[Call State Callback with Velocities and dt];
    BQ --> BB;
    BP -- No --> BB;

    BL -- No --> BR[Calculate dt];
    BR --> BS[Adjust Wheel Vel by Install Dir];
    BS --> BT[Update Last Wheel Velocities];
    BT --> BU[Convert Wheel Vel to Robot Vel];
    BU --> BN;

    BB --> BV[Call set_vel method];
    BV --> BW[Convert Robot Vel to Wheel Vel];
    BW --> BX[Acquire Write Lock set_vel];
    BX --> BY[Update setpos with Velocity*dt];
    BY --> BZ[Clamp setpos];
    BZ --> CA[Log Clamping];
    CA --> CB{Debug Callback Registered set_vel?};
    CB -- Yes --> CC[Call Debug Callback setpos];
    CC --> CD[Release Write Lock set_vel];
    CD --> BB;
    CB -- No --> CD;
    BZ --> CB;

    BB --> CE[Call stop method];
    CE --> CF[Set Quit Event];
    CF --> CG[Allow Threads to Stop];
    CG --> CH[Controller Stopped];

    CI[Object Garbage Collected] --> CJ[__del__ Method];
    CJ --> CE;

    L --> AP;
    M --> AX;

Class Structure

classDiagram
    class BaseController {
        -device_name: str
        -curpos: dict
        -setpos: dict
        -last_left_vel: float
        -last_right_vel: float
        +LEFT_NODE_ID: int
        +RIGHT_NODE_ID: int
        +WHEEL_BASE: float
        +WHEEL_D: float
        +__init__(name: str)
        +set_vel(linear_vel: float, angular_vel: float)
        +stop()
        -feedback_cb(msg, caller)
        -odrive_can_thread(node_id: int)
    }

    class ODriveCAN {
        +start()
        +clear_errors()
        +set_controller_mode()
        +set_input_pos()
        +get_encoder()
        +get_velocity()
    }

    BaseController --> ODriveCAN : uses

Component Interaction

flowchart TD
    subgraph BaseController
        VH[Velocity Handler]
        KM[Kinematics Module]
        PH[Position Handler]
        FB[Feedback Handler]
    end

    subgraph ODrive
        LC[Left Controller]
        RC[Right Controller]
        LE[Left Encoder]
        RE[Right Encoder]
    end

    subgraph External
        VC[Velocity Commands]
        SF[State Feedback]
    end

    VC -->|cmd_vel| VH
    VH -->|compute| KM
    KM -->|wheel velocities| PH
    PH -->|position commands| LC & RC
    LE & RE -->|encoder data| FB
    FB -->|state| SF

    style BaseController fill:#bbf,stroke:#333,stroke-width:2px
    style ODrive fill:#fbf,stroke:#333,stroke-width:2px
    style External fill:#fbb,stroke:#333,stroke-width:2px

Control Flow

sequenceDiagram
    participant App
    participant Controller
    participant Kinematics
    participant ODriveLeft
    participant ODriveRight

    App->>Controller: set_vel(linear, angular)
    Controller->>Kinematics: compute_wheel_velocities()
    Kinematics-->>Controller: left_vel, right_vel
    Controller->>ODriveLeft: set_input_pos()
    Controller->>ODriveRight: set_input_pos()
    
    loop Feedback
        ODriveLeft-->>Controller: encoder_feedback
        ODriveRight-->>Controller: encoder_feedback
        Controller->>App: state_callback
    end

State Machine

stateDiagram-v2
    [*] --> Initializing
    Initializing --> ClearingErrors: Start
    ClearingErrors --> ConfiguringControllers: Errors Cleared
    ConfiguringControllers --> Ready: Configuration Complete
    Ready --> Moving: Velocity Command
    Moving --> Ready: Stop Command
    Ready --> [*]: Shutdown
    Moving --> ErrorState: Error Detected
    ErrorState --> ClearingErrors: Recovery

Differential Drive Kinematics

graph LR
    subgraph Inputs
        LV[Linear Velocity]
        AV[Angular Velocity]
    end

    subgraph Parameters
        WB[Wheel Base]
        WD[Wheel Diameter]
    end

    subgraph Computation
        K[Kinematics]
    end

    subgraph Outputs
        LW[Left Wheel Velocity]
        RW[Right Wheel Velocity]
    end

    LV --> K
    AV --> K
    WB --> K
    WD --> K
    K --> LW
    K --> RW

    style Inputs fill:#bbf
    style Parameters fill:#fbf
    style Computation fill:#bfb
    style Outputs fill:#fbb

Error Handling

flowchart TD
    A[Error Detected] --> B{Error Type}
    B -->|Communication| C[Reset CAN]
    B -->|Controller| D[Clear ODrive Error]
    B -->|Position| E[Reset Position]
    C & D & E --> F{Recovery Success?}
    F -->|Yes| G[Resume Operation]
    F -->|No| H[Emergency Stop]

Thread Management

graph TD
    subgraph Main Thread
        A[Command Processing]
        B[State Management]
    end

    subgraph CAN Threads
        C[Left ODrive Thread]
        D[Right ODrive Thread]
    end

    subgraph Callback Thread
        E[Feedback Processing]
    end

    A -->|commands| C & D
    C & D -->|feedback| E
    E -->|updates| B

    style Main Thread fill:#bbf
    style CAN Threads fill:#fbf
    style Callback Thread fill:#bfb

Configuration Parameters

• LEFT_NODE_ID: CAN ID for left motor
• RIGHT_NODE_ID: CAN ID for right motor
• WHEEL_BASE: Distance between wheels (m)
• WHEEL_D: Wheel diameter (m)
• Position gain: 20.0
• Velocity gains: [0.1, 0.0]
• Velocity limit: 20 turns/s
• Acceleration limit: 1 turn/s²

Dependencies

• odrive_can: ODrive CAN interface
• threading: Thread management
• math: Mathematical computations
• time: Timing functions
• logging: Error and debug logging

Notes

• Uses position control with trajectory for smooth motion
• Implements velocity commands through position integration
• Thread-safe velocity commands and state updates
• Automatic error recovery
• Real-time feedback processing
• Independent control threads per motor
• Position difference limiting for safety