Draco4 Code Analysis - Complete Function-Level Documentation


Home | API Reference | Developer Guide | Code Analysis | Usage Examples

Overview

The Draco4 Multi-Motor EtherCAT Control Framework is a sophisticated real-time control system designed for complex robotic applications. The architecture provides layered abstraction from individual motor control up to multi-network robot coordination, with comprehensive safety systems, automatic motor detection, and configuration-driven parameter management.

System Architecture

The framework implements a hierarchical multi-layer control architecture:

Layer 1: Robot Controller (robot_controller.cpp)

Layer 2: Network Controller (network_controller.cpp)

Layer 3: Motor Controller (motor_controller.cpp)

Layer 4: Supporting Systems

File-by-File Function Analysis

1. motor_constants.cpp

Provides configuration-driven motor constants and parameters with fallback defaults.

Key Functions:

Purpose: Acts as a safety layer providing fallback constants when configuration files aren’t available while allowing motor-specific tuning when configs are loaded.

2. motor_configuration.cpp

Implements comprehensive CSV configuration file parsing for motor parameters.

Core Classes:

Key Functions:

Purpose: Enables motor-agnostic operation by loading motor-specific parameters from CSV files, supporting different motor models (JD8, JD10, JD12) with their unique characteristics.

3. motor_controller.cpp

Main motor control class implementing EtherCAT-based control for individual motors.

Constructor & Lifecycle:

Core Control Functions:

Control Mode Functions:

Feedback Functions:

State Management:

Internal Control Logic:

Configuration Management:

Purpose: Implements the complete control stack for a single motor, handling real-time control, safety systems, and motor-specific configuration.

4. motor_manager.cpp

Provides motor type detection and configuration management.

Core Functions:

Purpose: Enables automatic motor detection and configuration file selection based on detected motor hardware, supporting seamless multi-motor setups with different motor types.

5. motor_sdo_manager.cpp

Handles EtherCAT SDO (Service Data Object) communication for uploading configuration parameters.

Core SDO Operations:

High-Level Configuration Upload:

Internal Helpers:

Purpose: Provides the communication layer between the configuration system and the motor hardware, ensuring parameters are safely uploaded and verified.

6. network_controller.cpp

Implements the EtherCAT network management layer.

Network Lifecycle:

Real-Time Communication:

Data Access:

Internal Management:

Motor Detection Integration:

Purpose: Manages the complete EtherCAT network infrastructure, providing real-time communication and slave management services to motor controllers.

7. robot_controller.cpp

Provides multi-network coordination for complex robotic systems.

Network Management:

Robot-Level Operations:

Status and Diagnostics:

Internal Coordination:

Purpose: Orchestrates multiple EtherCAT networks with synchronized timing, enabling complex robotic systems with distributed motor control across multiple network segments.

Control Logic Flow

Data Flow Architecture

Configuration Data Flow:

  1. CSV configuration files → MotorConfigParser → MotorController
  2. MotorController → SDOManager → EtherCAT SDO → Motor hardware
  3. Motor-specific parameters enable adaptive control for different motor types

Real-Time Control Data Flow:

  1. User commands → MotorController (position/velocity/torque setpoints)
  2. MotorController → NetworkController (OutputPDO structures)
  3. NetworkController → EtherCAT network → Motor drives
  4. Motor drives → EtherCAT network → NetworkController (InputPDO structures)
  5. NetworkController → MotorController → User applications (feedback data)

Safety Data Flow:

  1. Fault detection in MotorController (from InputPDO status)
  2. Automatic fault recovery (torque ramp to zero)
  3. Emergency stop propagation from RobotController to all networks
  4. Parameter validation at SDO upload and real-time command levels

Initialization Sequence and Program Flow

System Startup Sequence

Phase 1: Robot Controller Initialization

  1. RobotController() constructor (line 19)
    • Sets global timing parameters (frequency, cycle time)
    • Initializes synchronization primitives and error handling
    • Prepares for multi-network coordination

Phase 2: Network Configuration

  1. addNetwork() (line 42)
    • Validates network configuration (interface name, frequency, etc.)
    • Creates NetworkController instances for each EtherCAT segment
    • Establishes network-specific timing parameters

Phase 3: Network Infrastructure Setup

  1. initializeAllNetworks() (line 134)
    • Parallel initialization of all EtherCAT masters
    • NetworkController::initialize() (line 46)
      • Initializes SOEM EtherCAT master on specified interface
      • Allocates IOmap buffer for process data
      • Sets up network communication infrastructure

Phase 4: EtherCAT Network Discovery

  1. startAllNetworks() (line 192)
    • Parallel network scanning and configuration
    • NetworkController::scanNetwork() (line 76)
      • Scans for EtherCAT slaves using ec_config_init()
      • Detects all connected motor drives
    • NetworkController::configureSlaves() (line 100)
      • Maps process data with ec_config_map()
      • Configures distributed clocks for synchronization
      • Sets up PDO pointers for real-time communication

Phase 5: Motor Detection and Configuration

  1. initializeSlaveInfo() (line 430)
    • Extracts slave information (product ID, vendor ID, PDO sizes)
    • Motor Manager detects motor types from product IDs
    • Maps motor types to appropriate configuration files
  2. Motor Controller Configuration:
    • MotorController::loadConfig() (line 603)
    • MotorConfigParser::parseCSV() (line 88)
      • Parses motor-specific parameters from CSV files
      • Validates critical parameters (encoder resolution, safety limits)
    • MotorController::uploadConfig() (line 642)
      • Uploads parameters to motor hardware via SDO

Phase 6: EtherCAT Operational State

  1. NetworkController::startOperation() (line 149)
    • Initializes output PDOs with safe values (zero torque, shutdown state)
    • Transitions all slaves to operational state using CIA402 state machine
    • Verifies operational state and calculates expected working counter
    • Performs initial communication cycle

Real-Time Operation Loop

Main Control Cycle (typically 250Hz)

  1. RobotController::updateAllNetworks() (line 278)
    • Parallel execution across all networks
    • Maintains global timing synchronization

Per-Network Communication Cycle

  1. NetworkController::performCommunicationCycle() (line 287)
    • ec_send_processdata(): Sends output PDOs to all slaves
    • ec_receive_processdata(): Receives input PDOs from all slaves
    • Updates working counter and error detection

Per-Motor Control Update

  1. MotorController::update() (line 50)
    • Executes active control mode (position, velocity, or torque)
    • Performs safety checks and fault detection
    • Updates control commands based on current mode

Control Mode Execution:

Motor Enable Sequence

CIA402 State Machine Implementation:

  1. MotorController::enable_motor() (line 68)
    • Executes state machine transition over multiple cycles
    • enable_motor_sequence() (line 412) implements:
      • Step 0: Fault reset if needed (controlword = 0x0080)
      • Step 1: Shutdown (controlword = 0x0006) → Ready to Switch On
      • Step 2: Switch On (controlword = 0x0007) → Switched On
      • Step 3: Enable Operation (controlword = 0x000F) → Operation Enabled

Safety and Fault Handling

Fault Detection:

Emergency Stop Sequence:

  1. RobotController::emergencyStopAll() (line 330)
    • Immediate stop across all networks
    • Parallel emergency stop execution
    • Sets emergency stop flag preventing further operation

Graceful Shutdown:

  1. RobotController::shutdownAll() (line 358)
    • Transitions slaves to safe operational state
    • Closes EtherCAT master connections
    • Releases all resources

Configuration Parameter Flow

Parameter Upload Sequence:

  1. Motor-specific CSV files define tuning parameters
  2. MotorConfigParser validates and organizes parameters
  3. MotorSDOManager uploads in priority order:
    • Priority 1: Safety limits (critical for protection)
    • Priority 2: Control gains (important for performance)
    • Priority 3: Motor specifications (optimization parameters)

Real-Time Parameter Validation:

Key Design Features

1. Hierarchical Architecture

2. Real-Time Performance

3. Safety Systems

4. Configuration Management

5. Advanced Control Features

Conclusion

The Draco4 Multi-Motor EtherCAT Control Framework represents a comprehensive solution for high-performance robotic control applications. Its hierarchical architecture, robust safety systems, and configuration-driven approach make it suitable for complex multi-motor systems requiring precise, synchronized control with real-time performance guarantees.

The framework’s design emphasizes:

This makes it an excellent foundation for advanced robotic systems requiring reliable, high-performance motor control.