#include <Servo.h>
/*

Servo myservo;  // create servo object to control a servo, later attatched to D9

void setup() {
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo.write(125);

}

void loop() {
  // put your main code here, to run repeatedly:

}



	Capitulo 5 de Arduino desde cero en Español
	Primer programa que envía mediante el Monitor Serial el valor de distancia
	leído por el sensor ultrasónico HC-SR04.

	Autor: bitwiseAr  

*/

int TRIG = 11;			// trigger en pin 10
int ECO = 10;			// echo en pin 9
//int LED = ;			// LED en pin 3
int DURACION;
int DISTANCIA;

Servo myservo;  // create servo object to control a servo, later attatched to D9

int Read = 0;
float distance = 0.0;
float elapsedTime, time, timePrev;        //Variables for time control
float distance_previous_error, distance_error;
int period = 50;  //Refresh rate period of the loop is 50ms

float kp=3; //Mine was 8
float ki=0; //Mine was 0.2
float kd=1; //Mine was 3100
float distance_setpoint = 200;           //Should be the distance from sensor to the middle of the bar in mm
float PID_p, PID_i, PID_d, PID_total;


void setup()
{
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo.write(0);
  pinMode(TRIG, OUTPUT); 	// trigger como salida
  pinMode(ECO, INPUT);		// echo como entrada
  pinMode(LED, OUTPUT);		// LED como salida
  Serial.begin(9600);  		// inicializacion de comunicacion serial a 9600 bps
  time = millis();

}

void loop()
{
  
  digitalWrite(TRIG, HIGH); 		// generacion del pulso a enviar
  delay(1);				// al pin conectado al trigger
  digitalWrite(TRIG, LOW);		// del sensor
  
  DURACION = pulseIn(ECO, HIGH);	// con funcion pulseIn se espera un pulso
  					// alto en Echo
  DISTANCIA = DURACION / 58.2;		// distancia medida en centimetros
  Serial.println(DISTANCIA);		// envio de valor de distancia por monitor serial
  // myservo.write(DISTANCIA);
  delay(200);				// demora entre datos

   if (millis() > time+period)
  {
    time = millis();    
    distance = DISTANCIA;   
    distance_error = distance_setpoint - distance;   
    PID_p = kp * distance_error;
    float dist_diference = distance_error - distance_previous_error;     
    PID_d = kd*((distance_error - distance_previous_error)/period);
      
    if(-3 < distance_error && distance_error < 3)
    {
      PID_i = PID_i + (ki * distance_error);
    }
    else
    {
      PID_i = 0;
    }
  
    PID_total = PID_p + PID_i + PID_d;  
    PID_total = map(PID_total, -150, 150, 0, 150);
  
    if(PID_total < 20){PID_total = 20;}
    if(PID_total > 160) {PID_total = 160; } 
  
    myservo.write(PID_total+30);  
    distance_previous_error = distance_error;
  }
   
}