Dummy-Robot Smart Robotic Arm

The Dummy-Robot implements both forward and inverse kinematics using traditional DH parameter calculations.

Inverse Kinematics (IK) Strategy

Since inverse kinematics for a 6-DOF arm can result in multiple solutions (up to 8), the algorithm selects the configuration that results in the minimum total angular change across all six joints compared to the current pose. This ensures smooth transitions and minimal movement.

Motion Planning (Trajectory Generation)

To ensure synchronized movement across all joints, the system uses a trapezoidal velocity/acceleration profile:

1. When a MoveJ (joint motion) command is received, the controller calculates the angular difference for all 6 joints.
2. It identifies the largest angular difference ($\theta_{max}$).
3. It calculates the required time for $\theta_{max}$ based on the JointSpeed parameter (accounting for acceleration/deceleration).
4. It then scales the acceleration, maximum velocity, and travel time for the remaining 5 motors so that all joints start and finish their motion simultaneously.

Synchronization via CAN Bus

Motors are synchronized using a broadcast mechanism. Each motor listens to two IDs: its own unique ID and a global ID 0.

• Motion commands are stored in a shadow register upon receipt.
• The actual movement is triggered only when the motor receives a broadcast synchronization signal on ID 0, ensuring all joints move in unison.

The Dummy firmware supports three distinct command modes, which can be received via USB, Serial (UART), or CAN. Choose a mode based on your application's requirements for frequency, execution style, and latency:

1. SEQ (Sequential Command Mode):

   • Execution: Uses a FIFO (First-In-First-Out) queue. Commands are executed one after another.
   • Interruptibility: Cannot be interrupted by new commands.
   • Characteristics: There is a pause between commands because the robot decelerates to zero before starting the next one.
   • Best for: Applications where reaching specific key poses is critical, such as visual grasping or palletizing.

2. INT (Real-time Command Mode):

   • Execution: New commands immediately overwrite the currently executing command.
   • Interruptibility: Highly interruptible; provides immediate response.
   • Characteristics: If you send a burst of commands at once, only the last one will effectively be executed.
   • Best for: Motion synchronization and real-time control.

3. ToDoTRJ (Trajectory Tracking Mode):

   • Execution: Uses automatic interpolation to execute commands at a fixed, high frequency.
   • Interruptibility: Cannot be interrupted.
   • Characteristics: Operates at a fixed frequency of 200Hz. Speed may be reduced to maintain precision.
   • Best for: Applications requiring precise path following, such as 3D printing, engraving, or drawing.

| Mode | Frequency | Execution Style | Interruptible | Pause between commands | Best Use Case |
| :--- | :--- | :--- | :--- | :--- | :--- |
| **SEQ** | Random, Low (<5Hz) | FIFO queue | No | Yes | Visual grasping, palletizing |
| **INT** | Random, Unlimited | Overwrites current command | Yes | No | Motion synchronization |
| **ToDoTRJ** | Fixed, High (200Hz) | Auto-interpolation | No | No | 3D printing, engraving, drawing |

The stepper motor drivers (20 and 42 versions) are designed for closed-loop control via CAN or UART.

Initial Setup

1. Download and flash the provided firmware.
2. Power on the motor for the first time; the motor will automatically perform encoder calibration.
3. If calibration succeeds, press Button 1 to enter closed-loop mode.

Button Controls

• Power-on with both buttons held: Forces an automatic encoder calibration (useful if the first attempt fails).
• Short press Button 1: Toggles between Enable Closed-loop and Disable Closed-loop.
• Long press Button 1: Restarts the board.
• Short press Button 2: Clears stall protection.
• Long press Button 2: Resets the target value to zero (e.g., resets position in position mode).

Communication

Control commands are sent via CAN or UART. Implementation details for these interfaces can be found in the source code files UserApp/interface_can.cpp and UserApp/interface_uart.cpp.

// Refer to these files for command implementation details:
// UserApp/interface_can.cpp
// UserApp/interface_uart.cpp

To set up the stepper motor driver, you must attach the hardware driver and encoder to the Motor instance, initialize the components, and then start the hardware timer interrupts.

Typical initialization sequence:

1. Attach the driver (e.g., TB67H450) and encoder (e.g., MT6816).
2. Call .Init() on the controller, driver, and encoder.
3. Start the hardware timer interrupts (e.g., HAL_TIM_Base_Start_IT) to drive the control loops.

The control loop relies on two main frequencies:

• 100Hz: Used for low-frequency tasks like button debouncing and status LED updates.
• 20kHz: Used for high-frequency motor control (motor.Tick20kHz()) or encoder calibration (encoderCalibrator.Tick20kHz()).

motor.AttachDriver(&tb67H450);
motor.AttachEncoder(&mt6816);
motor.controller->Init();
motor.driver->Init();
motor.encoder->Init();

// Start control loops
HAL_TIM_Base_Start_IT(&htim1);  // 100Hz
HAL_TIM_Base_Start_IT(&htim4);  // 20kHz

The core firmware is based on FreeRTOS and provides a high-level API for robot control. The central class is DummyRobot, which handles kinematics and motion planning.

Initialization Requirements

When initializing the DummyRobot class, you must configure two main sets of parameters:

1. Stepper Motor Driver Information: For each joint, specify:

   • CAN Node ID
   • Reverse Direction (boolean)
   • Gear Reduction Ratio
   • Motion Limit Range

2. DH Parameters: The robot's configuration must follow the Denavit-Hartenberg (DH) convention. The robot design must satisfy the Pieper criterion (three adjacent joint axes intersecting at a single point or three axes being parallel) to ensure an analytical solution for inverse kinematics.

Firmware Structure

• BSP Driver: Hardware drivers (OLED, IMU, LED, Buzzer, Non-volatile storage).
• Robot: The core library containing kinematics algorithms and driver code.
• UserApp: The top-level application layer where you implement custom logic using the provided APIs.

The BoardConfig_t structure is used to manage persistent motor and controller settings stored in EEPROM. It defines limits for current, velocity, and acceleration, as well as PID parameters for the Direct Current Control (DCE).

Key fields include:

• canNodeId: The CAN identifier for the node.
• encoderHomeOffset: Offset for the encoder home position.
• defaultMode: The initial Motor::Mode_t the motor enters on boot.
• currentLimit: Rated current in Amperes.
• velocityLimit: Rated velocity in r/s.
• velocityAcc: Rated acceleration in r/s^2.
• dce_kp, dce_kv, dce_ki, dce_kd: PID control parameters.
• enableMotorOnBoot: Boolean to enable motor power immediately after boot.
• enableStallProtect: Boolean to enable stall protection.

Note: The configStatus field manages the lifecycle of settings: CONFIG_OK for normal operation, CONFIG_COMMIT to save changes to EEPROM, and CONFIG_RESTORE to trigger a system reset after saving.

boardConfig = BoardConfig_t{
    .configStatus = CONFIG_OK,
    .canNodeId = defaultNodeID,
    .encoderHomeOffset = 0,
    .defaultMode = Motor::MODE_COMMAND_POSITION,
    .currentLimit = 1 * 1000,    // A
    .velocityLimit = 30 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS, // r/s
    .velocityAcc = 100 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS,   // r/s^2
    .calibrationCurrent=2000,
    .dce_kp = 200,
    .dce_kv = 80,
    .dce_ki = 300,
    .dce_kd = 250,
    .enableMotorOnBoot=false,
    .enableStallProtect=false
};

The DummyRobot class is the primary interface for controlling the robotic arm. It manages joint states, command modes, and motion execution.

Key Methods

• Init(): Initializes the robot hardware and communication.
• IsEnabled(): Returns whether the robot is currently in an active/enabled state.
• MoveJoints(targetJoints): Sends control commands to motors to move to specific target joint positions. This is used in several command modes.
• UpdateJointAngles(): Updates the internal joint angle states (used when the robot is disabled).
• UpdateJointPose6D(): Updates the 6D pose (XYZ and ABC) based on current joint angles.

Command Modes (commandMode)

The robot operates in different modes that dictate how MoveJoints behaves:

• COMMAND_TARGET_POINT_SEQUENTIAL
• COMMAND_TARGET_POINT_INTERRUPTABLE
• COMMAND_CONTINUES_TRAJECTORY
• COMMAND_MOTOR_TUNING: Uses tuningHelper.Tick(ms) instead of direct joint movement.

Data Members

• currentJoints.a[6]: Array containing the current angles of the 6 joints.
• currentPose6D: Contains the 6D pose data (X, Y, Z, A, B, C).
• jointsStateFlag: A bitmask representing the state of the joints.

// Example of typical control loop logic
if (dummy.IsEnabled())
{
    switch (dummy.commandMode)
    {
        case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
        case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
        case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
            dummy.MoveJoints(dummy.targetJoints);
            dummy.UpdateJointPose6D();
            break;
        case DummyRobot::COMMAND_MOTOR_TUNING:
            dummy.tuningHelper.Tick(10);
            dummy.UpdateJointPose6D();
            break;
    }
}

The firmware utilizes PWM and SSD1306 (OLED) classes for hardware control and visualization.

PWM Configuration

• Initialization: PWM pwm(frequency, frequency); defines the PWM frequency for two sets of channels.
• Starting: Call pwm.Start() to enable the timers.
• Setting Duty Cycle: Use pwm.SetDuty(channel, duty_cycle) where channel is a constant like PWM::CH_A1 and duty_cycle is a float (e.g., 0.5).

OLED (SSD1306) Configuration

• Initialization: oled.Init() prepares the display.
• Drawing: Supports standard graphics like oled.clearBuffer(), oled.drawBox(...), and oled.setCursor(...).
• Text: Use oled.setFont(...) to select fonts and oled.printf(...) to print formatted strings to the screen.
• Displaying: Data is written to a buffer and must be sent to the hardware using oled.sendBuffer().

// PWM setup
PWM pwm(21000, 21000);
pwm.Start();
pwm.SetDuty(PWM::CH_A1, 0.5);

// OLED usage
SSD1306 oled(&hi2c0);
// ... inside a loop
oled.clearBuffer();
oled.setCursor(0, 10);
oled.printf("Text: %d", value);
oled.sendBuffer();