#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "ESP32Servo.h"


Servo servo1;
Servo servo2;
int durasi = 5000;
unsigned long mulai;
int servo1pin = 23;
int servo2pin = 22;

TaskHandle_t xTask1Handle = NULL;
TaskHandle_t xTask2Handle = NULL;
void vTask1(void *arg);
void vTask2(void *arg);

void vTask1(void *arg){

  int pos1;
  for(;;){
    while(millis()-mulai < mulai+durasi){
      pos1 = map(millis() - mulai, mulai, mulai+durasi, 10, 150);
      servo1.write(pos1);
      vTaskDelay(20/portTICK_PERIOD_MS);
    }
  }
  servo1.detach();
  vTaskDelete(NULL);
}

void vTask2(void *arg){

  int pos2;
  for(;;){
    while(millis()-mulai < mulai+durasi){
      pos2 = map(millis() - mulai, mulai, mulai+durasi, 100, 50);
      servo2.write(pos2);
      vTaskDelay(20/portTICK_PERIOD_MS);
    }
  }
  servo2.detach();
  vTaskDelete(NULL);
}



void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  servo1.setPeriodHertz(100);
  servo2.setPeriodHertz(100);
  servo1.attach(servo1pin);
  servo1.write(10);
  servo2.attach(servo2pin);
  servo2.write(100);


  mulai = millis();
  Serial.println(mulai);
  xTaskCreate(vTask1, "servo1", 2048, NULL, 1, NULL);
	xTaskCreate(vTask2, "servo2", 2048, NULL, 1, NULL);
}

void loop() {
 
  // put your main code here, to run repeatedly:
  delay(1); // this speeds up the simulation
}