Lift Controller (lift_controller.py)

flowchart

graph TD
    A[Instantiate LiftController] --> 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[Echo Data to Stdout];
    M --> K;
    L -- Yes --> N[Read Remaining Packet Bytes];
    N --> O{Packet Length Correct?};
    O -- No --> K;
    O -- Yes --> P{Packet Type?};
    P --> |COMM_TYPE_PONG| Q{Pong Callback Registered?};
    Q -- Yes --> R[Call Pong Callback];
    R --> K;
    Q -- No --> K;
    P --> |COMM_TYPE_FEEDBACK| S[Unpack Raw Position];
    S --> T[Convert Raw to SI Units Meters];
    T --> U[Record Current Time];
    U --> V[Acquire State Lock];
    V --> W{First Update?};
    W -- Yes --> X[Initialize Last State and Time];
    X --> Y[Release State Lock];
    Y --> Z{State Callback Registered?};
    Z -- Yes --> AA[Call State Callback with State];
    AA --> K;
    Z -- No --> K;
    W -- No --> AB[Calculate Velocity and Effort];
    AB --> AC[Update Last State and Time];
    AC --> Y;

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

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

    BI --> BM[Call stop method];
    BM --> BN[Set Quit Event];
    BN --> BO[Receive Thread Exits Loop];
    BO --> BP[Close Serial Port];
    BP --> BQ[Controller Stopped];

    BR[Object Garbage Collected] --> BS[__del__ Method];
    BS --> BM;

    J -- Quit Event Set --> BO;

Class Structure

classDiagram
    class LiftController {
        -current_height: float
        -target_height: float
        -height_limits: tuple
        -motor_controller
        -state: str
        +__init__()
        +set_height(height)
        +get_height()
        +home()
        +stop()
        -update_state()
        -check_limits()
    }

    class MotorController {
        +set_position()
        +get_position()
        +get_velocity()
        +enable()
        +disable()
    }

    class SafetyMonitor {
        +check_limits()
        +check_load()
        +check_motor_temp()
    }

    LiftController --> MotorController : controls
    LiftController --> SafetyMonitor : uses

Component Interaction

flowchart TD
    subgraph LiftController
        HC[Height Controller]
        SM[Safety Monitor]
        FB[Feedback Handler]
    end

    subgraph Hardware
        LM[Lift Motor]
        LS[Limit Switches]
        LC[Load Cell]
    end

    subgraph External
        CMD[Commands]
        ST[State]
    end

    CMD -->|height| HC
    HC -->|check| SM
    SM -->|validated| LM
    LM -->|position| FB
    LS -->|limits| SM
    LC -->|load| SM
    FB -->|state| ST

    style LiftController fill:#bbf,stroke:#333,stroke-width:2px
    style Hardware fill:#fbf,stroke:#333,stroke-width:2px
    style External fill:#fbb,stroke:#333,stroke-width:2px

Control Flow

sequenceDiagram
    participant External
    participant Controller
    participant Safety
    participant Motor
    participant Sensors

    External->>Controller: set_height(target)
    Controller->>Safety: check_limits()
    Safety->>Sensors: get_limit_state()
    Sensors-->>Safety: limit_status
    Safety-->>Controller: validated_height
    Controller->>Motor: move_to_position()
    
    loop State Update
        Motor->>Controller: position_feedback
        Sensors->>Controller: sensor_feedback
        Controller->>External: lift_state
    end

State Machine

stateDiagram-v2
    [*] --> Initializing
    Initializing --> Homing: Start
    Homing --> Ready: Home Complete
    Ready --> Moving: Height Command
    Moving --> Ready: Motion Complete
    Ready --> [*]: Shutdown
    
    state Moving {
        [*] --> ValidatingCommand
        ValidatingCommand --> CheckingLimits
        CheckingLimits --> ExecutingMotion
        ExecutingMotion --> MonitoringLoad
        MonitoringLoad --> [*]
    }

Motion Profile

graph LR
    subgraph Height Profile
        H1[Current Height]
        H2[Target Height]
        H3[Interpolated Path]
    end

    subgraph Velocity Profile
        V1[Acceleration]
        V2[Constant Velocity]
        V3[Deceleration]
    end

    subgraph Constraints
        L1[Height Limits]
        L2[Load Limits]
        L3[Speed Limits]
    end

    H1 --> H3
    H2 --> H3
    V1 & V2 & V3 --> H3
    L1 & L2 & L3 -->|constrain| H3

    style Height Profile fill:#bbf
    style Velocity Profile fill:#fbf
    style Constraints fill:#fbb

Error Handling

flowchart TD
    A[Error Detected] --> B{Error Type}
    B -->|Height Limit| C[Stop Motion]
    B -->|Overload| D[Emergency Stop]
    B -->|Motor Error| E[Disable Power]
    C --> F[Return to Safe]
    D --> G[Release Load]
    E --> H[Reset Required]
    F & G --> I[Resume Ready]

Configuration

Motor Parameters

graph LR
    subgraph Motion Control
        P1[Position PID]
        P2[Velocity Limits]
        P3[Acceleration]
    end

    subgraph Safety Limits
        S1[Height Range]
        S2[Load Limit]
        S3[Temperature]
    end

    subgraph Feedback
        F1[Position]
        F2[Velocity]
        F3[Current]
    end

    style Motion Control fill:#bbf
    style Safety Limits fill:#fbf
    style Feedback fill:#fbb

Dependencies

• ROS2 Packages:
  • rclpy
  • sensor_msgs
  • std_msgs
• Python Libraries:
  • numpy
  • math
  • time

Topics

Published Topics

• /lift/state (sensor_msgs/JointState)
  • lift_joint position
  • velocity
  • effort (load)

Subscribed Topics

• /lift/command (std_msgs/Float64)
  • Target height

Notes

• Implements smooth height control
• Enforces height limits
• Load monitoring and safety cutoff
• Real-time state feedback
• Homing sequence support
• Emergency stop functionality
• Temperature monitoring
• Power management