Unit 8Servos2-I2C Arduino Tutorial

#include <M5Unified.h>
#include <M5Unit8Servos2.h>

constexpr int I2C_SDA_PIN             = 2;
constexpr int I2C_SCL_PIN             = 1;
constexpr uint8_t UNIT_I2C_ADDRESS    = UNIT_8SERVOS2_DEFAULT_ADDR;
constexpr uint8_t SERVO_CHANNEL_COUNT = 8;
constexpr uint32_t I2C_FREQUENCY      = 400000;
constexpr int16_t STATUS_LINE_HEIGHT  = 40;

// Store the Unit controller and current servo position.
M5Unit8Servos2 servos2;
int16_t angle     = 0;
int8_t angle_step = 20;

// Refresh the display with servo and power data.
void drawStatus(uint16_t dc_voltage, uint16_t grove_voltage, uint16_t current_mA)
{
    M5.Display.fillScreen(TFT_BLACK);
    M5.Display.setCursor(0, 0);
    M5.Display.printf("Unit 8Servos2-I2C\n");
    M5.Display.setCursor(0, STATUS_LINE_HEIGHT);
    M5.Display.printf("Angle: %d deg", angle);
    M5.Display.setCursor(0, STATUS_LINE_HEIGHT * 2);
    M5.Display.printf("DC: %umV", static_cast<unsigned>(dc_voltage));
    M5.Display.setCursor(0, STATUS_LINE_HEIGHT * 3);
    M5.Display.printf("Current: %umA", static_cast<unsigned>(current_mA));
    M5.Display.setCursor(0, STATUS_LINE_HEIGHT * 4);
    M5.Display.printf("Grove: %umV", static_cast<unsigned>(grove_voltage));
}

void setup()
{
    auto cfg = M5.config();
    M5.begin(cfg);
    Serial.begin(115200);
    M5.Display.setFont(&fonts::FreeMonoBold12pt7b);
    M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);

    Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN, I2C_FREQUENCY);
    while (!servos2.begin(&Wire, UNIT_I2C_ADDRESS, -1, -1, I2C_FREQUENCY)) {
        Serial.println("Unit 8Servos2-I2C not found");
        M5.Display.fillScreen(TFT_BLACK);
        M5.Display.setCursor(0, 0);
        M5.Display.printf("Unit not found");
        M5.Display.setCursor(0, STATUS_LINE_HEIGHT);
        M5.Display.printf("Check I2C and address");
        delay(1000);
    }

    // Set all channels to servo mode.
    for (uint8_t channel = 0; channel < SERVO_CHANNEL_COUNT; ++channel) {
        servos2.setMode(channel, M5_8SERVOS2_MODE_SERVO);
    }

    // Set both PWM timers to 50 Hz for standard servos.
    servos2.setTimerFrequency(0, 50);
    servos2.setTimerFrequency(1, 50);

    // Read the initial power telemetry.
    const uint16_t dc_voltage    = servos2.getDCVoltage();
    const uint16_t grove_voltage = servos2.getGroveVoltage();
    const uint16_t current_mA    = servos2.getSysCurrent();
    Serial.println("Unit 8Servos2-I2C ready");
    drawStatus(dc_voltage, grove_voltage, current_mA);
}

void loop()
{
    // Apply the current angle to all servo channels.
    for (uint8_t channel = 0; channel < SERVO_CHANNEL_COUNT; ++channel) {
        servos2.setServoAngle(channel, static_cast<uint8_t>(angle));
    }

    // Read and report the latest power telemetry.
    const uint16_t dc_voltage    = servos2.getDCVoltage();
    const uint16_t grove_voltage = servos2.getGroveVoltage();
    const uint16_t current_mA    = servos2.getSysCurrent();
    Serial.printf(
        "Servo angle: %d deg, DC: %u mV, Grove: %u mV, Current: %u mA\n", angle,
        static_cast<unsigned>(dc_voltage), static_cast<unsigned>(grove_voltage),
        static_cast<unsigned>(current_mA));
    drawStatus(dc_voltage, grove_voltage, current_mA);

    // Sweep the servos between 0 and 180 degrees.
    angle += angle_step;
    if (angle >= 180) {
        angle      = 180;
        angle_step = -20;
    } else if (angle <= 0) {
        angle      = 0;
        angle_step = 20;
    }

    delay(200);
}