gistfile1.txt

//PID constants
double kp = 1;
double ki = 0;
double kd = 0;

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

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

int val = output;
 // reads the value of the potentiometer (value between 0 and 1023)
juan = map(val, -40, 40, 85, 170); // scale it to use it with the servo (value between 0 and 180)
myservo.write(juan);


Serial.println(juan);

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