//Ultrasonic Parameter
int trigPin = 10;    // Trigger
int echoPin = 9;    // Echo
long duration, currentPosition;

//Motor Parameter
int fan1 = 7;  //  IN1
int fan2 = 6;   //  IN2
int pwm = 5;         // PWM Port
//PID parameters.
//Guide 1  : https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
//Guide 2 : https://stackoverflow.com/questions/14614124/how-do-i-use-a-pid-controller
// Set kp,ki,kd to Zero and follow instruction from above link to input value.

double kp=1;   //proportional parameter
double ki=0.0001;   //integral parameter
double kd=0 n;   //derivative parameter
unsigned long currentTime, previousTime;

double elapsedTime;
double error;
double lastError;
double input, error_, output, setPoint;
double cumError, rateError;


// SetPoint or Height
double SetP= 25;


void setup() {

  //Serial Port begin
  Serial.begin (38400);
  //Define inputs and outputs
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(fan1,OUTPUT);
  pinMode(fan2,OUTPUT);
  digitalWrite(fan1, HIGH);
  digitalWrite(fan2, LOW);

}

void loop() {

  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  digitalWrite(trigPin, HIGH);


  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);

  // Convert the time into a distance
  currentPosition = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  error_ = computePID(currentPosition);
  if (error_ < -5){
    error_ = -5;
    }
  output = map(error_, 0,-5 , 187, 192);
//
//  if (output < 0){
//    output = 190;
//      }
//  if (currentPosition <= 25){
//    output = 187;
//    delay(10000);
//      }
  analogWrite(pwm, output);

  Serial.println(String(currentPosition) + "," + String(kp) + "," + String(ki) + "," + String(kd) + "," + String(SetP) + "," + String(output) + "," + String(error_));
}

double computePID(double inp){
  currentTime = millis();                //get current time
  elapsedTime = (double)(currentTime - previousTime);        //compute time elapsed from previous computation

  error = SetP - inp;                                // determine error
  cumError += error * elapsedTime;                // compute integral
  rateError = (error - lastError)/elapsedTime;   // compute derivative
  double out = kp*error + ki*cumError + kd*rateError;                //PID output
  lastError = error;                                //remember current error
  previousTime = currentTime;                        //remember current time
  return out;
}