#include <ovr/io.

h>
#include <ovr/interrupt.h>
#include <LiquidCrystal.h>
LiquidCrystal lcd(13,12, 11,10, 9, 8);
float sp,ang,U,en;
float KV=1;//nuev0
int valor,porpwm;
//VARIABLES DE ENTRADA
float xn_1=0;
float xn_2=0;
float xn=0;
// VARIABLES DE SALIDA
float yn_1=0;
float yn_2=0;
float yn=0;
//pines analogos
float potmotor=0;
float setpoint=1;
void setup() {
cli();
TCCR1A=0;
TCCR1B=0;
OCR1A=124;
TCCR1B |=(1<<WGM12);
TCCR1B |=(1<<CS12);
TCCR1B |=(1<<CS10);
TIMSK1 |=(1<<OCIE1A);
sei();
// set up the LCD's number of columns and rows:
lcd.begin(16, 2);
// MENSAJE A IMPRIMI EN LA LCD.
pinMode(2,OUTPUT);
lcd.setCursor(0, 0);
lcd.print("PM=");
lcd.setCursor(8, 0);
lcd.print("VP=");
lcd.setCursor(0, 1);
lcd.print("SP=");
lcd.setCursor(9, 1);
lcd.print("v=");
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
Serial.begin(9600);
}
void loop() {
lcd.setCursor(11, 0);
lcd.print(sp);

lcd.setCursor(6, 1);
lcd.print(" ");
lcd.setCursor(13, 1);
lcd.print(" ");
lcd.setCursor(11, 1);
lcd.print(valor);
lcd.setCursor(4, 1);
lcd.print(porpwm);
Serial.println(U);
}
ISR(TIMER1_COMPA_vect){
ang=(analogRead(potmotor))*180/1023; // potenciometro del MOTOR
sp=(analogRead(setpoint))*180/1023; // POT set point for PWM
xn=ang;// nuevo
yn=0.03394*xn_1 +0.03394*xn_2 +1.597*yn_1 -0.665*yn_2; // nuevo
en=sp-yn;// nuevo
U=KV*en;// nuevo
if (U<-100) // nuevo
{
U=-100;
}
if (U>100)
{
U=100;
}
U=(U*255)/100;
if (U<0)
{
digitalWrite(5, LOW);
analogWrite(6, -U);
}
if(U>=0)
{
digitalWrite(6, LOW);
analogWrite(5, U);
}
yn_2=yn_1;
yn_1=yn;
xn_2=xn_1;
xn_1=xn;
}