balancer code

//PID constants
double kp = 8;
double ki = 1;
double kd = 6;

unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double input, output;
const double setPoint = 45;
double cumError, rateError;

int big = 170;
int small = 85;

double KP;
double KI;
double KD;

#include <Servo.h>

Servo myservo; 



int trigPin = 9;    // TRIG pin
int echoPin = 8;    // ECHO pin

float duration_us, distance_cm;

void setup() {
  // begin serial port
  Serial.begin(9600);

  // configure the trigger pin to output mode
  pinMode(trigPin, OUTPUT);
  // configure the echo pin to input mode
  pinMode(echoPin, INPUT);

myservo.attach(10);




}

void loop() {
 // generate 10-microsecond pulse to TRIG pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(echoPin, HIGH);

  // calculate the distance
  distance_cm = 0.017 * duration_us;

  // print the value to Serial Monitor

  




 input = distance_cm;            //read from rotary encoder connected to A0
 output = computePID(input);
 delay(10);
 

if ( output >= 85 && output <= 170){
  myservo.write(output);
}
else if(output < 85){
  myservo.write(small);
}
else if(output > 170){
  myservo.write(big);
}

//Serial.println(output);
Serial.print(setPoint);
Serial.print(",");
Serial.print(KP);
Serial.print(",");
Serial.print(KI);
Serial.print(",");
Serial.println(KD);



}






double computePID(double inp){     
       
        currentTime = millis();                //get current time
        elapsedTime = (double)(currentTime - previousTime);        //compute time elapsed from previous computation
        
        error = setPoint - inp;                                // determine error
        cumError += error * elapsedTime;                // compute integral
        rateError = (error - lastError)/elapsedTime;   // compute derivative


KP = kp * error;
KI = ki * cumError;
KD = kd * rateError;




        double out = KP+KI+KD;                //PID output               

        lastError = error;                                //remember current error
        previousTime = currentTime;                        //remember current time

        return out;                                        //have function return the PID output
}