#include <EEPROM.h>

#include <SoftwareSerial.h>
SoftwareSerial sSerial(7, 18); // RX, TX

int pwm,cc,dd;

float Vsp, Isp;
float Vin,Vout,Iin,Iout;
volatile float xVin,xVout,xIin,xIout,tIin, tIout;
int oIin,oIout;


void get_biquad(float,int,int);
void biquad_filter(float *,int);

float aa[5][4],d1[5][4],d2[5][4];
float w0[5][4],w1[5][4],w2[5][4];
int bqnp[4];

volatile int f50c,f50;

volatile char buf[80],*bp,bcount;

void get_biquad(float fc, int np, int idx)
 {  
 float s,r;
 int i;  
 float a = tan(M_PI*fc);
 float a2 = a*a;
 bqnp[idx] = np;
 for(i=0; i<np; ++i)
   {
   r =  sin(3.141592926 * (2.0*i+1.0)/(4.0*(float)np));
   s = a2 + 2.0*a*r + 1.0;
   aa[i][idx] = a2/s;
   d1[i][idx] = 2.0*(1-a2)/s;
   d2[i][idx] = -(a2 - 2.0*a*r + 1.0)/s;
   w1[i][idx]=w2[i][idx]=w0[i][idx]=0.0;
   }
 }

void biquad_filter(float *val, int idx)
{
 float s;
 int i;
 s = *val;
 for(i=0; i<bqnp[idx]; ++i)
   {
   w0[i][idx] = d1[i][idx]*w1[i][idx] + d2[i][idx]*w2[i][idx] + s;
   s = aa[i][idx]*(w0[i][idx] + 2.0*w1[i][idx] + w2[i][idx]);
   w2[i][idx] = w1[i][idx];
   w1[i][idx] = w0[i][idx];
   }
 *val = s;
}


void setup()
 {  
 pwm = 1;
 pinMode(7,INPUT);   // let comparator inputs float, dont drive them internally
 pinMode(6,INPUT);
 
 sSerial.begin(9600);
 bp = &buf[0];
 for(bcount=0; bcount < 80; bcount++)
   buf[bcount]=20;
 bcount = 0;
 pinMode(9, OUTPUT);
 pinMode(10, OUTPUT);
 TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
 TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
 OCR1A = 1;
 //OCR1B = 2;
 ICR1 = 800;
 TIMSK1 |= (1 << TOIE1);   // enable timer overflow interrupt
 Serial.begin(115200);
 
 Vsp = 10.0;
 Isp = 55.0;

 get_biquad(0.1,2,0);
 get_biquad(0.1,2,1);
 get_biquad(0.02,2,2);
 get_biquad(0.02,2,3);
 cc = 0;
 dd = 0;
 
 f50c=0;
 f50=0;
 xVin = 0.0898;
 xVout = 0.0900;
 xIin = 0.235;
 xIout = 0.244;
 tIin = 0.00;
 tIout = 0.00;
 oIin = 511;
 oIout = 511;
 Serial.println("setup complete");
}

ISR(TIMER1_OVF_vect)        
{
OCR1A = pwm;    // pin 9
// OCR1B = pwm;    // pin 10
f50c++;
if(f50c > 399)
 {
   f50c=0;
   f50=1;
 }
}  




 
void loop()
  {
  int a0,fa,fi,nupwm;
  float p,e;
  char fbuf[10],gbuf[10],hbuf[10];
 
  // voltage setpoint via pot on A5, 0 to 45V
  Vsp = Vsp * 0.99 + 0.01 * 60.0 * analogRead(5)/1024.0;
     
  // get output voltage  
  Vout = xVout * (float)analogRead(2);
  biquad_filter(&Vout,0);

  // get input voltage
  Vin = xVin * (float)analogRead(0);
  biquad_filter(&Vin,1);

  // get output current
  Iout = - tIout + xIout * (float)(analogRead(3) - oIout);
  biquad_filter(&Iout,2);

  // get input current
  Iin = -tIin + xIin * (float)(analogRead(1) - oIin);
  biquad_filter(&Iin,3);
   
  if (f50 == 1)
   {
   f50=0;
   nupwm = pwm;
   if ( Vin < Vsp)    // if input V is less than output V, shut down    
         nupwm -= 5;                
   
     if (Vout < Vsp && Iout < Isp && ( Vin > Vsp))   nupwm ++;  
     if (Vout > Vsp || Iout > Isp || ( Vin < Vsp))   nupwm --;  
     if (Iout > Isp)       nupwm -= 5;    
     
     // clamp to reasonable values
     if (nupwm > 799) nupwm = 799;
     if (nupwm < 1) nupwm = 1;
     pwm = nupwm;
     
   // write out LCD data, one byte at a time  
   sSerial.write(buf[bcount]);
   bcount++;
   if (bcount > 79)
     {
       bcount = 0;
       sSerial.write('$');  
     }
   }

 
 
  if (dd++ > 200)
   {
     dd=0;

     Serial.print(Vout * Iout /10.0);      Serial.print(" ");
     Serial.print(Vin * Iin /10.0);      Serial.print(" ");
     Serial.print(Vin);      Serial.print(" ");
     Serial.print(Vout);      Serial.print(" ");  
     Serial.print(Iin);      Serial.print(" ");
     Serial.print(Iout);      Serial.print(" ");  
     Serial.print(Vsp);      Serial.print(" ");
     Serial.print(" 200 ");  
     Serial.println();

     /*
     for (a0=0; a0 < 8; a0++)
       {
         Serial.print(analogRead(a0)); Serial.print (" ");
       }
     Serial.println();
     */  
     dtostrf(Vin,4,1,fbuf);  
     dtostrf(Iin,4,1,hbuf);      
     sprintf(bp,"In  %4sV %4sA    ",fbuf,hbuf); buf[19] = 20;
     dtostrf(Vout,4,1,fbuf);  
     dtostrf(Iout,4,1,hbuf);      
     sprintf(bp+40,"Out %4sV %4sA    ",fbuf,hbuf); buf[59] = 20;
     p = Vout*Iout;
     dtostrf(p,5,0,fbuf);        
     sprintf(bp+20,"%5sW Out         ",fbuf); buf[39] = 20;
     dtostrf(Vsp,4,1,fbuf);        
     sprintf(bp+60,"setpoint %4s      ",fbuf); buf[59] = 20;
   }  
  }

#include <LiquidCrystal_PCF8574.h>
#include <Wire.h>
LiquidCrystal_PCF8574 lcd(0x3f);

#include <SoftwareSerial.h>
SoftwareSerial sSerial(10, 11); // RX, TX

void setup()
{
sSerial.begin(9600);
lcd.begin(20,4);    
lcd.setBacklight(255);
lcd.clear();
lcd.print("Serial -> LCD       ");
lcd.print("pin D10 for RX      ");
lcd.print("9600 baud           ");
delay(2000);

}

void loop()
{
 int c;
while (sSerial.available())
   {
     c = sSerial.read();
     if (c == '$')
       lcd.setCursor(0,0);
     else
       lcd.write(c);
   }

}