#include <VarSpeedServo.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Adafruit_TCS34725.h>

RF24           radio(9, 10);
int            data[4];
int            arr[4];
/*
  VarSpeedServo myservo0;
  VarSpeedServo myservo1;
  VarSpeedServo myservo2;
  VarSpeedServo myservo3;
*/
//--------- Magic --------------
VarSpeedServo myServoArr[4];
//------------------------------
int pos0=90, pos1=90, pos2=90, pos3=50;
int col4=0;

#define S0 4
#define S1 5
#define S2 6
#define S3 7
#define sensorOut 8

#define Speed  100

int redFrequency = 0;
int greenFrequency = 0;
int blueFrequency = 0;

int redColor = 0;
int greenColor = 0;
int blueColor = 0;

#define INTERVAL 10*1000
long int prevMillisColor = 0;
#define INTERVAL_SERIAL 500
long int prevMillisSerial = 0;


void setup() {
  //----------- More magic --------------
 // for (int i=0; i<4; i++) {
 //   myServoArr[i].attach(i+3);
    // Тут проверь, чтобы он к корректным пинам подключался
    // а то я запамятовал куда что подключается
  //}
  //---------------------------------
  Serial.begin(9600);
  //Ниже не понятно почему у тебя 2 раза к 3 пину подключается
  myServoArr[0].attach(3);
  myServoArr[1].attach(A3);
  myServoArr[2].attach(A4);
  myServoArr[3].attach(A5);


  radio.begin();
  radio.setChannel(5);
  radio.setDataRate     (RF24_1MBPS);
  radio.setPALevel      (RF24_PA_HIGH);
  radio.openReadingPipe (1, 0x1234567890LL);
  radio.startListening  ();

  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);

  pinMode(sensorOut, INPUT);

  // Установка масштабирования частоты на 20%
  digitalWrite(S0,HIGH);
  digitalWrite(S1,LOW);
}

void loop() {

  //Процедура опроса датчика, выполняющаяся раз в INTERVAL миллисекунд
  if (millis()-prevMillisColor>INTERVAL) {

    digitalWrite(S2,LOW);
    digitalWrite(S3,LOW);

    redFrequency = pulseIn(sensorOut, LOW);
    redColor = map(redFrequency, 70, 120, 255,0);
    delay(30);

    digitalWrite(S2,HIGH);
    digitalWrite(S3,HIGH);

    greenFrequency = pulseIn(sensorOut, LOW);
    greenColor = map(greenFrequency, 100, 200, 255, 0);
    delay(30);

    digitalWrite(S2,LOW);
    digitalWrite(S3,HIGH);

    blueFrequency = pulseIn(sensorOut, LOW);
    blueColor = map(blueFrequency, 60, 100, 255, 0);
    delay(30);

    if(redColor > greenColor && redColor > blueColor){
        Serial.println(" - RED detected!");
        col4 = 0;
    }
    if(greenColor > redColor && greenColor > blueColor){
        Serial.println(" - GREEN detected!");
        col4 = 0;
    }
    if(blueColor > redColor && blueColor > greenColor){
        Serial.println(" - BLUE detected!");
        col4 = 100;
    }
    prevMillisColor = millis();
  }



if (radio.available() > 0) {
    radio.read(&data, sizeof(data));

    arr[0] = pos0;
    arr[1] = pos1;
    arr[2] = pos2;
    arr[3] = pos3;

    if(millis()-prevMillisSerial>INTERVAL_SERIAL){
      Serial.print("X1: ");
      Serial.println(data[0]);
      Serial.print("Y1: ");
      Serial.println(data[1]);
      Serial.print("X2: ");
      Serial.println(data[2]);
      Serial.print("Y2: ");
      Serial.println(data[3]);
      Serial.println(col4);
      Serial.print("pos0: ");
      Serial.println(arr[0]);
      Serial.print("pos1: ");
      Serial.println(arr[1]);
      Serial.print("pos2: ");
      Serial.println(arr[2]);
      Serial.print("pos3: ");
      Serial.println(arr[3]);
      prevMillisSerial = millis();
    }

    for(int i=0; i<4; i++){
       if (data[i]<300 && arr[i]>0){
           arr[i]--;
           myServoArr[i].slowmove (arr[i], Speed);
    }
      if (data[i]>600 && arr[i]<180){
          arr[i]++;
          myServoArr[i].slowmove (arr[i], Speed);
    }
    //delete [] myservo; //Ето вот в твоём случае делат не надо, можешь удалять
    }
    /*if (data[1]<300 && pos1>0){
      pos1-=2;
      myservo1.slowmove (pos1, Speed);
      //delay(300);
    }
    if (data[1]>600 && pos1<180){
      pos1+=2;
      myservo1.slowmove (pos1, Speed);
      //delay(300);
    }
    if (data[2]<300 && pos2>0){
      pos2-=2;
      myservo2.slowmove (pos2, Speed);
      //delay(300);
    }
    if (data[2]>600 && pos2<180){
      pos2+=2;
      myservo2.slowmove (pos2, Speed);
      //delay(300);
    }
    if (data[3]<300 && pos3>0  ){
      pos3--;
      myservo3.slowmove (pos3, Speed);
      //delay(300);
    }
    if (data[3]>600 && pos3<180 ){
      pos3++;
      myservo3.slowmove (pos3, Speed);
      //delay(300);
    }*/
    //col4=0;
  }

}