// tracker-prog.ino --------------------
// code - use 2 lite detectors & PID controller to run a solar tracker
// ctl variable X = DIFF betw 2 lite detectors
// X sent to PID controller (with deadband), output u
// u is control to "servoAngle", which is filtered (expon), constrained
// & sent to motor

#include <Servo.h>
Servo myservo;
int servoPin = 10;
int sensorPin1 = A1;
int sensorPin2 = A3;


void setup()  // -------------------------------------------
{
  Serial.begin(9600);
  myservo.attach(servoPin);
}

void loop()  // ---------------------------------------------
{
  //int servoAngle;
  int sensor1 = analogRead(sensorPin1);  //0 - 1023
  int sensor2 = analogRead(sensorPin2);
  int x = sensor2 - sensor1;  // x = diff: -1023 to +1023
  float u;
  static float servoAngle;
  float servoAngleFiltered;
  u = computePID(x);
  servoAngle += u;  // add control to servoAngle
  servoAngle = constrain(servoAngle, 70, 110);
  servoAngleFiltered = exponFilterCT(servoAngle);               // filter servoAngle
  servoAngleFiltered = constrain(servoAngleFiltered, 70, 110);  // constrain value
  myservo.write((int)servoAngleFiltered);                       // command motor
  //myservo.write((int)servoAngle); // option: command motor unfiltered
  float x1 = x;  // output to SM to debug
  float x2 = servoAngle;
  Serial.print(x1);
  Serial.print(" ");
  Serial.println(x2);
  delay(100);
}

float computePID(int x)  //---------------------------------
{
  float Kp = 0.01;  //P gain - PLAY WITH THIS
  float Kd = 0.10;  //D gain - PLAY WITH THIS
  float v, u;
  int ramptime = 2000;  // time to ramp u
  float xd = 0.0;
  float vd = 0.0;
  unsigned long t;
  static unsigned long to = 0;
  static float xo = 0;
  int minerror = 20;  // min error for deadband
  t = millis();
  v = (float)(x - xo) / (t - to);  // compute veloc
  xo = x;                          //reset old xo,to for next look V calc
  to = t;
  u = Kp * (xd - x) + Kd * (vd - v);  // PD controller
  // dead band (no u if close to desired)

/*
  if (abs(x - xd) < minerror) {
    u = 0.0;
  }
*/  


  // ramp up u slowly (code) (TEST THIS) ??
  /*
  if (t <= ramptime)
  {
   u = u * t/ramptime; // ramp u for 1st 2 seconds
  }
  */

  return u;
}

int exponFilterCT(int rawData)  //--------------------------------------
{
  static int lastFilteredData;
  float w = 0.05;  // w = % raw data (faster, noisy)
  int y;
  y = w * rawData + (1 - w) * lastFilteredData;
  lastFilteredData = y;
  return y;
}

//end ------------------------