#include <Arduino_FreeRTOS.h>
#include <Servo.h>
#include <semphr.h>

const char *taskName[2] = {"Robot1", "Robot2"};
const uint8_t servoPin[2] = {8, 2};
const int speed[2] = {12, 9};
int angle[2] = {};
bool dir[2] ={};

SemaphoreHandle_t sem;

void setup() {

  sem = xSemaphoreCreateCounting(1, 0);

  for(byte i = 0; i < 2; i++) {
    xTaskCreate(RobotTask, taskName[i],
    configMINIMAL_STACK_SIZE, i, 0, NULL);
  }
}

void RobotTask(int i){
  Servo myServo; 
  myServo.attach(servoPin[i]);

  for(;;){
    if (!dir[i]){
      if(angle[i] <= 110 && angle[i] + speed[i] >110){
        xSemaphoreTake(sem, portMAX_DELAY);
      }
      angle[i] += speed[i];
      if(angle[i] > 180){
        vTaskDelay(pdMS_TO_TICKS(random(1000)));
        dir[i] = 1;
      }
    }
    else{
      if(angle[i] >= 110 && angle[i] - speed[i] < 110){
        xSemaphoreGive(sem);
      }
      angle[i] -= speed[i];
      if(angle[i] < 0){
        vTaskDelay(pdMS_TO_TICKS(random(5000)));
        dir[i] = 0;
    }
  }
  myServo.write(angle[i]);
  vTaskDelay(pdMS_TO_TICKS(100));
 }
  vTaskDelete(NULL);
}

void loop() {
  // put your main code here, to run repeatedly:

}