ard

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

//Motor Parameter
int pwm = 3;         // 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=01;   //proportional parameter
double ki=0.0001;   //integral parameter
double kd=1;   //derivative parameter
unsigned long currentTime, previousTime;

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


// SetPoint or Height
byte distanceP,SetP;
char Pos;

void setup() {
  //Serial Port begin
  Serial.begin (9600);
  //Define inputs and outputs
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

}

void loop() {
  while (Serial.available()>0) {
    Pos =  ((byte)Serial.read());
    SetP = 43 - Pos;
  }

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

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