Here is the code for the ATMEGA328PB

Here is the code for the ATMEGA328PB

#include <avr/io.h>
#define F_CPU 20000000UL // Use external 20 MHz clock, set to external oscillator in the fuses
#include <util/delay.h>

const uint8_t MAX_CAP_VOLTAGE = 145; // Assume output of cap is more or less at ADC
const uint8_t START_CAP_VOLTAGE = 130; // When to start charging - hysteresis

const uint8_t COUNT_FET_DEFAULT = 0; // When starting to charge, the lower FET on time
const uint8_t COUNT_LOW_DEFAULT = 200; // The starting count for all switches off

// Enumerated type for the state machine
typedef enum {IDLE, CHARGING, STOP} state_var;

int main(void)
{
    // Variable to hold the state
    volatile state_var the_state = IDLE;
    
    // The low side on time
    uint8_t count_fet = COUNT_FET_DEFAULT;
    
    // The idle time when everything is low
    uint8_t count_low = COUNT_LOW_DEFAULT;
    
    // PB0 for V high (high side)
    // PB1 for V low (low side)
    // PB2 for trigger
    DDRB = 7;
    PORTB = 0;
    
    //DDRD = 255 - (1 << 5);
    
    // Setup the analog comparator, set bandgap reference (instead of AIN0)
    ACSR = (1 << 6);
    
    // Setup the ADC (use AVCC, left adjust, and ADC0 as the input)
    ADMUX = (1 << 6) + (1 << 5) + 0;
    
    // Enable ADC, start it up, auto trigger, divide by 128 for prescalar
    ADCSRA = (1 << 7) + (1 << 6) + (1 << 5) + 7;
    
    // Free running mode
    ADCSRB = 0;
    
    // At start, just wait in case of issues with programming
    _delay_ms(3000);
        
    while (1) {
        
        if ((PIND & (1 << 5)) == 0) {
            the_state = STOP;
        }
                
        if (the_state == IDLE) {  // Idle state
            
            // Wait to let the ADC readings settle
            _delay_ms(3);
            
            if (ADCH < START_CAP_VOLTAGE) { // Begin charging
                // Initialize lower FET on time
                count_fet = COUNT_FET_DEFAULT;
                count_low = COUNT_LOW_DEFAULT;
                
                // Update state variable
                the_state = CHARGING;
            }
        }
        else if (the_state == CHARGING) { // Charging state
            
            for (uint8_t qq = 0; qq < 128; qq++) { // Cycles of current control
                // Charging loop
                // Turn on the high side FET
                PORTB = 1 + 4;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                PORTB = 1;
                                        
                // Turn off the high side FET, prevent shoot-through
                PORTB = 0;
                PORTB = 0;
                PORTB = 0;
                
                // Turn on the low side FET
                for (uint8_t jj = 0; jj < count_fet; jj++) {
                    PORTB = 2;
                }
                
                // Turn off the low side FET, all low
                PORTB = 0;                
                for (uint8_t jj = 0; jj < count_low; jj++) {
                    PORTB = 0;
                }
                        
                if ((ACSR & (1 << 5)) == 0) { // Current too high                    
                    if (count_fet < 255) {
                        count_fet = count_fet + 1;
                    }                    
                }
                else { // Current too low
                    if (count_fet > 0) {
                        count_fet = count_fet - 1;                    
                    }
                
                    if (count_low > 0) {
                        count_low = count_low - 1;
                    }
                }
            }
            if (ADCH > MAX_CAP_VOLTAGE) {
                // Done charging, set to IDLE
                the_state = IDLE;
            }                                    
        }    
        else if (the_state == STOP) {
            PORTB = 0;
            _delay_ms(1);
            if ((PIND & (1 << 5)) == (1 << 5)) {
                the_state = IDLE;
            }
        }                
    }
}