#include <ESP32Servo.h>

// Deklarasi pin
const int servoPin1 = 18;
const int servoPin2 = 5;

// Inisialisasi objek
Servo servo1;
Servo servo2;

// Variabel global
volatile float pos1 = 10.0;
volatile float pos2 = 100.0;
volatile unsigned long lastInterruptTime = 0;
volatile int interruptCounter;

hw_timer_t * timer = NULL;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;

void IRAM_ATTR onTimer() {      //Defining Inerrupt function with IRAM_ATTR for faster access
 portENTER_CRITICAL_ISR(&timerMux);
 interruptCounter++;
 portEXIT_CRITICAL_ISR(&timerMux);
}

void setup() {
  Serial.begin(115200);
  // Inisialisasi motor servo
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  
  // Inisialisasi timer
  timer = timerBegin(0, 80, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, 1000000, true); 
  timerAlarmEnable(timer);
}

void loop() {

    
  // Gerakkan servo1
  if (pos1 <= 150.0) {
    pos1 += 0.14; // Nilai ini dihitung sebagai (150-10)/1000
    servo1.write((int)pos1);
  }
    
   // Gerakkan servo2
  if (pos2 >= 50.0) {
     pos2 -= 0.05; // Nilai ini dihitung sebagai (100-50)/1000
     servo2.write((int)pos2);
  }

  Serial.print("Servo 1:");
  Serial.println(pos1);
  Serial.print("Servo 2:");
  Serial.println(pos2);
  Serial.print("Waktu :");
  Serial.println(millis());
}