Mann Morrision Arduino Code

float prevLast = -100;
float last = -100;
float f = 0.25;

float timeDelay = max(0.001, 1/(40*f));
float pi = 3.14159265359;

int analogPinPositive = A0;

int rst = 0;
int rstPin = 7;
float offSet = 2.5;
int sample = 0;
float cutoff = 2.5;
float VpeakReal = 2;


void setup() {
  // put your setup code here, to run once:
    Serial.begin(9600);
    Serial.print("Setup completed!!\n");
}

// Mann-Morrision algorithm approach for peak voltage calculation
float getPeakVol(float curr){
  float vDash = curr - prevLast;
  vDash = vDash/2;
  vDash = vDash/timeDelay;

  float omega = 2*pi*f;
  float last2 = last*last;

  float term2 = vDash/omega;
  term2 = term2*term2;

  float vPeak = last2 + term2;
  vPeak = sqrt(vPeak);
  return vPeak;
}

// Mann-Morrision algorithm approach for phase calculation
float getPhaseDeg(float curr){
  float vDash = curr - prevLast;
  vDash = vDash/2;
  vDash = vDash/timeDelay;

  float omega = 2*pi*f;
  float term1 = vDash/omega;
  float radPhase = atan2(last, term1) - omega*sample*timeDelay;
  float degPhase = radPhase*(180/pi);

  Serial.print(degPhase, 2);
  Serial.print(" ");

  return degPhase;
}

// Two sampling method to calculate phase
float getPhaseTwoSample(float currVolt, float currI, float peakV, float peakI) {
  float omega = 2 * pi * f;
  float theta = omega * timeDelay;
  float deno = peakV * peakI * sin(theta) * sin(theta);
  float term1 = prevVolt * prevI + currVolt * currI;
  float term2 = (prevI * currVolt + prevVolt * currI) * cos(theta);
  float num = term1 - term2;
  float phase = num / deno;
  phase = acosf(phase);
  float degPhase = phase * (180 / pi);
  return degPhase;
}



void loop() {
  // put your main code here, to run repeatedly:
  delay(timeDelay*1000);

  rst = digitalRead(rstPin);

// Reset the circuit values if reset is enabled
  if(rst){
    prevLast = 0;
    last = 0;
    sample = 0;
    return;
  }

  // Scanning user input
  int sensorValue = analogRead(analogPinPositive); // read the analog input value
  float voltage = sensorValue * (5.0 / 1023.0); // convert the sensor value to voltage (assuming 5V reference voltage)
  voltage = voltage - offSet;

  // Input wave
  Serial.print(voltage, 2); // print the input voltage value with 2 decimal places
  Serial.print(" ");

  // Setting default values
  if(prevLast ==-100){
    prevLast = voltage;
    return;
  }

  // Setting default values
  if(last==-100){
    last = voltage;
    return;
  }

  // Average peak voltage
  float rmsVol = getPeakVol(voltage);
  Serial.print(rmsVol, 2); // print the peak rms value with 2 decimal places

  // Avergae phase
  float degPhase = getPhaseDeg(voltage);
  Serial.print(degPhase, 2);

  Serial.print(" ");
  float error = VpeakReal - peakVol;
  Serial.print(error);
  Serial.print(" ");


  if(peakVol>cutoff){
    // Fault issue
    Serial.print(" Fault ");
  }
  else{
    // Input voltage is under range
    Serial.print(" NoFault ");
  }

  prevLast = last;
  last = voltage;
  Serial.print("\n");
}