#include <ESP32Servo.h>

Servo myservo1;
Servo myservo2;

TaskHandle_t Task1, Task2;

void Task1code(void *parameter) {
  int pos1 = 0;
  int servoPin1 = 22;
  unsigned long startTime = millis(); // Waktu awal
  myservo1.setPeriodHertz(50);
  myservo1.attach(servoPin1);
  myservo1.write(30);
  Serial.println("Init pos1: " + String(myservo1.read()));
  while (millis() - startTime < 5000) { // Berhenti setelah 5 detik
    pos1 = map(millis() - startTime, 0, 5000, 30, 180); // Melakukan pemetaan waktu ke posisi servo
    myservo1.write(pos1);
    delayMicroseconds(71428);
  }
  Serial.println("End pos1: " + String(myservo1.read()));
  myservo1.detach(); // Lepas servo setelah selesai
  vTaskDelete(NULL);
}

void Task2code(void *parameter) {
  int pos2 = 0;
  int servoPin2 = 18;
  unsigned long startTime = millis(); // Waktu awal
  myservo2.setPeriodHertz(50);
  myservo2.attach(servoPin2);
  myservo2.write(180);
  Serial.println("Init pos2: " + String(myservo2.read()));
  while (millis() - startTime < 5000) { // Berhenti setelah 5 detik
    pos2 = map(millis() - startTime, 0, 5000, 180, 100); // Melakukan pemetaan waktu ke posisi servo
    myservo2.write(pos2);
    delayMicroseconds(83333);
  }
  Serial.println("End pos2: " + String(myservo2.read()));
  myservo2.detach(); // Lepas servo setelah selesai
  vTaskDelete(NULL);
}

void setup() {
  Serial.begin(115200);
  // Allow allocation of all timers
  xTaskCreatePinnedToCore(
      Task1code, /* Function to implement the task */
      "Task1",   /* Name of the task */
      10000,     /* Stack size in words */
      NULL,      /* Task input parameter */
      1,         /* Priority of the task */
      &Task1,    /* Task handle. */
      0);        /* Core where the task should run */

  xTaskCreatePinnedToCore(
      Task2code, /* Function to implement the task */
      "Task2",   /* Name of the task */
      10000,     /* Stack size in words */
      NULL,      /* Task input parameter */
      1,         /* Priority of the task */
      &Task2,    /* Task handle. */
      1);        /* Core where the task should run */
}

void loop() {
}