Arduino PWM-based pressure regulator sketch, now with PID!

README.md

Arduino PWM-based pressure regulator sketch, now with PID!

Here's take 2 of my fuel pump controller, this time controlled via a PID loop.

Create a directory named fuel-pump and place all of these files into it.

clamp.h

// Arduino value clamping.
// Copyright 2021 Jason Pepas.
// Released under the terms of the MIT License, see https://opensource.org/licenses/MIT

#ifndef _CLAMP_H_
#define _CLAMP_H_

float clampf(float value, float min, float max);

#endif

#ifdef _CLAMP_H_IMPLEMENTATION_

float clampf(float value, float min, float max) {
    if (value < min) {
        return min;
    } else if (value > max) {
        return max;
    } else {
        return value;
    }
}

#endif

die.h

// Arduino halt-on-error handling.
// Copyright 2021 Jason Pepas.
// Released under the terms of the MIT License, see https://opensource.org/licenses/MIT

#ifndef _DIE_H_
#define _DIE_H_

void die();

#endif

#ifdef _DIE_H_IMPLEMENTATION_

// Loop forever, indicating to the user there was an unrecoverable error.
void die() {
    #ifdef ARDUINO_AVR_PROMICRO
    pin_t led_pin = 17;
    #else
    pin_t led_pin = 13;
    #endif
    pinMode(led_pin, OUTPUT);
    while(1) {
        digitalWrite(led_pin, HIGH);
        delay(1000);
        digitalWrite(led_pin, LOW);
        delay(1000);
    }
}

#endif

fuel-pump.ino

// Arduino PWM output based on potentiometer ADC input.
// Copyright 2021 Jason Pepas.
// Released under the terms of the MIT License, see https://opensource.org/licenses/MIT

#include <stdint.h>

#define _PRESCALE_H_IMPLEMENTATION_
#include "prescale.h"

#define _PID_H_IMPLEMENTATION_
#include "pid.h"

#define _READ_ADC_H_IMPLEMENTATION_
#include "read_adc.h"

#define _DIE_H_IMPLEMENTATION_
#include "die.h"

#define _CLAMP_H_IMPLEMENTATION_
#include "clamp.h"

// Types:
typedef int pin_t;  // An Arduino pin number.
typedef int adc_t;  // An Arduino 10-bit ADC value (0-1023).
typedef int pwm_t;  // An Arduino PWM output value (0-255).
typedef float psi_t;  // Pounds per square inch.
typedef float volts_t;  // Voltage.
typedef unsigned long micros_t;  // Microseconds as returned by micros().

// Modes of operation:
typedef uint8_t mode_t;
mode_t g_mode = 0;
#define MODE_STOPPED 0
#define MODE_OPEN_LOOP_DUTY 1
#define MODE_OPEN_LOOP_VOLTS 2
#define MODE_CLOSED_LOOP_POT 3
#define MODE_CLOSED_LOOP_KEY 4
#define MODE_TEST_CYCLE_1 5
#define MODE_TEST_CYCLE_2 6

// Pin assignments:
const pin_t g_pot_pin = A9;  // The user input potentiometer pin.
const pin_t g_xducer_pin = A7;  // The pressure transducer input pin.
const pin_t g_pump_vcc_pin = A6;  // The fuel pump voltage-sense pin (12 to 15V).
const pin_t g_pump_pin = 10;  // The PWM'ed fuel pump output pin.
const pin_t g_vcc_pin = A0;

// PID config and state:
pid_t g_pid;

psi_t g_psi_set = 0.0f;
pwm_t g_test1_high_pwm_set = 100;
pwm_t g_test1_low_pwm_set = 65;
micros_t g_time_of_last_measurement = 0;

// Limits:
const psi_t g_pressure_limit = 65.0f;
const volts_t g_volts_limit = 7.0f;

volts_t read_aref() {
    // I used a voltage divider and see an ADC reading of 750 at 4.66V Vcc.
    const float fullscale = 6.356f;

    analogReference(INTERNAL);
    // The first few readings will be inaccurate.
    for (uint8_t i=0; i < 16; i++) {
        analogRead(g_vcc_pin);
    }
    
    volts_t vcc = read_adc(g_vcc_pin, OVERSAMPLE_16x) / 1023.0f * fullscale;

    analogReference(DEFAULT);
    // The first few readings will be inaccurate.
    for (uint8_t i=0; i < 16; i++) {
        analogRead(g_vcc_pin);
    }

    return vcc;
}

psi_t read_pump_vcc(volts_t aref, oversample_t osample) {
    volts_t volts = read_adc(g_pump_vcc_pin, osample) / 1023.0f * aref;
    return volts * 3.0f;  // a 2:1 voltage divider is used to measure pump Vcc.
}

psi_t read_pressure_xducer(volts_t aref, oversample_t osample) {
    volts_t volts = read_adc(g_xducer_pin, osample) / 1023.0f * aref;
    // Transducer produces linear output: 0.5V == 0psi, 4.5V == 100psi.
    // psi_t pressure = (volts - 0.5) / 4.0f * 100.0f;
    // Actual values are a bit tweaked:
    psi_t pressure = (volts - (0.5 - 0.063f)) / (4.0f - 0.2f) * 100.0f;
    return pressure;
}

/*
Cheat sheet:

s: mode = stop.
z: mode = MODE_OPEN_LOOP_DUTY
x: mode = MODE_OPEN_LOOP_VOLTS
c: mode = MODE_CLOSED_LOOP_POT
v: mode = MODE_CLOSED_LOOP_KEY
b: mode = MODE_TEST_CYCLE_1
n: mode = MODE_TEST_CYCLE_2

P: Kp += 10%
p: Kp -= 10%
I: Ki += 10%
i: Ki -= 10%
D: Kd += 10%
d: Kd -= 10%
M: Tau += 10%
m: Tau -= 10%

+: test cycle 1 pwm upper set point += 1
=: test cycle 1 pwm upper set point -= 1
_: test cycle 1 pwm lower set point += 1
-: test cycle 1 pwm lower set point -= 1

`: psi set point = 5
~: psi set point = 10
1: psi set point = 15
2: psi set point = 20
3: psi set point = 25
4: psi set point = 30
5: psi set point = 35
6: psi set point = 40
7: psi set point = 45
8: psi set point = 50
9: psi set point = 55
0: psi set point = 60

Presets calculated using ziegler nichols:
q: PID preset p
w: PID preset pi
e: PID preset pd
r: PID preset 'classic'
t: PID preset 'pessen integral'
y: PID preset 'some overshoot'
u: PID preset 'no overshoot'
o: PID preset 'custom1'
'/': zero-out the integrator

h: 39kHz PWM.
j: 490Hz PWM.
k: 122Hz PWM.
l: 31Hz PWM.

*/

// Handle a single character of user input.
void handle_serial_input(uint8_t ch) {
    switch (ch) {

    case 's':
        // Stop the pump.
        g_mode = MODE_STOPPED;
        break;

    case 'h':
        // Use 39kHz PWM freq.
        mega328_TCCR1B_set_prescale_8();
        break;
    case 'j':
        // Use 490Hz PWM freq (default).
        mega328_TCCR1B_set_prescale_64();
        break;
    case 'k':
        // Use 122Hz PWM freq.
        mega328_TCCR1B_set_prescale_256();
        break;
    case 'l':
        // Use 31Hz PWM freq.
        mega328_TCCR1B_set_prescale_1024();
        break;

    case 'P':
        // Increase Kp by 10%.
        g_pid.Kp *= 1.1f;
        break;
    case 'p':
        // Decrease Kp by 10%.
        g_pid.Kp *= 0.9f;
        break;
    case 'I':
        // Increase Ki by 10%.
        g_pid.Ki *= 1.1f;
        break;
    case 'i':
        // Decrease Ki by 10%.
        g_pid.Ki *= 0.9f;
        break;
    case 'D':
        // Increase Kd by 10%.
        g_pid.Kd *= 1.1f;
        break;
    case 'd':
        // Decrease Kd by 10%.
        g_pid.Kd *= 0.9f;
        break;

    case 'M':
        // Increase tau by 10%.
        g_pid.tau *= 1.1f;
        break;
    case 'm':
        // Decrease tau by 10%.
        g_pid.tau *= 0.9f;
        break;

    case '+':
        g_test1_high_pwm_set += 1;
        break;
    case '=':
        g_test1_high_pwm_set -= 1;
        break;

    case '_':
        g_test1_low_pwm_set += 1;
        break;
    case '-':
        g_test1_low_pwm_set -= 1;
        break;

    case '`':
        g_psi_set = 5.0f;
        break;
    case '~':
        g_psi_set = 10.0f;
        break;
    case '1':
        g_psi_set = 15.0f;
        break;
    case '2':
        g_psi_set = 20.0f;
        break;
    case '3':
        g_psi_set = 25.0f;
        break;
    case '4':
        g_psi_set = 30.0f;
        break;
    case '5':
        g_psi_set = 35.0f;
        break;
    case '6':
        g_psi_set = 40.0f;
        break;
    case '7':
        g_psi_set = 45.0f;
        break;
    case '8':
        g_psi_set = 50.0f;
        break;
    case '9':
        g_psi_set = 55.0f;
        break;
    case '0':
        g_psi_set = 60.0f;
        break;

    case 'z':
        // Start the pump using open-loop potentiometer control.
        // Here, the potentiometer directly controls PWM duty cycle.
        g_mode = MODE_OPEN_LOOP_DUTY;
        break;
    case 'x':
        // Start the pump using open-loop potentiometer control.
        // Here, the potentiometer selects the output voltage.
        g_mode = MODE_OPEN_LOOP_VOLTS;
        break;
    case 'c':
        // Start the pump using closed-loop (PID) potentiometer control.
        // Here, the potentiometer sets the desired PSI, and the PWM duty
        // cycle is controlled using PID.
        g_mode = MODE_CLOSED_LOOP_POT;
        break;
    case 'v':
        // Start the pump using closed-loop (PID) potentiometer control.
        // Here, the desired PSI is selected using keyboard keys, and the
        // PWM duty cycle is controlled using PID.
        g_mode = MODE_CLOSED_LOOP_KEY;
        break;
    case 'b':
        // Start the pump in test cycle #1.
        // Here, the PWM duty cycle alternates between two fixed values.
        g_mode = MODE_TEST_CYCLE_1;
        break;
    case 'n':
        // Start the pump in test cycle #2.
        // Here, the PSI set-point cycles between two fixed values.
        g_mode = MODE_TEST_CYCLE_2;
        break;

    case 'q':
        set_pid_p(&g_pid);
        break;

    case 'w':
        set_pid_pi(&g_pid);
        break;

    case 'e':
        set_pid_pd(&g_pid);
        break;

    case 'r':
        set_pid_classic(&g_pid);
        break;

    case 't':
        set_pid_pessen(&g_pid);
        break;

    case 'y':
        set_pid_some_overshoot(&g_pid);
        break;

    case 'u':
        set_pid_no_overshoot(&g_pid);
        break;

    case 'o':
        set_pid_custom1(&g_pid);
        break;

    case '/':
        g_pid.integrator = 0.0f;
        break;

    }
}

void run_mode_stopped() {
    // Stop the pump.
    analogWrite(g_pump_pin, 0);
    g_pid.integrator = 0.0f;
    Serial.println("Pump stopped.");
    delay(200);
}

// Potentiometer directly sets PWM duty cycle.
void run_mode_open_loop_duty() {
    volts_t aref = read_aref();
    psi_t pressure = read_pressure_xducer(aref, OVERSAMPLE_64x);

    // If the pressure transducer is disconnected, abort.
    const psi_t xducer_disconnected = -5.0f;
    if (pressure <= xducer_disconnected) {
        Serial.println("Error: pressure transducer not connected!");
        g_mode = 0;
        return;
    }

    adc_t current_pot_adc = read_adc(g_pot_pin, OVERSAMPLE_64x);
    // Scale the 0-1023 value to 0-255.
    pwm_t pwm_set = pwm_t(float(current_pot_adc) / 4.0f);

    // If we are over the pressure limit, abort.
    if (pressure >= g_pressure_limit) {
        Serial.println("Error: pressure limit exceeded!");
        g_mode = 0;
        return;
    }

    volts_t xducer = read_adc(g_xducer_pin, OVERSAMPLE_64x) / 1023.0f * aref;
    volts_t vpot = read_adc(g_pot_pin, OVERSAMPLE_64x) / 1023.0f * aref;

    analogWrite(g_pump_pin, pwm_set);

    Serial.print("aref:");
    Serial.print(aref, 3);
    Serial.print(", pwm:");
    Serial.print(pwm_set);
    Serial.print(", psi:");
    Serial.print(pressure, 1);
    Serial.print(", xducer:");
    Serial.print(xducer, 3);
    Serial.print(", vpot:");
    Serial.print(vpot, 3);
    Serial.println();
}

// Potentiometer selects desired voltage 0-7, PWM is calculated.
void run_mode_open_loop_volts() {
    volts_t aref = read_aref();
    psi_t pressure = read_pressure_xducer(aref, OVERSAMPLE_64x);

    // If the pressure transducer is disconnected, abort.
    const psi_t xducer_disconnected = -5.0f;
    if (pressure <= xducer_disconnected) {
        Serial.println("Error: pressure transducer not connected!");
        g_mode = 0;
        return;
    }

    volts_t pump_vcc = read_pump_vcc(aref, OVERSAMPLE_8x);

    volts_t volts_set = read_adc(g_pot_pin, OVERSAMPLE_32x) / 1023.0f * g_volts_limit;
    pwm_t pwm_set = pwm_t((volts_set / pump_vcc) * 255.0f);

    // If we are over the pressure limit, abort.
    if (pressure >= g_pressure_limit) {
        Serial.println("Error: pressure limit exceeded!");
        g_mode = 0;
        return;
    }

    analogWrite(g_pump_pin, pwm_set);

    Serial.print("volts_set: ");
    Serial.print(volts_set, 2);
    Serial.print(", pump_vcc: ");
    Serial.print(pump_vcc, 2);
    Serial.print(", pwm: ");
    Serial.print(pwm_set);
    Serial.print(", psi: ");
    Serial.print(pressure, 1);
    Serial.println();
}

void run_mode_closed_loop(bool use_pot) {
    micros_t now = micros();
    micros_t elapsed_u = now - g_time_of_last_measurement;
    seconds_t elapsed_s = elapsed_u / 1000000.0f;

    const seconds_t period_min = 0.01f;
    if (elapsed_s < period_min) {
        return;
    }

    volts_t aref = read_aref();
    psi_t pressure = read_pressure_xducer(aref, OVERSAMPLE_16x);

    // If the pressure transducer is disconnected, abort.
    const psi_t xducer_disconnected = -5.0f;
    if (pressure <= xducer_disconnected) {
        Serial.println("Error: pressure transducer not connected!");
        g_mode = 0;
        return;
    }

    psi_t target_psi;
    if (use_pot) {
        target_psi = read_adc(g_pot_pin, OVERSAMPLE_16x) / 1023.0f * g_pressure_limit;
    } else {
        target_psi = g_psi_set;
    }

    volts_t calculated_volts = pid_update(&g_pid, target_psi, pressure, elapsed_s);

    volts_t pump_vcc = read_pump_vcc(aref, OVERSAMPLE_8x);

    float pwm_set_f = calculated_volts / pump_vcc * 255.0f;
    pwm_set_f = clampf(pwm_set_f, 0.0f, 255.0f);
    pwm_t pwm_set = pwm_t(pwm_set_f);

    // If we are over the pressure limit, abort.
    if (pressure >= g_pressure_limit) {
        Serial.println("Error: pressure limit exceeded!");
        g_mode = 0;
        return;
    }

    analogWrite(g_pump_pin, pwm_set);

    Serial.print("set: ");
    Serial.print(target_psi, 1);
    Serial.print(", psi: ");
    Serial.print(pressure, 1);
    Serial.print(", pwm: ");
    Serial.print(pwm_set);
    Serial.print(", Kp: ");
    Serial.print(g_pid.Kp, 3);
    Serial.print(", Ki: ");
    Serial.print(g_pid.Ki, 4);
    Serial.print(", Kd: ");
    Serial.print(g_pid.Kd, 4);
    Serial.print(", elapsed: ");
    Serial.print(elapsed_s, 6);
    Serial.println();

    // Set up for next iteration:
    g_time_of_last_measurement = now;
}

void run_mode_test_cycle_1() {
    volts_t aref = read_aref();
    
    psi_t pressure = read_pressure_xducer(aref, OVERSAMPLE_8x);

    // If the pressure transducer is disconnected, abort.
    const psi_t xducer_disconnected = -5.0f;
    if (pressure <= xducer_disconnected) {
        Serial.println("Error: pressure transducer not connected!");
        g_mode = 0;
        return;
    }

    // If we are over the pressure limit, abort.
    if (pressure >= g_pressure_limit) {
        Serial.println("Error: pressure limit exceeded!");
        g_mode = 0;
        return;
    }

    const uint8_t test_phase_high = 0;
    const uint8_t test_phase_low = 1;

    static pwm_t pwm_set = 0;

    micros_t now = micros();
    const micros_t period = 2000000;
    uint8_t current_test_phase = (now / period) % 2;
    switch (current_test_phase) {
    case test_phase_high:
        if (pwm_set != g_test1_high_pwm_set) {
            pwm_set = g_test1_high_pwm_set;
            analogWrite(g_pump_pin, pwm_set);
        }
        break;
    case test_phase_low:
        if (pwm_set != g_test1_low_pwm_set) {
            pwm_set = g_test1_low_pwm_set;
            analogWrite(g_pump_pin, pwm_set);
        }
        break;
    default:
        die();
    }

    Serial.print("pwm: ");
    Serial.print(pwm_set);
    Serial.print(", psi: ");
    Serial.print(pressure, 1);
    Serial.println();
}

void run_mode_test_cycle_2() {
    micros_t now = micros();
    micros_t elapsed_u = now - g_time_of_last_measurement;
    seconds_t elapsed_s = elapsed_u / 1000000.0f;

    const seconds_t period_min = 0.005f;
    if (elapsed_s < period_min) {
        return;
    }

    volts_t aref = read_aref();
    psi_t pressure = read_pressure_xducer(aref, OVERSAMPLE_16x);

    // If the pressure transducer is disconnected, abort.
    const psi_t xducer_disconnected = -5.0f;
    if (pressure <= xducer_disconnected) {
        Serial.println("Error: pressure transducer not connected!");
        g_mode = 0;
        return;
    }

    const uint8_t test_phase_high = 0;
    const uint8_t test_phase_low = 1;

    static psi_t target_psi = 0;
    const psi_t high_psi_set = 40;
    const psi_t low_psi_set = 20;

    const micros_t period = 2000000;
    uint8_t current_test_phase = (now / period) % 2;
    switch (current_test_phase) {
    case test_phase_high:
        if (target_psi != high_psi_set) {
            target_psi = high_psi_set;
        }
        break;
    case test_phase_low:
        if (target_psi != low_psi_set) {
            target_psi = low_psi_set;
        }
        break;
    default:
        die();
    }

    volts_t calculated_volts = pid_update(&g_pid, target_psi, pressure, elapsed_s);

    volts_t pump_vcc = read_pump_vcc(aref, OVERSAMPLE_8x);

    float pwm_set_f = calculated_volts / pump_vcc * 255.0f;
    pwm_set_f = clampf(pwm_set_f, 0.0f, 255.0f);
    pwm_t pwm_set = pwm_t(pwm_set_f);

    // If we are over the pressure limit, abort.
    if (pressure >= g_pressure_limit) {
        Serial.println("Error: pressure limit exceeded!");
        g_mode = 0;
        return;
    }

    analogWrite(g_pump_pin, pwm_set);

    Serial.print("s:");
    Serial.print(target_psi, 1);
    Serial.print(", psi:");
    Serial.print(pressure, 1);
    Serial.print(", V:");
    Serial.print(calculated_volts, 2);
    Serial.print(", pwm:");
    Serial.print(pwm_set);
    Serial.print(",\tKp:");
    Serial.print(g_pid.Kp, 3);
    Serial.print(", Ki:");
    Serial.print(g_pid.Ki, 3);
    Serial.print(", Kd:");
    Serial.print(g_pid.Kd, 3);
    Serial.print(", t:");
    Serial.print(elapsed_s, 4);
    Serial.print(", i:");
    Serial.print(g_pid.integrator, 2);
    Serial.print(", tau:");
    Serial.print(g_pid.tau, 3);
    Serial.println();

    // Set up for next iteration:
    g_time_of_last_measurement = now;
}

void set_pid_p(pid_t* pid) {
    pid->Kp = 0.25f;
    pid->Ki = 0.0f;
    pid->Kd = 0.0f;
    pid->integrator = 0.0f;
}

void set_pid_pi(pid_t* pid) {
    pid->Kp = 0.225f;
    pid->Ki = 2.03f;
    pid->Kd = 0.0f;
    pid->integrator = 0.0f;
}

void set_pid_pd(pid_t* pid) {
    pid->Kp = 0.4f;
    pid->Ki = 0.0f;
    pid->Kd = 0.0066f;
    pid->integrator = 0.0f;
}

void set_pid_classic(pid_t* pid) {
    pid->Kp = 0.3f;
    pid->Ki = 4.5f;
    pid->Kd = 0.005f;
    pid->integrator = 0.0f;
}

void set_pid_pessen(pid_t* pid) {
    pid->Kp = 0.35f;
    pid->Ki = 6.57f;
    pid->Kd = 0.007f;
    pid->integrator = 0.0f;
}

void set_pid_some_overshoot(pid_t* pid) {
    pid->Kp = 0.165f;
    pid->Ki = 2.48f;
    pid->Kd = 0.007f;
    pid->integrator = 0.0f;
}

void set_pid_no_overshoot(pid_t* pid) {
    pid->Kp = 0.1f;
    pid->Ki = 1.5f;
    pid->Kd = 0.004f;
    pid->integrator = 0.0f;
}

void set_pid_custom1(pid_t* pid) {
    pid->Kp = 0.23f;
    pid->Ki = 1.4f;
    pid->Kd = 0.02f;
    pid->tau = 0.03f;
    pid->integrator = 0.0f;
}

void setup() {
    pinMode(g_pot_pin, INPUT);
    pinMode(g_xducer_pin, INPUT);
    pinMode(g_pump_vcc_pin, INPUT);
    pinMode(g_vcc_pin, INPUT);

    pinMode(g_pump_pin, OUTPUT);

    // Use 39kHz PWM freq.
    mega328_TCCR1B_set_prescale_8();

    pid_init(&g_pid);
    g_pid.output_min = 0.0f;
    g_pid.output_max = g_volts_limit;
    set_pid_custom1(&g_pid);

    Serial.begin(115200);
    while (!Serial) {
        ; // Wait for serial port to connect.  Needed for native USB port only.
    }
    Serial.println();
    Serial.println("PWM EFI fuel pump controller starting.");
}

void loop() {
    if (Serial.available()) {
        uint8_t ch = Serial.read();
        handle_serial_input(ch);
        Serial.print("Mode: ");
        Serial.println(g_mode);
    } else {
        switch (g_mode) {
        case MODE_STOPPED:
            run_mode_stopped();
            break;
        case MODE_OPEN_LOOP_DUTY:
            run_mode_open_loop_duty();
            break;
        case MODE_OPEN_LOOP_VOLTS:
            run_mode_open_loop_volts();
            break;
        case MODE_CLOSED_LOOP_POT:
            run_mode_closed_loop(true);
            break;
        case MODE_CLOSED_LOOP_KEY:
            run_mode_closed_loop(false);
            break;
        case MODE_TEST_CYCLE_1:
            run_mode_test_cycle_1();
            break;
        case MODE_TEST_CYCLE_2:
            run_mode_test_cycle_2();
            break;
        }
    }
}

pid.h

// This PID implementation is modified from "Phil's Lab", which is MIT licensed.
// See https://github.com/pms67/PID
// This modified version is also MIT licensed.
// See see https://opensource.org/licenses/MIT

#ifndef _PID_H_
#define _PID_H_

#include "clamp.h"

struct _pid_t {
    // Controller gains.
    float Kp;
    float Ki;
    float Kd;

    // Derivative low-pass filter time constant.
    float tau;

    // Output limits.
    // Integrator does not wind up when output is clamped.
    float output_min;
    float output_max;

    // Controller "memory".
    float integrator;
    float prev_error;  // Required for integrator.
    float differentiator;
    float prev_measurement;  // Required for differentiator.
};
typedef struct _pid_t pid_t;

typedef float seconds_t;

void pid_init(pid_t *pid);
float pid_update(pid_t *pid, float setpoint, float measurement, seconds_t period);

#endif

#ifdef _PID_H_IMPLEMENTATION_

void pid_init(pid_t *pid) {
    pid->integrator = 0.0f;
    pid->prev_error  = 0.0f;
    pid->differentiator  = 0.0f;
    pid->prev_measurement = 0.0f;
}

float pid_update(pid_t *pid, float setpoint, float measurement, seconds_t period) {
    float error = setpoint - measurement;

    float proportional = pid->Kp * error;

    float previous_integrator = pid->integrator;
    pid->integrator = pid->integrator + 0.5f * pid->Ki * period * (error + pid->prev_error);

    // Derivative (band-limited differentiator)
    // Note: derivative on measurement, therefore minus sign in front of equation!
    pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prev_measurement)
                        + (2.0f * pid->tau - period) * pid->differentiator)
                        / (2.0f * pid->tau + period);

    // Store error and measurement for later use.
    pid->prev_error = error;
    pid->prev_measurement = measurement;

    float output = proportional + pid->integrator + pid->differentiator;
    if (output < pid->output_min || output > pid->output_max) {
        // Do not wind-up the integrator while the output is clamped.
        pid->integrator = previous_integrator;
    }
    output = clampf(output, pid->output_min, pid->output_max);
    return output;
}

#endif

prescale.h

// Arduino PWM prescale adjustment functions.
// Copyright 2021 Jason Pepas.
// Released under the terms of the MIT License, see https://opensource.org/licenses/MIT

#ifndef _PRESCALE_H_
#define _PRESCALE_H_

void mega328_TCCR1B_set_prescale_1();
void mega328_TCCR1B_set_prescale_8();
void mega328_TCCR1B_set_prescale_64();
void mega328_TCCR1B_set_prescale_256();
void mega328_TCCR1B_set_prescale_1024();

#endif

#ifdef _PRESCALE_H_IMPLEMENTATION_

void mega328_TCCR1B_set_prescale_1() {
    // Set CS12-CS10 to 001.
    volatile uint8_t reg;
    cli();
    reg = TCCR1B;
    reg &= ~(1 << CS12);
    reg &= ~(1 << CS11);
    reg |= (1 << CS10);
    TCCR1B = reg;
    sei();
}

void mega328_TCCR1B_set_prescale_8() {
    // Set CS12-CS10 to 010.
    volatile uint8_t reg;
    cli();
    reg = TCCR1B;
    reg &= ~(1 << CS12);
    reg |= (1 << CS11);
    reg &= ~(1 << CS10);
    TCCR1B = reg;
    sei();
}

void mega328_TCCR1B_set_prescale_64() {
    // Set CS12-CS10 to 011.
    volatile uint8_t reg;
    cli();
    reg = TCCR1B;
    reg &= ~(1 << CS12);
    reg |= (1 << CS11);
    reg |= (1 << CS10);
    TCCR1B = reg;
    sei();
}

void mega328_TCCR1B_set_prescale_256() {
    // Set CS12-CS10 to 100.
    volatile uint8_t reg;
    cli();
    reg = TCCR1B;
    reg |= (1 << CS12);
    reg &= ~(1 << CS11);
    reg &= ~(1 << CS10);
    TCCR1B = reg;
    sei();
}

void mega328_TCCR1B_set_prescale_1024() {
    // Set CS12-CS10 to 101.
    volatile uint8_t reg;
    cli();
    reg = TCCR1B;
    reg |= (1 << CS12);
    reg &= ~(1 << CS11);
    reg |= (1 << CS10);
    TCCR1B = reg;
    sei();
}

#endif

read_adc.h

// Arduino ADC oversampling.
// Copyright 2021 Jason Pepas.
// Released under the terms of the MIT License, see https://opensource.org/licenses/MIT

#ifndef _READ_ADC_H
#define _READ_ADC_H

#include "die.h"

typedef int pin_t;  // An Arduino pin number.
typedef int adc_t;  // An Arduino 10-bit ADC value (0-1023).

typedef uint8_t oversample_t;  // The degree of oversampling (e.g. 16x).
#define OVERSAMPLE_1x 1
#define OVERSAMPLE_2x 2
#define OVERSAMPLE_4x 4
#define OVERSAMPLE_8x 8
#define OVERSAMPLE_16x 16
#define OVERSAMPLE_32x 32
#define OVERSAMPLE_64x 64

uint8_t shift_value_for(oversample_t oversample);
adc_t read_adc(pin_t sensor_pin, oversample_t oversample);

#endif

#ifdef _READ_ADC_H_IMPLEMENTATION_

// Returns the shift value corresponding to the oversampling rate.
// Usage: accumulator >>= shift_value_for(OVERSAMPLE_16x);
uint8_t shift_value_for(oversample_t oversample) {
    switch (oversample) {
        case 1: return 0;
        case 2: return 1;
        case 4: return 2;
        case 8: return 3;
        case 16: return 4;
        case 32: return 5;
        case 64: return 6;
        default: die();
    }
}

// ADC read with oversampling.
// Usage: adc_t value = read_adc(my_sensor_pin, OVERSAMPLE_16x);
adc_t read_adc(pin_t sensor_pin, oversample_t oversample) {
    // Note: arduino analogRead takes about 100us (10kHz) according to
    // http://yaab-arduino.blogspot.com/2015/02/fast-sampling-from-analog-input.html
    // An oversample rate of 64x fits 10-bit values neatly into a 16-bit unsigned accumulator, at about 150Hz.
    uint16_t accumulator = 0;
    for (uint8_t i=0; i < oversample; i++) {
        int reading = analogRead(sensor_pin);
        accumulator += reading;
    }
    accumulator >>= shift_value_for(oversample);
    return (adc_t)accumulator;
}

#endif