/*
 * Control_function_variable_declare.h
 *
 *  Created on: Aug 14, 2020
 *      Author: Liwei Zhou
 */

#ifndef CONTROL_FUNCTION_VARIABLE_DECLARE_H_
#define CONTROL_FUNCTION_VARIABLE_DECLARE_H_

/*
 * Control_function_variable_declare.h
 *
 *  Created on: Aug 14, 2020
 *      Author: Liwei Zhou
 */

// ...................Control variable define .........................//

//Inductor current observer
float Lp_obs = 0.5; //Observer proportional gain
float C_obs = 36E-6; //Observer parameter: filter capacitor
float L_obs = 450E-6; //Observer parameter: filter inductor

//Define fixed values
#define FskHz (10.0) //Define desired switching frequency, kHz
#define sysClkMHz (200.0) //Define DSP clock frequency, MHz
#define PERIOD (5000U) //Define period in PWM clocks
//#define PERIOD (1000.0*sysClkMHz/(2.0*FskHz)) //Define period in PWM clocks, **ASSUMING PWMCLK = SYSCLK**
#define DBR (100U) //Define deadband on rising edge, clock cycles
#define DBF (100U) //Define deadband on falling edge, clock cycles
#define PI (3.14159265359) //Definition of pi
#define PI2 (6.28318530718) //Definition of 2*pi
#define gridFreq (60.0) //Grid frequency
#define sqrt3 (1.73205080757) //sqrt(3)
//Component parameters

//Some other operational parameters
#define MWAS (5.0) //Moving Window Average Samples--the delay (in switching cycles) between position measurements used for speed calculation
//#define Ts (1*50E-6) //The sample time. Directly related to PERIOD. Define a PERIOD or Ts to get the other, per TI's spruhx5c document 0.00005
float Ts = 50E-6;//50E-6;
//#define Ts (((2.0*PERIOD/sysClkMHz)/1000.0)/1000.0) //The sample time. Directly related to PERIOD. Define a PERIOD or Ts to get the other, per TI's spruhx5c document

//Define global variables related to DSP
//float PERIOD = 10000; //Maximum value of TBCTR-- (SYSCLK/PERIOD)/2 = Switching frequency (up-down count mode)

int time; //calculate the control implementing time in CPU cycles

//Define global variables (different from #define as global variables can change--#define CANNOT)
float Iabc_grid[3] = {0.0, 0.0, 0.0}; //A, B, C -- Measured grid current
float Vcap_meas[3] = {0.0, 0.0, 0.0}; //A, B, C -- Measured capacitor voltage
float Va_avg;
float Vb_avg;
float Vc_avg;
float Ia_avg;
float Ib_avg;
float Ic_avg;
float Iind_obs[3] = {0.0, 0.0, 0.0}; //A, B, C -- Estimated inductor current
float Vcap_obs[3] = {0.0, 0.0, 0.0}; //A, B, C -- Estimated capacitor voltage
float Vcap[3] = {0.0, 0.0, 0.0}; //A, B, C --  Capacitor voltage fed to control
float Iabc_ind[3] = {0.0, 0.0, 0.0}; //A, B, C -- Inductor current fed to control
float Vdc = 0.0; //DC-link voltage--this will be populated with DC-link voltage measurement
int   Flag_Vdc_duty = 1;//0 means using constant Vdc for duty; 1 means using measured Vdc for duty
//float Vdc_initial = 84.85;
float duty_initial_DC = 0.0;
float Vdc_control = 120;
float Vdc_duty = 120;
float dutyABC[3] = {0.0, 0.0, 0.0}; //duty cycles to be applied to phases A, B, C
float theta = 0.0; //Electric position of motor
float omega = PI2*gridFreq; //Electric speed, rad/s
float Idq0_meas[3] = {0.0, 0.0, 0.0};
unsigned short count = 0; //Counts ADC cycles to toggle a GPIO
int   counter_delay = 0; //delay when enable control before enable PWM

//Define grid service working mode variables
int Grid_mode = 0;
unsigned short Grid_state = 0;
float Grid_CTRL[5] = {0.0, 0.0, 0.0, 0.0, 0.0};

//Values for use in control
float Vabc[3] = {0.0, 0.0, 0.0}; //Three-phase voltages used for duty cycle generation
float Valpha_cap = 0.0;//cap alpha voltage
float Vbeta_cap = 0.0;//cap beta voltage
float Ialpha_ind = 0.0;//inductor alpha current
float Ibeta_ind = 0.0;//inductor beta current
float Idq0_grid[3] = {0.0, 0.0, 0.0}; //grid dq0 currents
float Idq0_ind[3] = {0.0, 0.0, 0.0}; //grid dq0 currents
float Vdq0[3] = {0.0, 0.0, 0.0}; //dq0 voltages applied to inverter (from FOC)
float Vcapdq0[3] = {0.0, 0.0, 0.0}; //dq0 voltages (of filter capacitor ~= grid voltage)
float Vcapdq0_initial[3] = {0.0, 0.0, 0.0}; //constant feedforward cap voltage.
float Vd_rated = 391.0/2;//rated d frame voltage

//Define control parameters
//Inductor current control
float V_PI[3] = {0.0, 0.0, 0.0}; //Vdq0 obtained from PI controller
float Kp_ind  = 1.0; //Initial current control proportional gain
float Ki_ind  = 10.0; //Initial current integral gain
//float Kp_ind_q = 0.5; //Initial current control proportional gain
//float Ki_ind_q = 5.0; //Initial current integral gain
//float Kp_ind_0 = 0.5; //Initial current control proportional gain
//float Ki_ind_0 = 5.0; //Initial current integral gain
float Idq0_ind_ref[3] = {0.0, 0.0, 0.0}; //Initialize Idq0_ind references
float Intdq0_ind[3] = {0.0, 0.0, 0.0}; //dq integral values. Global so they can be stored (and viewed) easily
float ed = 0.0;
float eq = 0.0;
float e0 = 0.0;
float edq_ind_star[3] = {0.0, 0.0, 0.0}; //Used for anti-windup

//Grid current control
float Kp_grid = 0.005; //Initial current control proportional gain
float Ki_grid = 5.0; //Initial current integral gain
float Intdq0_grid[3] = {0.0, 0.0, 0.0}; //dq integral values. Global so they can be stored (and viewed) easily
float edq_grid_star[3] = {0.0, 0.0, 0.0}; //Used for anti-windup
float Idq0_grid_ref[3] = {0.0, 0.0, 0.0}; //Initialize Idq0_ind references
float Iq_max = 32.0; //Saturation parameter for Iq reference generation

//Phase-locked loop (PLL): Used to get grid voltage angle information
float omega_PLL = 0.0;
float Int_PLL = 0.0; //PLL integrator
float Kp_PLL = -2.0;
float Ki_PLL = -10.0;
unsigned char PLL_init = 0; //We use this to get an estimate of theta via the alpha-beta voltages WHEN THE SYSTEM FIRST STARTS

//DC voltage (power control)
float Vdc_ref = 120.0;
float Int_Vdc = 0.0;
float Kp_Vdc = -1.0;
float Ki_Vdc = -1.0;
float eVdc_star = 0.0; //DC voltage control anti-windup
float Id_max = 32.0; //Saturation parameter for Id reference generation

//Zero sequence voltage control
float Int_zs = 0.0;
float Kp_zs = 0.1;
float Ki_zs = 0.1;
float ezs = 0.0;
float ezs_star = 0.0; //Zero sequence voltage control anti-windup
float I0_max = 5.0; //Saturation parameter for I0 reference generation

//Reactive Power (grid current reference generation)
float PQ_ref[2] = {0.0, 0.0}; //Reference active and reactive power
float PQ[2] = {0.0, 0.0}; //Measured active and reactive power
float PQ_dq_Q = 0.0;
float Q_error = 0.0; //Reactive power error
float Q_error_int = 0.0; //Reactive power integral values in current sampling period
float Q_error_int_1 = 0.0; //Reactive power integral value in previous sampling period
float PF_ref = 0.0; //Power factor reference
float PF_meas = 0.0; //measured power factor
float Kp_Q = -0.001;
float Ki_Q = -0.001;
float eQ_star = 0.0; //Reactive power control anti-windup
float Q_max = 32.0; //Saturation parameter for Iq reference generation
float P_rated = 22000.0;//Rated active power
float S_rated = 24500.0;//Rated apparent power
float Q_rated = 10000.0;//Rated reactive power
//Grid service

//Inductor current observer
float Lp = 0.5; //Observer proportional gain
float iL_meas = 0.0; //We have one ADC available to compare measured and estimated inductor currents. Store and save via this variable
float C = 36E-6; //Observer parameter: filter capacitor
float L = 450E-6; //Observer parameter: filter inductor
float grad[2] = {0.0, 0.0}; //Gradient
float iHessian[2][2] = { {0.0, 0.0}, {0.0, 0.0} };
//float gamma = (1.0/6.0); //Multiplier for Newton solver
float gamma = 0.25; //Multiplier for Newton solver
float Vcap_meas_prev[3] = {0.0, 0.0, 0.0};
unsigned short OBS = 0; //Observer ON (==1) or OFF (==0)
unsigned short maxIters = 5; //5 iterations of optimization algorithm permitted
float maxError = 10.0; //Absolute error permitted

//Define values needed for ADC conversion interpretation & correction
//float correctionV = (90000.0/357.5)*1.014; //(HV series resistance)/(2.5*LV resistance)*linear slope from calibration
float G_Vdc = (90000.0/357.5);
float G_Vac = (80000.0/357.5);
float G_iv_ind = 1.0/0.04167; //Inverse inductor current sensor gain: A/V
float G_iv_grid = 1.0/0.025; //Inverse inductor current sensor gain: A/V
float Gvv = 403.2258065; //Lumped voltage scaling factor from Weizhong. No op amp circuit present
float iPointSix = 1.0/0.6; //ADC scaling factor is 0.6--pre-compute its inverse

//Variables for safety feature(s)
float Isafety = 63.0;//32.0; //Phase current maximum permissible before disabling
float Icritical = 63.0;//50.0; //Highest permissible current--SHUT OFF IMMEDIATELY
float Vgrid_square = 0;
float Vgrid_over = 1000;
unsigned short overCurrent = 0; //Overcurrent counter--once it reaches "consecutive," then we shut the system down
unsigned short consecutive = 2; //Consecutive overcurrents required to have the system shutting down. overCurrent is reset if not consecutive
float Iabc_fault[3] = {0.0, 0.0, 0.0}; //Initialize Idq references

//Some testing variables    `
//unsigned short time = 0;
float storage[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float V0_ref = 25.0;
float V0_trans = 300; //To be set during testing
float Vconst[3] = {0.0, 0.0, 0.0}; //For constant offset term in current control
unsigned short change_ZV = 0; //If change_ZV == 1, then V0_ref != Vdc/2

//Flags
unsigned short EN = 0; //Turn relays on
unsigned short CTRL = 0; //Enable control features
unsigned short EN_Q = 0; //Enable reactive power control
unsigned short EN_old = 0; //Previous value of EN
//unsigned short change_Q = 0;//force zero to q controller's Iq_ref by setting 0
unsigned short CTRL_old = 0; //Previous value of CTRL
unsigned short RLY_closed = 0; //Reset integrators when relay closes
unsigned short MODE = 0; //Switch the working modes for grid service


/*

//For oversampling ADCs
#define overSamp (10) //Number of samples to oversample by
unsigned short adcCount = 0; //Count the number of ADC acquisitons that have been done
float Isamp[3] = {0.0, 0.0, 0.0}; //Oversampled and averaged currents
float Vsamp[3] = {0.0, 0.0, 0.0}; //Oversampled and averaged capacitor (phase) voltages
float iLsamp = 0.0; //Over sampled and averaged indutor current
float Vdcsamp = 0.0; //DC voltage
//////////////////////////////////////////////////////////////////////////////////////////////////////////
float Isamp0_buffered[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float Isamp1_buffered[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float Isamp2_buffered[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float Vsamp0_buffered[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float Vsamp1_buffered[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float Vsamp2_buffered[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float Vdcsamp_buffered[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

float Isamp0_MA_sum = 0.0;
float Isamp1_MA_sum = 0.0;
float Isamp2_MA_sum = 0.0;
float Vsamp0_MA_sum = 0.0;
float Vsamp1_MA_sum = 0.0;
float Vsamp2_MA_sum = 0.0;
float Vdcsamp_MA_sum = 0.0;
int buffer_index = 0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//Arrays containing oversampled voltage values in digital. To be sorted for finding median value
int Va_samp[overSamp];
int Vb_samp[overSamp];
int Vc_samp[overSamp];
int Vdc_samp[overSamp];
int Ia_samp[overSamp];
int Ib_samp[overSamp];
int Ic_samp[overSamp];
int iL_samp[overSamp];

//Median filtered data for control algorithm
float Va_med = 0.0;
float Vb_med = 0.0;
float Vc_med = 0.0;

//For outputting currents on DAC
float output[2] = {0.0, 0.0}; //Initialize output

//Saving data to memory
#define sample_max (300U) //sample_max 500
#define vars (13U) //Number of variables I'm interested in
float data[vars][sample_max]; //Vector to store data
unsigned short capture = 0; //Begin capturing data
unsigned short transient = 0; //Trigger a transient event
unsigned short sample = 0; //What sample number we are at, from 0 to sample_max-1
unsigned short hold = 0; //Count up to hold_max for delay between samples (hold_max * Ts = space between samples)
unsigned short hold_max = 2; //We store every hold_max-th sample
unsigned short transient_hold = 0; //Declare variable
unsigned short transient_hold_max = 60; //We wait hold_max*transient_hold_max samples before making the transient occur
float Idq_trans[2] = {0.0, 0.0}; //The step I want for a transient. Global variable for changing during tests. Zero component omitted because we control it

*/


// ...................Function declaration .........................//

void linObs(unsigned short i); //Linear observer

void configTimer(void); //function to calculate the control implementing time
void gridCurrent(void); //Inductor current control
void indCurrent(void); //Inductor current control
void PLL(void); //Phase-locked loop to get grid voltage frequency
void powerCalc(void); //Get Idq references from power equations
void reactivepower(void); //Reactive power control
void constQmode(void); //Grid service constant Q mode
void constPFmode(void);//Grid service constant PF mode
void PQmode(void);//Grid service Watt-var mode
void VoltVarmode(void);//Grid service volt-var mode
void VoltWattmode(void);//Grid service grid voltage-active power mode
void FreqWattmode(void);//Grid service frequency-active power mode
void zeroVoltage(void); //Zero voltage control
void dcVoltage(void); //DC voltage control
void duty_AC(void); //Duty cycle and PWM generation
#endif /* CONTROL_FUNCTION_VARIABLE_DECLARE_H_ */
