/* PID with two PWM outputs 180° out of phase
   for https://forum.arduino.cc/t/closed-loop-with-2-pwm-signals-180-degree-apart/1286865/25
  
  Use: Adjust the Setpoint Slider to indicate the setpoint to the controller
       Simulate the process and process disturbances by adjusting the 
       "ProcessOutput/ControllerInput" pot based on what you think the process will 
       do based on the duty cycle and whatever else.
       Monitor the Scope to see how the PID adjusts the duty cycles

Adjust the:
 Kp variable higher or lower to tmake the duty cycle more or less responsive to errors
 Ki variable higher or lower to increase or reduce pressure on modifying the duty cycle
 Kd variable higher or lower to anticipate more or less strongly the future effects of change 

*/

#include "TimerHelpers.h" // https://www.gammon.com.au/forum/?id=11504
// for https://forum.arduino.cc/t/two-pwm-signals-on-pins-9-and-10-with-input-on-a1-signals-need-to-be-180-degrees-apart/1280072/4?u=davex

#include "TimerHelpers.h"

#include <PID_v1.h>

int pwmPin1 = 9;
int pwmPin2 = 10;
double analogPin0 = A0;
double analogPin1 = A1;
double Out = 0;
double Involtage = 0;

double Setpoint, Input, Output;
double Kp = 0.4, Ki = 0.05, Kd = 0;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  pinMode(pwmPin1, OUTPUT);
  pinMode(pwmPin2, OUTPUT);

  TCCR1A = 0;        // reset timer 1
  TCCR1B = 0;

  Timer1::setMode (8, Timer1::PRESCALE_8, Timer1::CLEAR_A_ON_COMPARE | Timer1::SET_B_ON_COMPARE);
  ICR1 = 250; // 250 -> 32kHz w PRESCALE_1 Sets frequency with prescaler as f=F_CPU/PRESCALE/ICR/2

  TCNT1 = 0;         // reset counter
  OCR1A =  ICR1 / 4;     // compare A register value
  OCR1B =  ICR1 - OCR1A;   // compliment and invert

  Serial.begin(115200);
  analogWrite(pwmPin1, 120);

  //initialize the variables we're linked to
  Input = analogRead(analogPin0);
  Setpoint = 200; // 175= 10v 220=12V

  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(0, ICR1);
}

void loop() {

  Out = analogRead(analogPin0);// read the output voltage pin
  Involtage = analogRead(analogPin1);// read the input voltage pin
  Setpoint = Involtage;
  Input = Out;
  if (myPID.Compute()) {

    OCR1A = Output; // duty cycle
    OCR1B = ICR1 - OCR1A; // complement for inversion
    if (0) {
      Serial.print ("input volts    ");
      Serial.print ((Involtage * (5.0 / 1023)) * (12.3)); // 12.3 for resistors
      Serial.print("  ");
      Serial.print (Involtage);

      Serial.print("    output volts   ");
      Serial.print ((Out * (5.0 / 1023)) * (12.9)); // 12.9 for resistors
      Serial.print("  ");
      Serial.println (Out);
    }
  }
  report();
}

void report() {
  static uint32_t last = 0;
  if (millis() - last >= 500) {
    last += 500;
    Serial.print("Input:"); Serial.print(Input);
    Serial.print(" Setpoint:"); Serial.print(Setpoint);
    Serial.print(" Output:"); Serial.print(Output);
    Serial.print('/');
    Serial.print(ICR1);
    Serial.println();
  }
}



void reportTimer1(void) {
  Serial.print("TCCR1A:0b");
  Serial.println(TCCR1A, BIN);
  Serial.print("TCCR1B:0b");
  Serial.println(TCCR1B, BIN);
  Serial.print("ICR1:");
  Serial.println(ICR1);
  Serial.print("OCR1A:");
  Serial.println(OCR1A);
  Serial.print("OCR1B:");
  Serial.println(OCR1B);
  Serial.print("Duty:");
  Serial.println(OCR1A * 100.0 / ICR1, 3);
  Serial.print("Freq:");
  Serial.print(F_CPU / 256.0 / ICR1 / 2, 4);
  Serial.println("Hz\n");
}