Module13.2 Servo2 Arduino Tutorial

#include <M5Unified.h>
#include <Wire.h>
#include "Adafruit_PWMServoDriver.h"

#define SERVO_FREQ 50  // Servo PWM frequency.

Adafruit_PWMServoDriver* pwm = nullptr;

void setup()
{
    auto cfg = M5.config();
    cfg.output_power = false;// Essential for Core2/CoreS3
    M5.begin(cfg);
    M5.Lcd.setFont(&fonts::FreeMonoBold12pt7b);

    // Get M5Bus I2C pins.
    const int8_t wireSda = M5.getPin(m5::pin_name_t::in_i2c_sda);
    const int8_t wireScl = M5.getPin(m5::pin_name_t::in_i2c_scl);

    // Find the configured SERVO2 address.
    uint8_t servo2Address = 0;
    Serial.println("Scanning SERVO2 addresses 0x40-0x47...");
    for (uint8_t address = 0x40; address <= 0x47; ++address) {
        if (M5.In_I2C.scanID(address)) {
            servo2Address = address;
            Serial.printf("Found PCA9685 at 0x%02X\n", address);
            break;
        }
    }

    if (servo2Address == 0) {
        Serial.println("PCA9685 not found on M5Bus");
        M5.Display.drawString("PCA9685 not found", 10, 80);
        while (true) {
            delay(1000);
        }
    }

    Wire.setPins(wireSda, wireScl);
    pwm = new Adafruit_PWMServoDriver(servo2Address, Wire);

    if (!pwm->begin()) {
        Serial.printf("Wire could not initialize PCA9685 at 0x%02X\n", servo2Address);
        M5.Display.drawString("Wire init failed", 10, 80);
        while (true) {
            delay(1000);
        }
    }

    // Set the PWM frequency for analog servos.
    pwm->setPWMFreq(SERVO_FREQ);

    M5.Lcd.setTextColor(TFT_GREEN, TFT_BLACK);
    M5.Lcd.drawCenterString("Module13.2 Servo2 Test", 160, 10);
}

// Set one PCA9685 channel using a pulse width in milliseconds.
void setServoPulse(uint8_t n, double pulse)
{
    double pulselength;

    // Calculate the PWM period in microseconds.
    pulselength = 1000000;
    pulselength /= SERVO_FREQ;

    // Print the period for debugging.
    Serial.print(pulselength);
    Serial.println(" us per period");

    // Convert the period to one 12-bit PWM step.
    pulselength /= 4096;
    Serial.print(pulselength);
    Serial.println(" us per bit");

    // Convert the pulse width from milliseconds to PWM steps.
    pulse *= 1000;
    pulse /= pulselength;
    Serial.println(pulse);

    // Output the requested high pulse.
    pwm->setPWM(n, 0, pulse);
}

void loop()
{
    // Drive all channels with a 0.5 ms pulse.
    for (int i = 0; i < 16; i++) {
        setServoPulse(i, 0.5);
    }
    M5.Lcd.fillRect(0, 70, 320, 20, TFT_BLACK);
    M5.Lcd.drawCenterString("Pulse width: 0.5 ms", 160, 80);
    delay(1000);

    // Drive all channels with a 2.5 ms pulse.
    for (int i = 0; i < 16; i++) {
        setServoPulse(i, 2.5);
    }
    M5.Lcd.fillRect(0, 70, 320, 20, TFT_BLACK);
    M5.Lcd.drawCenterString("Pulse width: 2.5 ms", 160, 80);
    delay(1000);
}