AtomMotion + PS4 Gamepad

[Cakedrinker]

Hello,
I'm trying to build a RC car with PS4 Gamepad as controller.
If I connect the servo directly to the AtomLite and Pin25, it is working like a charm, with the following example:

#include <ESP32Servo.h> 
#include <PS4Controller.h>

Servo myservo;
int servoPos = 0;
int servoPin = 25;

void setup(){
   ESP32PWM::allocateTimer(0);
   ESP32PWM::allocateTimer(1);
   ESP32PWM::allocateTimer(2);
   ESP32PWM::allocateTimer(3);
   myservo.setPeriodHertz(50);    // standard 50 hz servo
   myservo.attach(servoPin, 500, 2400);

  PS4.begin("4C:75:25:AD:78:7A");
  Serial.println("Ready.");
}

void loop() {

if (PS4.isConnected()) {
  servoPos =  (PS4.R2Value()) / 2;
  }

 myservo.write(servoPos);
 delay(100);
}

but if I change to MotionBase it is very laggy.

#include <M5Atom.h> 
#include "AtomMotion.h"
#include <PS4Controller.h>

AtomMotion Atom;
int servoPos;

void setup(){ 
  M5.begin(true, false, true);
  Atom.Init();

  PS4.begin("4C:75:25:AD:78:7A");
  Serial.println("Ready.");
}

void loop() {

     if (PS4.isConnected()) {
        servoPos =  (PS4.R2Value()) / 2;
     }
 
     Atom.SetServoAngle(1,servoPos);
     delay(100);
}

What I'm doing wrong?

[Cakedrinker]

I played a little bit with the sorce code and tried to write the register of the STM32 direclty over I2C.

#include <Wire.h>
#include <PS4Controller.h>

#define SERVO_ADDRESS  0X38

int servoPos;

void setup() {
  Wire.begin (25, 21);
  PS4.begin("4C:75:25:AD:78:7A");
  Serial.println("Ready.");
}

uint8_t SetServoAngle(uint8_t angle)
{
  Write1Byte(SERVO_ADDRESS,0,angle);
  return 0;
}

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

void loop() {
  if (PS4.isConnected()) {
     servoPos =  (PS4.R2Value()) / 2;
  }
  SetServoAngle(servoPos);
  delay(100);
}

But the problems still exists, so maybe the problem is the FW of the STM32F030F4.

[Cakedrinker]

My board was defect, I changed the MotionBase , now it is working perfectly.