Easy Light Tracking Device

/**
   Author: Kevin Chen AKA Stonez56 
   My Blog to see more tutorials: https://stonez56.blogspot.com
   YouTube channel:https://www.youtube.com/channel/UCLdNauvVOrss3fF8LJXrtAw
 
    Program function flow
   1. Reading four light sensor values
   2. Average top two(Top left, Top right), bottom two(Bottom left, Bottom right)
   3. Average left two(Top left, Bottom left), right two (Top right, Bottom right)
   4. Compare 4 averaged value and make servo mave toward the biggest number side
   
   v0 Light traking Gimbal base to get readings
   v1 Write servo code in and move servo accordingly
 
                    |
         Top left   |     Top right
   -----------------+----------------------
    　Bottom left   |  Bottom right
                    |
*/
#include <Servo.h>
Servo servo1_x;
Servo servo2_y;
 
//Light Sensor Pin definitions
const uint8_t  light_top_left_PIN = A6;
const uint8_t  light_bottom_left_PIN = A5;
const uint8_t  light_top_rigth_PIN = A2;
const uint8_t  light_bottom_right_PIN = A1;
//Potentiometer pin
const  int potPIN = A0;
//Servo pins
const int servo1_x_pin = 2;
const int servo2_y_pin = 4;
 
 
//User define variables
// Gimbal movement tolerance 50~255
byte gimbal_movement_tolerance = 100;
//Photo sensor max reading times for average (for more accuracy)
byte max_reading = 10;
// define original servo angle
uint8_t originalAngle = 92;
uint8_t x_last = originalAngle;  // X last postion
uint8_t y_last = originalAngle; // Y last postion
uint8_t moveSpeed = 10;  //How fast show this Servo move
uint8_t maxSpeed = 50; //Max speed
uint8_t minSpeed = 1;
uint8_t y_minAngle = 1;  //Mimum angle
uint8_t y_maxAngle = 180;  //Maximum angle
uint8_t x_minAngle = 90;  //Mimum angle
uint8_t x_maxAngle = 180;  //Maximum angle
 
void setup() {
  Serial.begin(57600);
 
  pinMode(light_top_left_PIN, INPUT);
  pinMode(light_bottom_left_PIN, INPUT);
  pinMode(light_top_rigth_PIN, INPUT);
  pinMode(light_bottom_right_PIN, INPUT);
 
  servo1_x.attach(servo1_x_pin);
  servo1_x.write(originalAngle); //move servo to defined angle
  servo2_y.attach(servo2_y_pin);
  servo2_y.write(originalAngle); //move servo to defined angle
 
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
 
}
 
void loop() {
  char moveTowards = ' ';
 
 
  int potValue = analogRead(potPIN);
  Serial.print(F("potValue: "));
  Serial.println(potValue);
  moveSpeed = map(potValue, 0, 500, minSpeed, maxSpeed);
  Serial.print(F("moveSpeed: "));
  Serial.println(moveSpeed);
  moveTowards = getFacingToward(); //find out which side to move
 
 
//  Serial.print(F("moveTowards: "));
//  Serial.println(moveTowards);
  moveServo(moveTowards);
 
  delay(80);
}
 
void moveServo(char moveTowards) {
 
 
  //previous position were stored in x_last, y_last
  switch (moveTowards) {
    case 'T': //Move towards top
      if (x_last - moveSpeed < x_minAngle) {
        x_last = x_minAngle;
        servo1_x.write(x_last);
      } else {
        x_last -= moveSpeed;
        servo1_x.write(x_last);
      }
      break;
    case 'B': //Move towards bottom
      if (x_last + moveSpeed > x_maxAngle) {
        x_last = x_maxAngle;
        servo1_x.write(x_last);
      } else {
        x_last += moveSpeed;
        servo1_x.write(x_last);
      }
      break;
    case 'L': //Move towards left
      if (y_last - moveSpeed < y_minAngle) {
        y_last = y_minAngle;
        servo2_y.write(y_last);
      } else {
        y_last -= moveSpeed;
        servo2_y.write(y_last);
      }
      break;
    case 'R':
      if (y_last + moveSpeed > y_maxAngle) {
        y_last = y_maxAngle;
        servo2_y.write(y_last);
      } else {
        y_last += moveSpeed;
        servo2_y.write(y_last);
      }
      break;
    default:
      //Don't move servo
      break;
  }
 
}
 
/**
   This fuction get reading max_reading times and return average
   if it sees an 0, it will skip it
*/
int averageReading(int PIN, byte max_reading) {
  int total = 0;
  int current = 0;
 
  for (byte i = 0; i <= max_reading; i++) {
    current = analogRead(PIN);
    if (current == 0) {
      current = analogRead(PIN);
    }
    total += current;
  }
 
  return total / (max_reading);
}
 
/**
 
 
   Parameters: N/A
   Return: char; T, B, R, L to indicate the moving directoin
                       '-' means not moving at all
*/
char getFacingToward() {
  //1. Read each pin max_reading times
 
  int top_left = averageReading(light_top_left_PIN, max_reading);
  int bottom_left = averageReading(light_bottom_left_PIN, max_reading);
  int top_right = averageReading(light_top_rigth_PIN, max_reading);
  int bottom_right = averageReading(light_bottom_right_PIN, max_reading * 3);
 
  //Show photo sensor readings...
  Serial.print(F("Top left-A6: "));
  Serial.println(top_left);
  Serial.print(F("Bottom left-A5: "));
  Serial.println(bottom_left);
  Serial.print(F("Top right-A2: "));
  Serial.println(top_right);
  Serial.print(F("Bottom rightA1: "));
  Serial.println(bottom_right);
 
  //2. Get max value sides(two averaged, see above)
  byte go_direction[4] = {0, 0, 0, 0};
  int toward[4] = {0, 0, 0, 0}; //Top, Bottom, Left, Right
  toward[0] = (top_left + top_right) / 2 ;
  toward[1] = (bottom_left + bottom_right) / 2 ;
  toward[2] = (top_left + bottom_left) / 2;
  toward[3] = (top_right + bottom_right) / 2;
 
  //3. Add all side and average,
  //    if each side is within the tolerance then don't move Gimbal
  //    average = toward[0] +...toward[3]
  //    average - toward[0] .... toward[3], if all within gimbal_movement_tolerance, then no move
  int total_toward = 0;
  total_toward += toward[0];
  total_toward += toward[1];
  total_toward += toward[2];
  total_toward += toward[3];
  //get average
  int total_average = total_toward / 4; //4 sides
  Serial.print(F("total_average:"));
  Serial.println(total_average);
  //    if each side is within the tolerance then don't move Gimbal
  boolean shouldMove = false;
  //else move the gimbal
  if (total_average - toward[0] > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[1] > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[2] > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[3] > gimbal_movement_tolerance) shouldMove = true;
 
  Serial.print(F("toward 0 TOP : "));
  Serial.println(toward[0]);
  Serial.print(F("toward 1 BOTTOM: "));
  Serial.println(toward[1]);
  Serial.print(F("toward 2 LEFT: "));
  Serial.println(toward[2]);
  Serial.print(F("toward 3 RIGHT: "));
  Serial.println(toward[3]);
 
  //Find the biggest number to decide which side to go,
  // but if four values are quite similar, send '-' back to indicate not moving
  char facing = ' ' ;
  if (shouldMove) {
    int max_ = 0;
    if (toward[0] > max_) {
      max_ = toward[0];
      facing = 'T';
    }
    if (toward[1] > max_) {
      max_ = toward[1];
      facing = 'B';
    }
    if (toward[2] > max_) {
      max_ = toward[2];
      facing = 'L';
    }
    if (toward[3] > max_) {
      max_ = toward[3];
      facing = 'R';
    }
  } else {
    facing = '-'; //no need to move
  }
  //  Serial.print(F("shouldMove: "));
  //  Serial.println(shouldMove);
  return facing;
}