/*
 * Timer1 is setup as a center aligned PWM. Timer2 serves as the DCO(Digital Controlled Oscillator)and generates an ISR when the overflow value is reached. All PLL code will run in this interrupt
 * The PLL uses a 1/2 park transform to act as a frequency / phase detector. When V_grid and the reference DCO(Digital Controlled Oscillator) are 90º out of phase 
 * the q output of the park transform will be zero. q is the phase error which is fed to the input of a PI controller
 * The output of the PI controler is mapped over to be the input overflow value for timer2. This closes the loop locking the grid signal and DCO output in quadrature
 * The base squarewave on PB6 and the pwm output on PA8 are in sync with the grid thanks to software counter sinInc_90 which runs +90º relative to counter sinInc
 * The pwm signal is provided on PA8 and can either be a pure 60Hz sinewave or it can follow the grid voltage input on PA0
 * Calculated constants are based on the user selected constants. Table one shows the effects of different increments on the PLL
 * The default increments is 256 for an ISR period of 65.08us
 * The default pwm frequency is 36kHz
 * 
 * revB
 * Add V_inv and I_inv and PV measuring variables
 * Add GRN & RED LED
 * Add QuickPID.h
 * Add #define manual_adjust true
 * Add PID for Inv_rms control
 * Add PV calculation
 * CHANGE rms measurements from 1/2 cycle to full cycle
 * Add Solar Constant voltage MPPT
 * Add Vgrid voltage check
 * 
 * RevC
 * Add PV_I & PV_Watts
 * Add analog input A2
 * Calibrate PV_I and PV_Watts
 * Add MPPT
 * Add inv_V check using PC817 daughterboard
 */
#include <STM32ADC.h>
STM32ADC myADC(ADC1);
#include "QuickPID.h"

/* Table1
 * Increments | pwm_ovf value | PWM Frequency |   ISR    | Phase Adj Increment  | Phase Jitter
 *    256     |     2343      |   15.365kHz   | 65.08us  |      +/-1.4º         |   +/-0.15º
 *    360     |     1666      |   21.609kHz   | 46.28us  |      +/-1º           |   +/-0.22º
 *    400     |     1500      |   24.000KHZ   | 41.67us  |      +/-0.9º         |   +/-0.24º
 *    512     |     1171      |   30.743kHz   | 32.5us   |      +/-0.7º         |   +/-0.3º
 */
// User selected constants
//********************************************************************************************************
const int clk_freq = 72E06;// Microcontroller nominal clock frequency
const int incrmnts = 256;// Number of increments for a 360º sine-wave (must be evenly divisable by 2)
const float grid_freq = 60.0;// Nominal grid frequency
// The phase may need to be compensated for transformer and other circuit effects.
// Table1 shows the minimum phase adjust increment based on the total number of full sinewave increments
int sinInc3 = 67;// Phase adjust when using V_grid for pwm modulation. (incrmnts / 4 = 90º)
//  Add or subtract from the +90º offset
const float phase_adjust = 2.0;// Phase adjust +/-increment when using DCO for pwm modulation
volatile float PV_setpoint = 24.5;
#define manual_adjust false
//********************************************************************************************************















#if manual_adjust == true
volatile float mod_factor = 0.0;
#else
volatile float mod_factor = 0.12; //.185 with 40kΩ load  0.12 with 200kΩ load
#endif

/*Calculated constants*/
const float grid_freq_upper = grid_freq + .5;// Grid frequency upper bound for pll lock
const float grid_freq_lower = grid_freq - .5;// Grid frequency lower bound for pll lock
const int phase_correction = incrmnts / 4;// corresponds to a 90º phase shift
const int half_incrmnts = incrmnts / 2;
const float grid_per = 1/60.;
const float incrmnt_sec = grid_per/incrmnts;
const int isr_freq = 1/incrmnt_sec;
const int pwm_ovf_edge = clk_freq / isr_freq;// Nominal overflow value for PWM edge aligned mode
const int pwm_ovf_center = (clk_freq / isr_freq)/2;// Nominal overflow value for PWM center aligned mode
const int pwm_ovf_upper = (grid_freq_upper / grid_freq)*pwm_ovf_center;
const int pwm_ovf_lower = (grid_freq_lower / grid_freq)*pwm_ovf_center;

// Global variables
static int DCO_sinArry[incrmnts];//array containg the DCO sine wave
static int grid_Arry[incrmnts];//array containg the grid sine wave
static float cos_theta[incrmnts];//0 +/- 1.0 cosine wave
static float sin_theta[incrmnts];//0 +/- 1.0 sine wave
int pwm_ovf = pwm_ovf_center;// Timer1 overflow value 
int V_grid;// Grid sample
int V_grid_rms;
int V_inv;// Inverter voltage
unsigned long sum_sq_V_grid;
int Inv_pk_volts;
int I_inv;// Inverter current
int PV_V;
int PV_I = 0.0;
float PV_Watts = 0.0;
float prev_PV_Watts = 0.0;
float PV_amps = 0.0;
float PV_volts = 0.0;
unsigned long sum_PV_V;
unsigned long sum_PV_I;
float I_inv_rms = 0.0;
unsigned long sum_sq_I_inv;
volatile int sinInc;// Software counter 1
volatile int sinInc_90 = phase_correction + phase_adjust;// Software counter 2
int DCO_sine;
int pwmVal;
int pot;
int cycleCnt;
float cos_angle;
float sin_angle;
volatile float feedback =0;

//enum last_cv{INCREASE, DECREASE};
int last_cv = 1;

// PLL PI Tuning variables
//**************************
int pll_out;
int phase_error;
int q;
float PI_p, PI_i;
float pll_Kp = 10, pll_Ki = .05;
//**************************

//Inverter current PID tuning variables
float Setpoint, Input, Output;
float Kp = 5, Ki = 10, Kd = 0; // 5,10,0
//Specify inverter current PID links
QuickPID myPID(&Input, &Output, &Setpoint);

bool inv_over_volt = false;
bool inv_under_volt = false;
bool gridTie = false;
bool startup_good = false;
bool grid_good = false;
bool inv_good = false;
bool PV_good = false;
bool grid_disconnect = false;
bool V_grid_good = true;



void setup() {
  //calibrate ADC.
  myADC.calibrate(); 
  myADC.setSampleRate(ADC_SMPR_7_5); // approx 3us per sample

  
  // Apply PID constants
  myPID.SetTunings(Kp, Ki, Kd);
  //turn the PID on
  myPID.SetMode(myPID.Control::automatic);
  // Constrain the Output
  myPID.SetOutputLimits(20,1000);//20,1000
  // Set the sampling rate in microseconds
  myPID.SetSampleTimeUs(10);//10us
  // Set controller action to direct (default) or reverse
  myPID.SetControllerDirection(myPID.Action::direct); 

  // Set up inputs
  pinMode(PA0, INPUT_ANALOG);// Grid voltage
  pinMode(PA1, INPUT_ANALOG);// Solar Panel V_Sense
  pinMode(PA2, INPUT_ANALOG);// Solar Panel I_sense
  pinMode(PA3, INPUT_ANALOG);// Inverter voltage
  pinMode(PA4, INPUT_ANALOG);// Inverter current
  pinMode(PA5, INPUT_ANALOG);// Pot
  pinMode(PB11, INPUT_PULLUP);//GT Manual Switch

  // Set up outputs
  pinMode(PB6, OUTPUT);// 60Hz sq-wave
  pinMode(PB8, OUTPUT);// RED LED
  pinMode(PB9, OUTPUT);// GRN LED  
  pinMode(PB12, OUTPUT);// Test
  pinMode(PB13, OUTPUT);// GT Relay
  // Turn OFF the GT relay
  gpio_write_bit(GPIOB,8,0); 
  gpio_write_bit(GPIOB,9,0);
  gpio_write_bit(GPIOB,13,0);

  delay(250);
  // Set up  Timer2 for ISR
  Timer2.setPrescaleFactor(1);//1 to 65,536 Freq = (72MHz / Prescale_val=1)
  Timer2.setOverflow(pwm_ovf);
  // Set up for center aligned mode
  bitSet(TIMER2_BASE->CR1,6);
  Timer2.attachInterrupt(TIMER_CH1, TIM2_ISR);
  
  delay(250);
  // Set up  Timer1 for PWM on Channel 1
  Timer1.setPrescaleFactor(1);//1 to 65,536 Freq = (72MHz / Prescale_val=1)
  Timer1.setOverflow(1000);//36kHz center aligned
  // Set up for center aligned mode
  bitSet(TIMER1_BASE->CR1,6);

  // Initially Shut OFF pwm
  pwmWrite(PA8,0);


  delay(250);
  
  for(int i = 0; i < incrmnts; i++)
  { 
    // Create sine array to near the nominal peak value of the grid sample
    int val = sin(i*M_PI/(half_incrmnts)) * 550;
    DCO_sinArry[i] = val;
    // Create full sine and cosine arrays -1 to +1
    float val2 = sin(i * M_PI/half_incrmnts);
    sin_theta[i] = val2;
    float val3 = cos(i * M_PI/half_incrmnts);
    cos_theta[i] = val3;
  }
  
  // Wait 1 sec before turing on the PWM GPIO
  // This will prevent output transients since PA8 was already written to zero during Timer1 setup
  delay(1000);
  pinMode(PA8, PWM);//Timer1 Channel 1(T1C1) output assigned to PA8 and setting it to PWM Output

} //end setup

void TIM2_ISR(){
gpio_write_bit(GPIOB,12,1);  
    V_grid = analogRead(PA0)-2036;//
    PV_V = analogRead(PA1);
    PV_I = analogRead(PA2)-2025;// Calibrated on 12/19/22 @9:43PM
    
    if(!gridTie)
    {
      V_inv = analogRead(PA3);
    }
      
    I_inv = analogRead(PA4)-2150;
    if(manual_adjust && V_grid_good)
    {
      pot = analogRead(PA5);
      mod_factor = pot *.0003;
    }
    
    // Store the grid sample values in the grid_array
    grid_Arry[sinInc_90] = V_grid;

    // DCO counter
    if(++sinInc >= incrmnts)
    {
      sinInc = 0;//
    }
    // Fixed 90º Phase Shift  
    if(++sinInc_90 >= incrmnts)
    {
      sinInc_90 = 0;//
    }
     
    // Adustable Phase Shift for grid_Arry sample
    if(++sinInc3 >= incrmnts)
    {
      sinInc3 = 0;//
    }
    
    // PLL
    // Timer1 interupts every 65.08us this corresponds to 60Hz grid taking 256 samples per cycle(65.08us * 256 incrmnts)
    // Variable "sinInc" is advanced by one count for each interrupt up to 359 these counts represent a φ of 1º per cnt
    // Using 1/2 of a park transformation as a frequency/phase detector where as q = (ß*cosφ)-(α * sinφ) where ß= internal oscillator DCO_sinArry[] and α = V_grid
    // cosφ and sinφ are generated in setup() and each are stored in an array.
    // The PI controller keeps Timer1 in sync with the grid by adjusting the timer1 overflow value every increment (1º) and has a max phase jitter of +/- 0.22º
    // maintaing a 90º difference between α and ß resulting in a q = phase error of 0 and a more positve error when grid freq is higher and visa-versa

    // Frequency / Phase Discriminator (1/2 Park Transformation)
    sin_angle = sin_theta[sinInc];
    cos_angle = cos_theta[sinInc];
    DCO_sine = DCO_sinArry[sinInc];
    q = (DCO_sine * cos_angle) - (V_grid * sin_angle);//
    phase_error = -q;
    // PI Controller
    PI_p = pll_Kp * phase_error;
    PI_i = PI_i + (pll_Ki * phase_error);
    pll_out =PI_p + PI_i;
    pll_out = map(pll_out, -1000,1000, pwm_ovf_lower, pwm_ovf_upper);
    pwm_ovf = pll_out;   
    Timer2.setOverflow(pwm_ovf);

    // Timer1 PWM value is either the absolute value of the pure reference sinewave or grid sinewave
    //pwmVal = mod_factor * abs(DCO_sinArry[sinInc_90]);
    pwmVal = (mod_factor + feedback) * abs(grid_Arry[sinInc3]);   
    
    // Generate a base square-wave and pwm in sync with the grid      
    if(sinInc_90 < half_incrmnts)
    {
      pwmWrite(PA8, pwmVal);
      gpio_write_bit(GPIOB,6,1);
    }
    else
    {
      pwmWrite(PA8, pwmVal);    
      gpio_write_bit(GPIOB,6,0);
    }

    // Measure Inverter voltage

    
    // Measure and calculate inverter rms voltage and rms current
    // Collect inverter voltage data during each cycle
    // Perform the calculations one time per cycle at sinInc3 = 0

    if(sinInc3 == 0)
    {
      cycleCnt++;
      if(!manual_adjust)
      {
        // Inverter current PI Controller
        // Controls at the current limit setpoint or around the solar panel voltage setpoint        
        Input = I_inv_rms;   
        if(gridTie)
          Setpoint = 0.43;// 0.43 = 100W  Use 0.33 for load testing 673Ω
        else
          Setpoint = 0.0;// reset the output to zero 

        if(PV_volts >= PV_setpoint)
          myPID.SetControllerDirection(myPID.Action::direct);       
        else
          myPID.SetControllerDirection(myPID.Action::reverse);
          
        myPID.Compute();
        
        if(gridTie)
          feedback = Output *.003;
        else
          feedback = 0.0;// keep fedback at zero when not grid tied

        // Perform MPPT control by varying the solar panel voltage setpoint 
        // The MPPT is active only when the rms output current is below the setpoint limit
        if(I_inv_rms < Setpoint - .043)// setpoint @ 100W - 10%
        {
          //gpio_write_bit(GPIOB,8,1);//
          //gpio_write_bit(GPIOB,9,1);//
          // Min/Max setpoint is 24.5/29.5 respectively
          // The MPPT executes once every 5 seconds
          if(cycleCnt >= 300)
          {
            cycleCnt = 0;
            if(PV_Watts >= prev_PV_Watts)
            {
              if(last_cv == 1 && PV_setpoint < 29.5)
                PV_setpoint += 0.5;
              else if(last_cv == -1 && PV_setpoint > 24.5)
                PV_setpoint -= 0.5;
            
            }
            else
            {
              if(last_cv == 1 && PV_setpoint > 24.5)
              {
                PV_setpoint -= 0.5;
                last_cv = -1;
              }
              else if(last_cv == -1  && PV_setpoint < 29.5)
              {
                PV_setpoint += 0.5;
                last_cv = 1;                 
              }
            
            }
           prev_PV_Watts = PV_Watts;
            
          }// END Cyclecnt > 60
        }//   END I_inv_rms < Setpoint
      }//     END !manual adjust
        
        // Calculate the measured values
        // Calculate the square-root of the sum of the squares ans convert to actual rms values
        V_grid_rms = sqrt(sum_sq_V_grid / incrmnts ) * .569;//     .569 Calibrated on 12/19/22 @9:07AM
        I_inv_rms = sqrt(sum_sq_I_inv / incrmnts ) * .00044;//     .00044 Calibrated on 12/19/22 @8:00AM 
        
        // Average the sum of the solar panel input values and convert to actual Volts, Amps and Watts
        PV_volts = (sum_PV_V / incrmnts) * .01255;//               .01255 Calibrated on 12/19/22 @9:12AM
        PV_amps = (sum_PV_I / incrmnts) * .013;//.013   Calibrated on 12/19/22 @10:00PM
        PV_Watts = PV_amps * PV_volts;//Calibrated on 12/19/22 @10:00PM

        // Reinitialize the variables
        sum_sq_V_grid = 0;
        sum_sq_I_inv = 0;
        sum_PV_V = 0;
        sum_PV_I = 0;

      }
      // Acquire measured values over one full cycle
      // Obtain the sum of the squares
      sum_sq_V_grid = sq(V_grid) + sum_sq_V_grid;
      sum_sq_I_inv = sq(I_inv) + sum_sq_I_inv;
      // sum of solar panel voltage
      sum_PV_V = PV_V + sum_PV_V;
      sum_PV_I = PV_I + sum_PV_I;

      // Get the peak inverter voltage at 90º and convert to peak volts
      if(sinInc3 == 64)
      {
        Inv_pk_volts = V_inv * .16;// Calibrated on 12/21/22 @11:37AM
      }
      

gpio_write_bit(GPIOB,12,0);    
}

void loop(){
  //Monitor Manual GT Relay switch
  if(!gpio_read_bit(GPIOB,11) && V_grid_good)
  {
    gpio_write_bit(GPIOB,13,1);
    gridTie = true;
  }
  else
  {
    gpio_write_bit(GPIOB,13,0);
    gridTie = false;
  }
  
  // Grid voltage Monitor
  if(V_grid_rms > 255 || V_grid_rms < 205 && gridTie)
  {
    V_grid_good = false;
    gridTie = false;
    mod_factor = 0.12;
    gpio_write_bit(GPIOB,8,0);// GRN LED OFF
    gpio_write_bit(GPIOB,9,1);// RED LED ON    
  }
  // Grid voltage calibration
  /*
  if(V_grid_rms >= 230)
  {
    gpio_write_bit(GPIOB,9,1);// RED LED ON
    gpio_write_bit(GPIOB,8,0);// GRN LED OFF 
  }
  else
  {
    gpio_write_bit(GPIOB,9,0);// RED LED OFF
    gpio_write_bit(GPIOB,8,1);// GRN LED ON
  }
  */

  //Inv current calibration 
  /*
  if(I_inv_rms >= .23)
  {
    gpio_write_bit(GPIOB,8,0);// GRN LED OFF
    gpio_write_bit(GPIOB,9,1);// RED LED ON
  }
  else if(I_inv_rms < .2)
  {
    gpio_write_bit(GPIOB,8,1);// GRN LED ON
    gpio_write_bit(GPIOB,9,0);// RED LED OFF 
  }
  */
  // Inv PEAK voltage calibration
  /*
  if(Inv_pk_volts >= 350)
  {
    gpio_write_bit(GPIOB,9,1);// RED LED ON
    gpio_write_bit(GPIOB,8,0);// GRN LED OFF
  }
  else if(Inv_pk_volts < 300)
  {
    gpio_write_bit(GPIOB,9,0);// RED LED OFF
    gpio_write_bit(GPIOB,8,1);// GRN LED ON
  }
  */
  // PV voltage calibration
  /*
  if(PV_volts < 25)
  {
    gpio_write_bit(GPIOB,9,1);// RED LED ON
    gpio_write_bit(GPIOB,8,0);// GRN LED OFF  
  }
  else if(PV_volts >=30)
  {
    gpio_write_bit(GPIOB,9,0);// RED LED OFF
    gpio_write_bit(GPIOB,8,1);// GRN LED ON  
  }
  */
  
  // PV Current calibration
  /*
  if(PV_amps > 1.0)
  {
    gpio_write_bit(GPIOB,9,1);// RED LED ON
    gpio_write_bit(GPIOB,8,0);// GRN LED OFF  
  }
  else if(PV_amps <0.8)
  {
    gpio_write_bit(GPIOB,9,0);// RED LED OFF
    gpio_write_bit(GPIOB,8,1);// GRN LED ON  
  }
  */
  // PV Watts calibration
  /*
  if(PV_Watts > 25.0)
  {
    gpio_write_bit(GPIOB,9,1);// RED LED ON
    gpio_write_bit(GPIOB,8,0);// GRN LED OFF  
  }
  else if(PV_Watts <20.0)
  {
    gpio_write_bit(GPIOB,9,0);// RED LED OFF
    gpio_write_bit(GPIOB,8,1);// GRN LED ON  
  }
  */
} //end loop