Servo2 and Touch screen is not working together

[biokys]

I stacked Servo2 module together with M5CoreS3. This code is working properly with touches, but servo controlling is not working. If i remove line CoreS3.update() than servo controlling starts to work, but obviously, touch screen is not working. (I did change the I2C address of Servo2 module to 0x41 )

Anyone has the same issue?

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


#define SERVO_ADDR 0x41


Adafruit_PWMServoDriver pwm;

static m5::touch_state_t prev_state;

void setServoPulse(uint8_t n, double pulse) {
   double pulselength;
   pulselength = 1000000;   // 1,000,000 us per second
   pulselength /= 50;   // 50 Hz
   Serial.print(pulselength); Serial.println(" us per period"); 
   pulselength /= 4096;  // 12 bits of resolution
   Serial.print(pulselength); Serial.println(" us per bit"); 
   pulse *= 1000;
   pulse /= pulselength;
   Serial.println(pulse);
   pwm.setPWM(n, 0, pulse);
}

void servo_angle_write(uint8_t n, int Angle) {
  double pulse = Angle;
  pulse = pulse/90 + 0.5;
  setServoPulse(n, pulse);
}

void setup() {

  
    auto cfg = M5.config();
    CoreS3.begin(cfg);
    CoreS3.Display.setTextColor(RED);
    CoreS3.Display.setTextDatum(middle_center);
    CoreS3.Display.setFont(&fonts::Orbitron_Light_24);
    CoreS3.Display.setTextSize(1);

    CoreS3.Display.drawString("Touch Button Test", CoreS3.Display.width() / 2,
                              15);
    CoreS3.Display.fillRect(0, CoreS3.Display.height() - 40,
                            CoreS3.Display.width() / 3, 40, WHITE);
    CoreS3.Display.fillRect(CoreS3.Display.width() / 3,
                            CoreS3.Display.height() - 40,
                            CoreS3.Display.width() / 3, 40, GREEN);
    CoreS3.Display.fillRect((CoreS3.Display.width() / 3) * 2,
                            CoreS3.Display.height() - 40,
                            CoreS3.Display.width() / 3, 40, YELLOW);
    CoreS3.Display.drawString("Btn A    Btn B    Btn C",
                              CoreS3.Display.width() / 2,
                              CoreS3.Display.height() - 20);

    
    
    Wire.begin(G12, G11, 100000UL);
    pwm = Adafruit_PWMServoDriver(SERVO_ADDR);
    pwm.begin();
    pwm.setPWMFreq(50); 

    Serial.begin(9600);

    
    delay(1000);
    

}

void loop() {


  CoreS3.update();
    auto touchPoint = CoreS3.Touch.getDetail();
    if (prev_state != touchPoint.state) {
        prev_state = touchPoint.state;
    }
    if ((CoreS3.Display.height() > touchPoint.y &&
         touchPoint.y > CoreS3.Display.height() - 40) &&
        touchPoint.state == m5::touch_state_t::touch_begin) {
        CoreS3.Display.fillRect(0, 40, CoreS3.Display.width(), 70, BLACK);
        if (CoreS3.Display.width() / 3 > touchPoint.x && touchPoint.x > 0) {
            CoreS3.Display.drawString("Btn A", CoreS3.Display.width() / 2,
                                      CoreS3.Display.height() / 2 - 30);
                                      servo_angle_write(5, 90);
        }
        if ((CoreS3.Display.width() / 3) * 2 > touchPoint.x && 
            touchPoint.x > CoreS3.Display.width() / 3) {
            CoreS3.Display.drawString("Btn B", CoreS3.Display.width() / 2,
                                      CoreS3.Display.height() / 2 - 30);
                                      servo_angle_write(5, 0);
            }
        if (CoreS3.Display.width() > touchPoint.x &&
            touchPoint.x > (CoreS3.Display.width() / 3) * 2)
            CoreS3.Display.drawString("Btn C", CoreS3.Display.width() / 2,
                                      CoreS3.Display.height() / 2 - 30);
        Serial.printf("x:%d ,y:%d", touchPoint.x, touchPoint.y);
    }

   servo_angle_write(5, 90);
  delay(500);
  servo_angle_write(5, 0);
  delay(500);
}