Unit 8Servos2-Chain Arduino Tutorial

#include <M5Unified.h>
#include "M5Chain.h"

#define TXD_PIN             17
#define RXD_PIN             18
#define SERVO_CHANNEL_COUNT 8
#define ANGLE_STEP          20
#define LOOP_DELAY_MS       200

// Chain state and device list.
Chain M5Chain;
device_list_t *devices_list = nullptr;
uint16_t device_nums = 0;
uint8_t operation_status = 0;
uint8_t angle = 0;
bool servo_ready = false;

void showMessage(const char *message)
{
    // Show a status message on the display.
    M5.Display.clear();
    M5.Display.setCursor(0, 0);
    M5.Display.println(message);
}

bool updateDeviceList()
{
    // Discover all devices on the Chain bus.
    if (!M5Chain.isDeviceConnected()) {
        Serial.println("Chain device not connected");
        return false;
    }

    if (M5Chain.getDeviceNum(&device_nums) != CHAIN_OK || device_nums == 0) {
        Serial.println("Failed to get Chain device count");
        return false;
    }

    // Allocate storage for the device list returned by M5Chain.
    devices_list = (device_list_t *)malloc(sizeof(device_list_t));
    if (devices_list == nullptr) {
        Serial.println("Failed to allocate device list");
        return false;
    }

    devices_list->count = device_nums;
    devices_list->devices = (device_info_t *)malloc(sizeof(device_info_t) * device_nums);
    if (devices_list->devices == nullptr) {
        free(devices_list);
        devices_list = nullptr;
        Serial.println("Failed to allocate device information");
        return false;
    }

    if (!M5Chain.getDeviceList(devices_list)) {
        free(devices_list->devices);
        free(devices_list);
        devices_list = nullptr;
        Serial.println("Failed to get Chain device list");
        return false;
    }

    // Print the detected device IDs and types.
    Serial.printf("Chain device count: %u\r\n", devices_list->count);
    for (uint8_t i = 0; i < devices_list->count; i++) {
        Serial.printf("ID[%u], type: 0x%02X\r\n", devices_list->devices[i].id,
                      devices_list->devices[i].device_type);
    }
    return true;
}

bool initializeServoMode()
{
    if (devices_list == nullptr) {
        return false;
    }

    // Configure all eight channels as servo outputs.
    user_gpio_mode_t modes[SERVO_CHANNEL_COUNT];
    for (uint8_t i = 0; i < SERVO_CHANNEL_COUNT; i++) {
        modes[i] = USER_GPIO_SERVO_MODE;
    }

    bool found = false;
    for (uint8_t i = 0; i < devices_list->count; i++) {
        if (devices_list->devices[i].device_type != UNIT_8SERVOS2_CHAIN_TYPE_CODE) {
            continue;
        }

        found = true;
        uint8_t device_id = devices_list->devices[i].id;
        chain_status_t status = M5Chain.setServosModeAll(
            device_id, modes, SERVO_CHANNEL_COUNT, &operation_status);
        if (status != CHAIN_OK || operation_status != 1) {
            Serial.printf("ID[%u] servo mode setup failed\r\n", device_id);
            continue;
        }

        Serial.printf("ID[%u] servo mode setup success\r\n", device_id);
    }
    return found;
}

bool setAndVerifyAngle(uint8_t target_angle)
{
    if (devices_list == nullptr) {
        return false;
    }

    // Set the same target angle on every servo channel.
    uint8_t target_angles[SERVO_CHANNEL_COUNT];
    uint8_t read_angles[SERVO_CHANNEL_COUNT] = {0};
    for (uint8_t i = 0; i < SERVO_CHANNEL_COUNT; i++) {
        target_angles[i] = target_angle;
    }

    bool all_success = true;
    for (uint8_t i = 0; i < devices_list->count; i++) {
        if (devices_list->devices[i].device_type != UNIT_8SERVOS2_CHAIN_TYPE_CODE) {
            continue;
        }

        uint8_t device_id = devices_list->devices[i].id;
        chain_status_t status = M5Chain.setServosAngleAll(
            device_id, target_angles, SERVO_CHANNEL_COUNT, &operation_status);
        if (status != CHAIN_OK || operation_status != 1 ||
            M5Chain.getServosAngleAll(device_id, read_angles, SERVO_CHANNEL_COUNT) != CHAIN_OK) {
            Serial.printf("ID[%u] angle operation failed\r\n", device_id);
            all_success = false;
            continue;
        }

        // Verify every channel by reading the angle back.
        for (uint8_t channel = 0; channel < SERVO_CHANNEL_COUNT; channel++) {
            if (read_angles[channel] != target_angle) {
                Serial.printf("ID[%u] CH[%u] angle mismatch: %u / %u\r\n",
                              device_id, channel, target_angle, read_angles[channel]);
                all_success = false;
            }
        }
    }
    return all_success;
}

bool readAndDisplayPower(uint8_t current_angle)
{
    if (devices_list == nullptr) {
        return false;
    }

    // Read voltage and current telemetry from each matching device.
    bool all_success = true;
    bool display_updated = false;
    for (uint8_t i = 0; i < devices_list->count; i++) {
        if (devices_list->devices[i].device_type != UNIT_8SERVOS2_CHAIN_TYPE_CODE) {
            continue;
        }

        uint8_t device_id = devices_list->devices[i].id;
        uint16_t dc_voltage = 0;
        uint16_t grove_voltage = 0;
        uint16_t system_current = 0;
        chain_status_t dc_status = M5Chain.getServosDcVoltage(device_id, &dc_voltage);
        chain_status_t grove_status = M5Chain.getServosGroveVoltage(device_id, &grove_voltage);
        chain_status_t current_status = M5Chain.getServosSysCurrent(device_id, &system_current);

        if (dc_status != CHAIN_OK || grove_status != CHAIN_OK || current_status != CHAIN_OK) {
            Serial.printf("ID[%u] power monitor failed: DC=%d, Grove=%d, Current=%d\r\n",
                          device_id, dc_status, grove_status, current_status);
            all_success = false;
            continue;
        }

        Serial.printf("ID[%u] power: DC=%umV, Grove=%umV, Current=%umA\r\n",
                      device_id, dc_voltage, grove_voltage, system_current);

        // Display the first matching device while logging all devices.
        if (!display_updated) {
            M5.Display.clear();
            M5.Display.setCursor(0, 0);
            M5.Display.println("Unit 8Servos2-Chain");
            M5.Display.setCursor(0, 40);
            M5.Display.printf("Angle: %u", current_angle);
            M5.Display.setCursor(0, 80);
            M5.Display.printf("DC: %umV", dc_voltage);
            M5.Display.setCursor(0, 120);
            M5.Display.printf("Current: %umA", system_current);
            M5.Display.setCursor(0, 160);
            M5.Display.printf("Grove: %umV", grove_voltage);
            display_updated = true;
        }
    }
    return all_success;
}

void setup()
{
    M5.begin();
    M5.Display.setFont(&fonts::FreeMonoBold12pt7b);
    M5.Display.setTextSize(1);
    Serial.begin(115200);
    Serial.println("Unit 8Servos2-Chain Test");

    // Start Chain UART communication.
    M5Chain.begin(&Serial2, 115200, RXD_PIN, TXD_PIN);
    if (!updateDeviceList()) {
        showMessage("Chain device not found");
        return;
    }

    servo_ready = initializeServoMode();
    if (!servo_ready) {
        showMessage("8Servos2-Chain not found");
        return;
    }
    showMessage("8Servos2-Chain ready");
}

void loop()
{
    if (!servo_ready) {
        delay(LOOP_DELAY_MS);
        return;
    }

    // Set the next angle and verify the response.
    if (setAndVerifyAngle(angle)) {
        Serial.printf("All servo channels set to %u degrees\r\n", angle);
    } else {
        Serial.println("Servo angle operation failed");
    }

    // Update the power monitor display and serial log.
    readAndDisplayPower(angle);

    // Advance through the 0-180 degree test range.
    angle += ANGLE_STEP;
    if (angle > 180) {
        angle = 0;
    }
    delay(LOOP_DELAY_MS);
}