//Ultrasonic Parameter
int trigPin = 11;    // Trigger
int echoPin = 12;    // Echo
long duration, currentPosition;

//Motor Parameter
int fan1 = 2;  //  IN1
int fan2 = 4;   //  IN2
int pwm = 0;         // PWM Port

//PID parameters.
//Guide : https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
double kp=0;   //proportional parameter
double ki=0;   //integral parameter
double kd=0;   //derivative parameter
unsigned long currentTime, previousTime;

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


// SetPoint or Height
double SetP;


void setup() {

  SetP = 15;
  //Serial Port begin
  Serial.begin (9600);
  //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);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // 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
  output = computePID(currentPosition);
  delay(100);
  long result = map ( output, 0, 255, 169, 220);
  analogWrite(pwm, result);
  Serial.println(String(currentPosition) + "," + String(kp) + "," + String(ki) + "," + String(kd) + "," + String(SetP) + "," + String(result));
  delay(250);
}

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;
}