M5core 12 channel Server Module not work.

[TheAlphaGhost]

Hi Gents,

i have a Problem with my Servo module. I buy the M5Core 12 channel Servo modul, and connect my servo on Port 0 from the Modul, but the Servo not work.

I have a running example for the Arduino, which works well with the Servo. I think the problem is my logic to move the code from Arduino Uno to M5Core 12 Channel Servo modul.

That is working with Arduino Uno:

#include <SoftwareSerial.h>

SoftwareSerial mySerial(10, 11); // RX, TX

void setup()
{
  Serial.begin(9600);
  mySerial.begin(115200);
  Serial.println("start");
}

void servo(char ServoId, char Mode, char Degrees, char Speed)
{

  // Build checksum;
  int sum = ServoId + Mode + Degrees + Speed;

  char zero = 0;
  while (sum > 255)
  {
    sum -= 255;
  }

  //Head (Preamble)
  mySerial.write(0xFA); // Byte 1: Preamble;
  mySerial.write(0xAF); // Byte 2: Preamble;

  //Data
  mySerial.write(ServoId); // Byte 3: ServoID (See label of servo)
  mySerial.write(Mode);    // Byte 4: Command; for “go to command” we have = 0x01; we will try to find some other commands shortly
  mySerial.write(Degrees); // Byte 5: position = 120 + desired position in degrees;

  mySerial.write(Speed); // Byte 6: duration = duration in ms / 20
  mySerial.write(zero);  // Byte 7: 0x00 (? we don’t know what this is yet)
  mySerial.write(zero);  // Byte 8: 0x01 (? we don’t know what this is yet)

  //Check & End
  mySerial.write(sum);  // checksum = sum(byte 3 through 8) mod 256
  mySerial.write(0xED); // closure = 0xED (from “END”?)
}

void loop()
{
  servo(16, 1, 45, 40);
  delay(2000);

  servo(16, 1, 0, 10);
  delay(2000);
}

And here i try move my code to M5Stack with 12 channel Servo Modul, which are not work on Port 0. Have any a idea ?

#include <Arduino.h>
#include <M5Stack.h>
#include <Wire.h>

#define SERVO_ADDR 0x53
void setup()
{
    M5.begin(true, false, true);
    M5.Power.begin();
    M5.Lcd.setTextFont(4);
    M5.Lcd.setCursor(70, 100);
    M5.Lcd.print("Servo Example");

    Wire.begin(21, 22, 115200);
}

void servo(char ServoId, char Mode, char Degrees, char Speed)
{

    Wire.beginTransmission(SERVO_ADDR);
    Wire.write(0x00);

    // Build checksum;
    int sum = ServoId + Mode + Degrees + Speed;

    char zero = 0;
    while (sum > 255)
    {
        sum -= 255;
    }

    //Head (Preamble)
    Wire.write(0xFA); // Byte 1: Preamble;
    Wire.write(0xAF); // Byte 2: Preamble;
    Wire.write(ServoId); // Byte 3: ServoID (See label of servo)
    Wire.write(Mode);    // Byte 4: Command; for “go to command” we have = 0x01; we will try to find some other commands shortly
    Wire.write(Degrees); // Byte 5: position = 120 + desired position in degrees;
    Wire.write(Speed); // Byte 6: duration = duration in ms / 20
    Wire.write(zero);  // Byte 7: 0x00 (? we don’t know what this is yet)
    Wire.write(zero);  // Byte 8: 0x01 (? we don’t know what this is yet)
    Wire.write(sum);  // checksum = sum(byte 3 through 8) mod 256
    Wire.write(0xED); // closure = 0xED (from “END”?)


    Wire.endTransmission();
}

void loop()
{
    servo(16, 1, 45, 40);
    delay(2000);

    servo(16, 1, 0, 10);
    delay(2000);
}

Can anyone help me, please... ?