#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#define NPWM 200
#define PPWM (160000 / NPWM)

uint8_t f50,oen;
uint16_t pcount,uf;
uint32_t vpwr,sst;
uint16_t l[NPWM+1];
float pwr,ch2,ch0;
uint16_t dbounce;
double Input, Output, Setpoint;
double errSum, lastErr,error,dErr;
double kp, ki, kd;
float xv[10],yv[10];

void setup()
{
int i;
float t,u;

for(t=0.0,i=0; i <= NPWM; i++,t += 3.14159/ (float) NPWM)
{
u = 16384.0 * sin(t);
l = (int)u;
}

pinMode(9, OUTPUT);
pinMode(8,INPUT_PULLUP);
DDRD = 0xff; // can NOT let arduino system alter any pins from D0 to D7 via calls to pinMode() etc.. It buggers d5 which I need to pull down low-Z upon boot
noInterrupts();
TCCR2A = 0; // I had some bits set which interfered with simple digital in PORTD. set to zero means no i/o pins driven by timer2.
TCCR2B = _BV(CS22)| _BV(CS21) | _BV(CS20);
TIMSK2 |= (1 << TOIE2);
TCCR1A = _BV(COM1A1) | _BV(COM1B0) | _BV(COM1B1) | _BV(WGM11);
TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
OCR1A = 10;
ICR1 = PPWM;
TIMSK1 |= (1 << TOIE1);

sst = 0;
f50 = 0;
pcount = 0;
uf = 0;
ch2 = ch0 = 0.0;
pwr = 0.0;
vpwr= 0;
dbounce = 0;
oen = 0;
// set up PID 0.1 0.0 0.01 for biquad LP Fc = 0.02, np=4
kp = 0.1;
ki = 0.0;
kd = 0.01;
errSum = 0.0;

interrupts(); // enable all interrupts
TIMSK0 = 0; // except arduino clock

sbi(ADCSRA,ADPS2) ; // ADC clock prescale = 16 now. this saves 100us in main loop with little cost to loop stability
cbi(ADCSRA,ADPS1) ;
cbi(ADCSRA,ADPS0) ;
}


ISR(TIMER2_OVF_vect)
{// 100 Hz
TCNT2=100;
pcount=0;
f50 = !f50;
TCNT1=0;
uf=1;
if(oen == 1)
sbi(PORTD,5);
else
cbi(PORTD,5);
check_switch();
}

ISR(TIMER1_OVF_vect)
{//20Khz SPWM
long c;
if(pcount == 1) // order of events is important. This gives the least time difference from 50Hz and 20kHz gate drive
{ // When this difference is big, larger AC output waveform distortion occurs.
if (f50 == 1)
cbi(PORTD,7);
else
sbi(PORTD,7);
}
c = (l[pcount] * vpwr) >> 14; // scale sin wave by vpwr 32 bit integer calcs.
if (f50 == 1) // one 1/2 wave or other of 50Hz output
OCR1A = c;
else
OCR1A = PPWM - c;
pcount++;
}


void do_pid()
{
double timeChange = 0.01; // always called at 100Hz, so dT will be 10msec..
error = Setpoint - Input;
errSum += (error * timeChange);
dErr = (error - lastErr) / timeChange;
Output = kp * error + ki * errSum + kd * dErr;
lastErr = error;
}


void loop()
{
float s,fch0;
// filter ch2,0 analog input and scale to 0 to 1
// ch0 = output ACV, via transformer, 220VAC = 3.5V DC
// ch2 = setpoint. About 4.0kHz sample rate, or 250us per 2ch sample and filter
ch2 = 0.99*ch2 + 0.01 * (float)analogRead(2)/1024.0; // get AC output setpoint, LP filter
fch0 = analogRead(0)/1024.0; // get sample of AC output

// this is a Bessel LP filter of some Fc.
xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4];
xv[4] = fch0 / 2.259747797e+05;
yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4];
yv[4] = (xv[0] + xv[4]) + 4 * (xv[1] + xv[3]) + 6 * xv[2]
+ ( -0.7428578687 * yv[0]) + ( 3.1954036268 * yv[1])
+ ( -5.1599230905 * yv[2]) + ( 3.7073065280 * yv[3]);
ch0 = yv[4];

if(uf == 1 && oen == 1 ) // this will execute at 100Hz, thanks to uf = 1, set within 100Hz ISR(TIMER2_OVF_vect)
{ // it runs at near zero volt crossing of AC output
uf=0;
if(sst < 250) // slow start timer counts to 200 with gentle ramp up, then switches over to PID control. (gulp!)
{
sst++; // comment this line to prevent PID control, it will remain in simple bang-bang control
if(ch0 < ch2)
pwr += 1.0/300.0;
else
pwr -= 1.0/300.0;
}
else
{
Setpoint = ch2; // PID control. set the setpoint, input get output, scale, convert to int
Input = ch0;
do_pid();
pwr = pwr + Output;
}
if (pwr > 1.0) pwr = 1.0; // clamp . Do math in floating point to prevent any integer overflow
if (pwr < 0.01) pwr = 0.01; // it's slower. We could do clamp after conversion to int but it's your mosfets...
s = (float)PPWM * pwr;
vpwr = (int)s;
}
}

void check_switch()
{
dbounce++;
if (dbounce > 100) dbounce = 100;
if (digitalRead(8) == 0) // momentary button pressed, pulling internal pullup low
{
if (dbounce > 80)
{
if(oen == 0)
{ // enable it. STAND BACK!
oen = 1;
pwr = 0.0;
vpwr = 0;
sst=0;
}
else
{ // stop inverter
oen = 0;
pwr = 0.0;
vpwr = 0;
errSum = 0.0;
sst=0;
}
dbounce=0;
}
}
}