#include <TimerOne.h>

void setup(){
  pinMode(A0,INPUT);
  Serial.begin(9600);
  for (int i=0;i<8;i++){
    pinMode(i,OUTPUT);}

  Timer1.initialize(200000); // microseconds (or 2 sec - or 0.5Hz)
  Timer1.attachInterrupt(timerIsr); // attach the service routine here
}

void loop(){
  
}

void timerIsr(){
  int r = analogRead(A0);
  int y = analogRead(A1);
  int error = r-y;   // Error
  int kp = 20; // Ganancia
  int u = error * kp; // Salida
  u= constrain(u, 0, kp*r);
  u = map(u,0,(kp*r),0,255); // Escalar salida a 8 bits 
  PORTD = u;
  Serial.print("\tref: ");
  Serial.print(r);
  Serial.print("\ty: ");
  Serial.print(y);
  Serial.print("\tError: ");
  Serial.print(error);
  Serial.print("\tu: ");
  Serial.print(u);
  Serial.print("\n");
}