#include <Servo.h>
Servo servo1;
Servo servo2;
int trig1 = A0; int trig2 = 2;
int echo1 = A1; int echo2 = 4;
const int trigPin[] = {A0, 2};
const int echoPin[] = {A1, 4};

float kp = 2.0;
float ki = 0.5;
float kd = 1.0;
float maxIntegral = 50.0;
float minIntegral = -50.0;
float previousError = 0;
float sigmaError = 0;
unsigned long previousTime = 0;
int baseSpeed = 90;



void setup() {
  // put your setup code here, to run once:
  for (int i = 0; i < 2; i++) {
    pinMode(trigPin[i], OUTPUT);
    pinMode(echoPin[i], INPUT);
  }
  Serial.begin(9600);
  servo1.attach(3);
  servo2.attach(5);
}
long readSensor(int trig, int echo) {
  int readings = 5;
  long resultRead = 0;
  for (int i = 0; i < readings; i++) {
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    int duration = pulseIn(echo, HIGH, 10000) / 58.475;
    resultRead += (duration != 0 && duration <= 80) ? duration : 80;
  }
  return resultRead / readings;
}



float PID(float setpoint, float measure) {
  float error = setpoint - measure;
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - previousTime) / 1000.0;
  
  if (deltaTime > 0) {
    // Anti-windup: batasi integral term
    sigmaError = constrain(sigmaError + (error * deltaTime), minIntegral, maxIntegral);
    
    float deltaError = (error - previousError) / deltaTime;
    
    // Hitung output PID
    float pTerm = kp * error;
    float iTerm = ki * sigmaError;
    float dTerm = kd * deltaError;
    
    float output = pTerm + iTerm + dTerm;
    
    previousError = error;
    previousTime = currentTime;
    return constrain(output, -90, 90); 
  }
  
  previousTime = currentTime;
  return 0;
}


void loop() {
  // put your main code here, to run repeatedly:
  int s1 = readSensor(trig1, echo1);
  int s2 = readSensor(trig2, echo2);
  float pidOut = PID(s1, s2);
  if (abs(pidOut) < 5) pidOut = 0;
  
  int speedLeft = constrain(baseSpeed + pidOut, 0, 180);
  int speedRight = constrain(baseSpeed - pidOut, 0, 180);
  servo1.write(speedLeft);
  servo2.write(speedRight);
  Serial.print(s1);Serial.print("||");Serial.print(s2);
  Serial.print("||");Serial.println(pidOut);

}