#include <Servo.h>
Servo myservo;
#include <AutoPID.h>
#define OUTPUT_MIN -255
#define OUTPUT_MAX 255
#define KP 2.8
#define KI 0
#define KD 0
#define Rpwm 10
#define Lpwm 9
double jarak, setPoint, outputVal;
AutoPID myPID(&jarak, &setPoint, &outputVal, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);
int trigPin = 13;    // Trigger
int echoPin = 12;    // Echo
long duration;
double cm;


void kontrol_motor(double pid){
  if(pid > 0){
    analogWrite(Lpwm,0);
    analogWrite(Rpwm,pid);
  }
  if(pid < 0){
    analogWrite(Rpwm,0);
    analogWrite(Lpwm,pid);
  }
}

void ultra(){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  cm = (duration/2) / 29.1;
  jarak = cm;
  delay(250);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  myservo.attach(3);
  pinMode(trigPin, OUTPUT); //  set pin trigger sebagai output
  pinMode(Rpwm, OUTPUT); //  set pin trigger sebagai output
  pinMode(Lpwm, OUTPUT); //  set pin trigger sebagai output
  pinMode(echoPin, INPUT); // set pin echo sebagai input
  //myPID.setBangBang(97);
  myPID.setTimeStep(250);
}

void loop() {
  // put your main code here, to run repeatedly:
  ultra();
  setPoint = 46.20;
  myPID.run();
  myservo.write(outputVal);
  Serial.println(outputVal);
}