const int pwmtop=128;
const float ADCAdjust=5.0/1024.0;
const float InVoltDivider=7.8*ADCAdjust;
const float OutVoltDivider=7.8*ADCAdjust;
unsigned long previousMillis;
float PWMValue=0.1;
float inputVoltage;
float outputVoltage;


void setup(){
//TCCR2B = TCCR2B & B11111000 | B00000001; //3+11 to 31KHz
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
TCCR2B = _BV(WGM22) | _BV(CS20);
OCR2A = pwmtop; //Go faster.

pinMode(3, OUTPUT); //PWM drive
pinMode(A0, INPUT); //From input voltage divider.
pinMode(A1, INPUT); //From output voltage divider.
Serial.begin(9600);
}

void loop(){
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis > 100) {
    previousMillis = currentMillis;
    takeReadings();
    everytick();
  }
}

float lastOut=0;


void takeReadings(){
  float avgIn=0;
  float avgOut=0;
  const int samples=32;
  for(int n=0;n<samples;n++){
    avgIn+=analogRead(A0);
    avgOut+=analogRead(A1);
  delay(1);
  }
  inputVoltage=((avgIn/samples)*InVoltDivider);;
  outputVoltage=inputVoltage-((avgOut/samples)*OutVoltDivider);
}

float updown=1;
void everytick(){
  float delta=abs(outputVoltage-lastOut);
  float slew=delta/5;
  if(slew<0.01)
    slew=0.01;
  if(slew>0.05)
    slew=0.05;
  if(outputVoltage<lastOut)
    updown=-updown;

  if(inputVoltage<7.0) //Undervolt protection.
    updown=-1;

  if(PWMValue>=1.0){
    updown=-1;
    PWMValue=1.0;
  }
  else
    if(PWMValue<=0.0){
      updown=1;
      PWMValue=0.0;
    }
  PWMValue+=updown*slew;
  lastOut=outputVoltage;
    
  analogWrite(3, pwmtop*PWMValue);

  Serial.print(inputVoltage);
  Serial.print(" - ");
  Serial.print(PWMValue);
  Serial.print(" - ");
  Serial.print(outputVoltage);
  Serial.print(" - ");
  Serial.println(delta);
}