#define echoPin	6 // attach pin D2 Arduino to pin Echo of HC-SR04
#define trigPin	7 // attach pin D3 Arduino to pin Trig of HC-SR04										 

float a = -10,
      b = -5,
      c = 0,
      d = 5,
	    e = 10,
      setpoint = 3,
      x = 0,
      x_old = 0;

float uNB, uNS, uM, uPS, uPB;
float uNB1, uNS1, uM1, uPS1, uPB1;

float fNB(float x){
  if (x<=a){
    return 1;
  }
  else if (a<=x && x<=b){
    return (b-x)/(b-a);
  }
  else{
    return 0;
  }
}

float fNS(float x){
  if (x<=a || x>=c){
    return 0;
  }
  else if (a<=x && x<=b){
    return (x-a)/(b-a);
  }
  else if (b<=x && x<=c){
    return (c-x)/(c-b);
  }
  else{
    return 1;
  }
}

float fM(float x){
  if (x<=b || x>=d){
    return 0;
  }
  else if (b<=x && x<=c){
    return (x-b)/(c-b);
  }
  else if (c<=x && x<=d){
    return (d-x)/(d-c);
  }
  else{
    return 1;
  }
}

float fPS(float x){
  if (x<=c || x>=e){
    return 0;
  }
  else if (c<=x && x<=d){
    return (x-c)/(d-c);
  }
  else if (d<=x && x<=e){
    return (e-x)/(e-d);
  }
  else{
    return 1;
  }
}

float fPB(float x){
  if (x<=d){
    return 0;
  }
  else if (d<=x && x<=e){
    return (x-d)/(e-d);
  }
  else{
    return 1;
  }
}

long duration; // Variable to store time taken to the pulse
			// to reach receiver

float distance, jarak; // Variable to store distance calculated using
			// formula

void setup()
{
	pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
	pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT

	// Serial Communication is starting with 9600 of
	// baudrate speed
	Serial.begin(9600);

	// The text to be printed in serial monitor
	Serial.println("Distance measurement using Arduino Uno.");
	delay(500);
}

void loop(){
	digitalWrite(trigPin, LOW);
	delayMicroseconds(2); // wait for 2 ms to avoid
						// collision in serial monitor

	digitalWrite(trigPin,HIGH); // turn on the Trigger to generate pulse
	delayMicroseconds(10); // keep the trigger "ON" for 10 ms to generate
			// pulse for 10 ms.

	digitalWrite(trigPin,LOW); // Turn off the pulse trigger to stop
					// pulse generation

	// If pulse reached the receiver echoPin
	// become high Then pulseIn() returns the
	// time taken by the pulse to reach the
	// receiver

	duration = pulseIn(echoPin, HIGH);
	distance = duration * 0.0344 / 2; // Expression to calculate
								// distance using time
  x = setpoint - distance; //error now
  if (x != x_old){
  //fuzzyfikasi x
    uNB = fNB(x);
    uNS = fNS(x);
    uM = fM(x);
    uPS = fPS(x);
    uPB = fPB(x);
    //fuzzyfikasi x_old
    uNB1 = fNB(x_old);
    uNS1 = fNS(x_old);
    uM1 = fM(x_old);
    uPS1 = fPS(x_old);
    uPB1 = fPB(x_old);
    x_old = x;
  }
  Serial.println("Distance: now | old ");
  Serial.println("uNB =" + String(uNB) + "|" + String(uNB1));
  Serial.println("uNS =" + String(uNS)+ "|" + String(uNS1));
  Serial.println("uM =" + String(uM)+ "|" + String(uM1));
  Serial.println("uPS =" + String(uPS)+ "|" + String(uPS1));
  Serial.println("uPB =" + String(uPB)+ "|" + String(uPB1));
  //Serial.println(error);
   
	delay(100);
}