#include <Servo.h> 

#define SERVOPINH  5 //Horizontal server
#define SERVOPINV  6 //Vertical server

#define dtime   50  //Delay parameter, the smaller the value, the faster the corresponding speed, and vice versa, the corresponding slower. The unit is millisecond. General value (10~100)
#define tol   50     
/*Corresponding range of illuminance, the smaller the more sensitive, and vice versa (the value is 10~100, the sensitivity is different according to the different intensity of the ambient light, the indoor light source varies greatly, but the change under the sun is small)
  The above 2 parameters are too small and will be extremely sensitive to subtle changes in light, which will cause jitter.
   To eliminate jitter, you can use filter processing or adjust parameters to slow down the response time or sensitivity. */
// Horizontal server Settings
Servo horizontal; // Horizontal server
int servoh = 90;   // Default angle

int servohLimitHigh = 175;  //Left and right angle
int servohLimitLow = 5;     //Left and right angle

// Vertical server settings
Servo vertical;     // Vertical server
int servov = 90;     // Default angle

int servovLimitHigh = 100;  //
int servovLimitLow = 20;    //The maximum elevation angle is not easy to be too large, the sensor may withstand the rack

//Wiring ports for 4 sensors  
const int ldrlt = A0; //Upper left
const int ldrrt = A1; //Upper right
const int ldrld = A2; //Lower left
const int ldrrd = A3; //Lower right

void setup()
{
  Serial.begin(9600);
  horizontal.attach(SERVOPINH); 
  vertical.attach(SERVOPINV);
  horizontal.write(servoh);
  vertical.write(servov);
  delay(100);
  //Test operation
  //Test the operation of the vertical axis, pay attention to check whether there is a jam (or wire winding).
  // for(int i=servovLimitLow;i<servovLimitHigh;i+=2)
  // {  vertical.write(i);
  //    delay(30);
  // }
  // //vertical.write((servovLimitLow + servovLimitHigh)/2);
  // delay(100);
  // //Test horizontal
  // for(int i=servohLimitLow;i<servohLimitHigh;i+=2)
  //  {  horizontal.write(i);
  //    delay(30);
  //  }
  //  //horizontal.write((servohLimitHigh + servohLimitLow)/2);
   //If there is no problem with the test, you can remove the code here. 

}

void loop() 
{
  int lt = analogRead(ldrlt); //up_l
  int rt = analogRead(ldrrt); //up_r
  int ld = analogRead(ldrld); //low_l
  int rd = analogRead(ldrrd); //low_r Read the illuminance value of 4 sensors separately
  
  int avt = (lt + rt) / 2; // up_l_r
  int avd = (ld + rd) / 2; // low_l_r
  int avl = (lt + ld) / 2; // left_up_low
  int avr = (rt + rd) / 2; // right_up_low Average the adjacent readings
  
  int dvert = avt - avd; // vertical-up-low
  int dhoriz = avl - avr;// horizontal-left-right Then calculate the average value of the upper and lower rows and the average value of the left and right rows

 Serial.print(lt);
 Serial.print(",");
 Serial.print(rt);
 Serial.print(",");
 Serial.print(ld);
 Serial.print(",");
 Serial.print(rd);
 Serial.print ("      |    ");    
 
 Serial.print(avt);
 Serial.print(",");
 Serial.print(avd);
 Serial.print(",");
 Serial.print(avl);
 Serial.print(",");
 Serial.print(avr);
 Serial.print(",   ");
 Serial.print(dtime);
 Serial.print(",   ");
 Serial.println(tol);    //The serial port outputs the reading value. After the debugging is normal, you can delete this code to improve the corresponding speed.
  
  
//Check if the difference is within tolerance, otherwise change the vertical Angle  
  if (-1*tol > dvert || dvert > tol) 
 {
  if (avt < avd)
  {
    servov = ++servov;
     if (servov > servovLimitHigh) 
     { 
      servov = servovLimitHigh;
     }
  }
  else if (avt > avd)
  {
    servov= --servov;
    if (servov < servovLimitLow)
  {
    servov = servovLimitLow;
  }
  }
  vertical.write(servov); //If the rotation angle of the servo is opposite to the light, use (180- servov) or (servov) to change the direction.
  }

  //Check whether the difference is within the tolerance range, otherwise change the horizontal angle 
  if (-1*tol > dhoriz || dhoriz > tol) 
  {
  if (avl < avr)
  {
    servoh = --servoh;
    if (servoh < servohLimitLow)
    {
    servoh = servohLimitLow;
    }
  }
  else if (avl > avr)
  {
    servoh = ++servoh;
     if (servoh > servohLimitHigh)
     {
     servoh = servohLimitHigh;
     }
  }
  else if (avl = avr)
  {
    // nothing
  }
  horizontal.write(servoh); //Steering gear rotation Angle and the light of the opposite with (180-servOH) or (servOH) can be reversed
  }
   delay(dtime);

}
