M5-ProductExampleCodes/Hat/8servos-hat/Arduino/8SERVOS/IIC_servo.cpp

#include "IIC_servo.h"

void IIC_Servo_Init()  // sda  0     scl  26
{
    Wire.begin(0, 26, 100000UL);
}

void IIC_Write_1bytes(uint8_t address, uint8_t Register_address, uint8_t data) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    Wire.write(data);
    Wire.endTransmission();
}

void IIC_Write_2bytes(uint8_t address, uint8_t Register_address,
                      uint16_t data) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    Wire.write(data >> 8);    // MSB
    Wire.write(data & 0xFF);  // LSB

    Wire.endTransmission();
}

uint8_t readBytes(uint8_t address, uint8_t subAddress, uint8_t count,
                  uint8_t* dest) {
    Wire.beginTransmission(address);  // Initialize the Tx buffer
    Wire.write(subAddress);           // Put slave register address in Tx buffer
    uint8_t i = 0;
    uint8_t temp;
    if (Wire.endTransmission(false) == 0 &&
        Wire.requestFrom(address, (uint8_t)count)) {
        while (Wire.available()) {
            // temp=Wire.read();
            // dest[i++] =temp ;// Put read results in the Rx buffer
            // M5.Lcd.println(temp,HEX);
            dest[i++] = Wire.read();
        }
        return true;
    }
    return false;
}

/*******************************************************************************/

uint8_t Servo_angle_set(uint8_t Servo_CH, uint8_t angle) {
    uint8_t Register_address = Servo_CH - 1;
    if (Register_address > 7) return 1;
    IIC_Write_1bytes(SERVO_ADDRESS, Register_address, angle);
    return 0;
}

uint8_t Servo_pulse_set(uint8_t Servo_CH, uint16_t width)  // 0x10        ->16
{
    uint8_t servo_ch         = Servo_CH - 1;
    uint8_t Register_address = 2 * servo_ch + 16;
    if (Register_address % 2 == 1 || Register_address > 32) return 1;
    IIC_Write_2bytes(SERVO_ADDRESS, Register_address, width);
    return 0;
}

uint8_t RGB_set(uint8_t R, uint8_t G, uint8_t B) {
    Wire.beginTransmission(SERVO_ADDRESS);
    Wire.write(32);
    Wire.write(G);
    Wire.write(R);
    Wire.write(B);
    Wire.endTransmission();
    return 0;
}

uint8_t Servo_angle_read(uint8_t Servo_CH) {
    uint8_t data             = 0;
    uint8_t Register_address = Servo_CH - 1;
    readBytes(SERVO_ADDRESS, Register_address, 1, &data);
    return data;
}

uint16_t Servo_pulse_read(uint8_t Servo_CH) {
    uint8_t data[2];
    uint8_t servo_ch         = Servo_CH - 1;
    uint8_t Register_address = 2 * servo_ch + 16;
    readBytes(SERVO_ADDRESS, Register_address, 2, data);
    return (data[0] << 8) + data[1];
}

uint32_t RGB_read() {
    uint8_t data[5];
    readBytes(SERVO_ADDRESS, 32, 3, data);
    return (data[0] << 16) + (data[1] << 8) + data[2];
}