7. ATmega328P Code

// Configuration and initialization of the analog-to-digital converter:
#include "ADC.hpp"
// Capacitive touch sensing:
#include "Touch.hpp"
// PID controller:
#include "Controller.hpp"
// Configuration of PWM and Timer2/0 for driving the motor:
#include "Motor.hpp"
// Reference signal for testing the performance of the controller:
#include "Reference.hpp"
// Helpers for low-level AVR Timer2/0 and ADC registers:
#include "Registers.hpp"
// Parsing incoming messages over Serial using SLIP packets:
#include "SerialSLIP.hpp"

#include <Arduino.h>         // setup, loop, analogRead
#include <Arduino_Helpers.h> // EMA.hpp
#include <Wire.h>            // I²C slave

#include "SMA.hpp"            // SMA filter
#include <AH/Filters/EMA.hpp> // EMA filter

// ------------------------------ Description ------------------------------- //

// This sketch drives up to four motorized faders using a PID controller. The
// motor is disabled when the user touches the knob of the fader.
//
// Everything is driven by Timer2, which runs (by default) at a rate of
// 31.250 kHz. This high rate is used to eliminate audible tones from the PWM
// drive for the motor. Timer0 is used for the PWM outputs of faders 3 and 4.
// Every 30 periods of Timer2 (960 µs), each analog input is sampled, and
// this causes the PID control loop to run in the main loop function.
// Capacitive sensing is implemented by measuring the RC time on the touch pin
// in the Timer2 interrupt handler. The “touched” status is sticky for >20 ms
// to prevent interference from the 50 Hz mains.
//
// There are options to (1) follow a test reference (with ramps and jumps), (2)
// to receive a target position over I²C, or (3) to run experiments based on
// commands received over the serial port. The latter is used by a Python script
// that performs experiments with different tuning parameters for the
// controllers.

// -------------------------------- Hardware -------------------------------- //

// Fader 0:
//  - A0:  wiper of the potentiometer          (ADC0)
//  - D8:  touch pin of the knob               (PB0)
//  - D2:  input 1A of L293D dual H-bridge 1   (PD2)
//  - D3:  input 2A of L293D dual H-bridge 1   (OC2B)
//
// Fader 1:
//  - A1:  wiper of the potentiometer          (ADC1)
//  - D9:  touch pin of the knob               (PB1)
//  - D7:  input 3A of L293D dual H-bridge 1   (PD7)
//  - D11: input 4A of L293D dual H-bridge 1   (OC2A)
//
// Fader 2:
//  - A2:  wiper of the potentiometer          (ADC2)
//  - D10: touch pin of the knob               (PB2)
//  - D4:  input 1A of L293D dual H-bridge 2   (PD4)
//  - D5:  input 2A of L293D dual H-bridge 2   (OC0B)
//
// Fader 3:
//  - A3:  wiper of the potentiometer          (ADC3)
//  - D12: touch pin of the knob               (PB4)
//  - D13: input 3A of L293D dual H-bridge 2   (PB5)
//  - D6:  input 4A of L293D dual H-bridge 2   (OC0A)
//
// If fader 1 is unused:
//  - D13: LED or scope as overrun indicator   (PB5)
//
// For communication:
//  - D0:  UART TX                             (TXD)
//  - D1:  UART RX                             (RXD)
//  - A4:  I²C data                            (SDA)
//  - A5:  I²C clock                           (SCL)
//
// Connect the outer connections of the potentiometers to ground and Vcc, it's
// recommended to add a 100 nF capacitor between each wiper and ground.
// Connect the 1,2EN and 3,4EN enable pins of the L293D chips to Vcc.
// Connect a 500kΩ pull-up resistor between each touch pin and Vcc.
// On an Arduino Nano, you can set an option to use pins A6/A7 instead of A2/A3.
// Note that D13 is often pulsed by the bootloader, which might cause the fader
// to move when resetting the Arduino. You can either disable this behavior in
// the bootloader, or use a different pin (e.g. A2 or A3 on an Arduino Nano).
// The overrun indicator is only enabled if the number of faders is less than 4,
// because it conflicts with the motor driver pin of Fader 1. You can choose a
// different pin instead.

// ----------------------------- Configuration ------------------------------ //

// Enable MIDI input/output.
#define WITH_MIDI 1
// Print to the Serial monitor instead of sending actual MIDI messages.
#define MIDI_DEBUG 0

struct Config {
    // Print the control loop and interrupt frequencies to Serial at startup:
    static constexpr bool print_frequencies = true;
    // Print the setpoint, actual position and control signal to Serial.
    // Note that this slows down the control loop significantly, it probably
    // won't work if you are using more than one fader without increasing
    // `interrupt_divisor`:
    static constexpr bool print_controller_signals = false;
    static constexpr uint8_t controller_to_print = 0;
    // Follow the test reference trajectory (true) or receive the target
    // position over I²C or Serial (false):
    static constexpr bool test_reference = false;
    // Increase this divisor to slow down the test reference:
    static constexpr uint8_t test_reference_speed_div = 4;
    // Allow control for tuning and starting experiments over Serial:
    static constexpr bool serial_control = false;
    // I²C slave address (zero to disable I²C):
    static constexpr uint8_t i2c_address = 8;
    // The baud rate to use for the Serial interface (e.g. for MIDI_DEBUG,
    // print_controller_signals, serial_control, etc.)
    static constexpr uint32_t serial_baud_rate = 1000000;
    // The baud rate to use for MIDI over Serial.
    // Use 31'250 for MIDI over 5-pin DIN, HIDUINO/USBMidiKliK.
    // Hairless MIDI uses 115'200 by default.
    // The included python/SerialMIDI.py script uses 1'000'000.
    static constexpr uint32_t midi_baud_rate = serial_baud_rate;

    // Number of faders, must be between 1 and 4:
    static constexpr size_t num_faders = 1;
    // Actually drive the motors. If set to false, runs all code as normal, but
    // doesn't turn on the motors.
    static constexpr bool enable_controller = true;
    // Use analog pins (A0, A1, A6, A7) instead of (A0, A1, A2, A3), useful for
    // saving digital pins on an Arduino Nano:
    static constexpr bool use_A6_A7 = true;
    // Use pin A2 instead of D13 as the motor driver pin for the fourth fader.
    // Allows D13 to be used as overrun indicator, and avoids issues with the
    // bootloader blinking the LED.
    // Can only be used if `use_A6_A7` is set to true.
    static constexpr bool fader_3_A2 = true;
    // Change the setpoint to the current position when touching the knob.
    // Useful if your DAW does not send any feedback when manually moving the
    // fader.
    static constexpr bool touch_to_current_position = true;

    // Capacitive touch sensing RC time threshold.
    // Increase this time constant if the capacitive touch sense is too
    // sensitive or decrease it if it's not sensitive enough:
    static constexpr float touch_rc_time_threshold = 100e-6; // seconds
    // Bit masks of the touch pins (must be on port B):
    static constexpr uint8_t touch_masks[] = {1 << PB0, 1 << PB1, 1 << PB2,
                                              1 << PB4};

    // Use phase-correct PWM (true) or fast PWM (false), this determines the
    // timer interrupt frequency, prefer phase-correct PWM with prescaler 1 on
    // 16 MHz boards, and fast PWM with prescaler 1 on 8 MHz boards, both result
    // in a PWM and interrupt frequency of 31.250 kHz
    // (fast PWM is twice as fast):
    static constexpr bool phase_correct_pwm = true;
    // The fader position will be sampled once per `interrupt_divisor` timer
    // interrupts, this determines the sampling frequency of the control loop.
    // Some examples include 20 → 320 µs, 30 → 480 µs, 60 → 960 µs,
    // 90 → 1,440 µs, 124 → 2,016 µs, 188 → 3,008 µs, 250 → 4,000 µs.
    // 60 is the default, because it works with four faders. If you only use
    // a single fader, you can go as low as 20 because you only need a quarter
    // of the computations and ADC time:
    static constexpr uint8_t interrupt_divisor = 60 / (1 + phase_correct_pwm);
    // The prescaler for the timer, affects PWM and control loop frequencies:
    static constexpr unsigned prescaler_fac = 1;
    // The prescaler for the ADC, affects speed of analog readings:
    static constexpr uint8_t adc_prescaler_fac = 64;

    // Turn off the motor after this many seconds of inactivity:
    static constexpr float timeout = 2;

    // EMA filter factor for fader position filters:
    static constexpr uint8_t adc_ema_K = 2;
    // SMA filter length for setpoint filters, improves tracking of ramps if the
    // setpoint changes in steps (e.g. when the DAW only updates the reference
    // every 20 ms). Powers of two are significantly faster (e.g. 32 works well):
    static constexpr uint8_t setpoint_sma_length = 0;

    // ------------------------ Computed Quantities ------------------------- //

    // Sampling time of control loop:
    constexpr static float Ts = 1. * prescaler_fac * interrupt_divisor * 256 *
                                (1 + phase_correct_pwm) / F_CPU;
    // Frequency at which the interrupt fires:
    constexpr static float interrupt_freq =
        1. * F_CPU / prescaler_fac / 256 / (1 + phase_correct_pwm);
    // Clock speed of the ADC:
    constexpr static float adc_clock_freq = 1. * F_CPU / adc_prescaler_fac;
    // Pulse pin D13 if the control loop took too long:
    constexpr static bool enable_overrun_indicator =
        num_faders < 4 || fader_3_A2;

    static_assert(0 < num_faders && num_faders <= 4,
                  "At most four faders supported");
    static_assert(use_A6_A7 || !fader_3_A2,
                  "Cannot use A2 for motor driver "
                  "and analog input at the same time");
    static_assert(!WITH_MIDI || !serial_control,
                  "Cannot use MIDI and Serial control at the same time");
    static_assert(!WITH_MIDI || !print_controller_signals,
                  "Cannot use MIDI while printing controller signals");
};
constexpr uint8_t Config::touch_masks[];
constexpr float Ts = Config::Ts;

// ----------------- ADC, Capacitive Touch State and Motors ----------------- //

ADCManager<Config> adc;
TouchSense<Config> touch;
Motors<Config> motors;

// ------------------------ Setpoints and References ------------------------ //

// Setpoints (target positions) for all faders:
Reference<Config> references[Config::num_faders];

// ------------------------------ Controllers ------------------------------- //

// The main PID controllers. Need tuning for your specific setup:

PID controllers[] {
    // This is an example of a controller with very little overshoot
    {
        6,     // Kp: proportional gain
        2,     // Ki: integral gain
        0.035, // Kd: derivative gain
        Ts,    // Ts: sampling time
        60, // fc: cutoff frequency of derivative filter (Hz), zero to disable
    },
    // This one has more overshoot, but less ramp tracking error
    {
        4,     // Kp: proportional gain
        11,    // Ki: integral gain
        0.028, // Kd: derivative gain
        Ts,    // Ts: sampling time
        40, // fc: cutoff frequency of derivative filter (Hz), zero to disable
    },
    // This is a very aggressive controller
    {
        8.55,  // Kp: proportional gain
        440,   // Ki: integral gain
        0.043, // Kd: derivative gain
        Ts,    // Ts: sampling time
        70, // fc: cutoff frequency of derivative filter (Hz), zero to disable
    },
    // Fourth controller
    {
        6,     // Kp: proportional gain
        2,     // Ki: integral gain
        0.035, // Kd: derivative gain
        Ts,    // Ts: sampling time
        60, // fc: cutoff frequency of derivative filter (Hz), zero to disable
    },
};

// ---------------------------------- MIDI ---------------------------------- //

#if WITH_MIDI
#include <Control_Surface.h>

#if MIDI_DEBUG
USBDebugMIDI_Interface midi {Config::serial_baud_rate};
#else
HardwareSerialMIDI_Interface midi {Serial, Config::midi_baud_rate};
#endif

template <uint8_t Idx>
void sendMIDIMessages(bool touched) {
    // Don't send if the UART buffer is (almost) full
    if (Serial.availableForWrite() < 6) return;
    // Touch
    static bool prevTouched = false; // Whether the knob is being touched
    if (touched != prevTouched) {
        const MIDIAddress addr = MCU::FADER_TOUCH_1 + Idx;
        touched ? midi.sendNoteOn(addr, 127) : midi.sendNoteOff(addr, 127);
        prevTouched = touched;
    }
    // Position
    static Hysteresis<6 - Config::adc_ema_K, uint16_t, uint16_t> hyst;
    if (prevTouched && hyst.update(adc.readFiltered(Idx))) {
        auto value = AH::increaseBitDepth<14, 10, uint16_t>(hyst.getValue());
        midi.sendPitchBend(MCU::VOLUME_1 + Idx, value);
    }
}

void updateMIDISetpoint(ChannelMessage msg) {
    auto type = msg.getMessageType();
    auto channel = msg.getChannel().getRaw();
    if (type == MIDIMessageType::PITCH_BEND && channel < Config::num_faders)
        references[channel].setMasterSetpoint(msg.getData14bit() >> 4);
}

void initMIDI() { midi.begin(); }

void updateMIDI() {
    while (1) {
        auto evt = midi.read();
        if (evt == MIDIReadEvent::NO_MESSAGE)
            break;
        else if (evt == MIDIReadEvent::CHANNEL_MESSAGE)
            updateMIDISetpoint(midi.getChannelMessage());
    }
}

#endif

// ---------------- Printing all signals for serial plotter ----------------- //

template <uint8_t Idx>
void printControllerSignals(int16_t setpoint, int16_t adcval, int16_t control) {
    // Send (binary) controller signals over Serial to plot in Python
    if (Config::serial_control && references[Idx].experimentInProgress()) {
        const int16_t data[3] {setpoint, adcval, control};
        SLIPSender(Serial).writePacket(reinterpret_cast<const uint8_t *>(data),
                                       sizeof(data));
    }
    // Print signals as text
    else if (Config::print_controller_signals &&
             Idx == Config::controller_to_print) {
        Serial.print(setpoint);
        Serial.print('\t');
        Serial.print(adcval);
        Serial.print('\t');
        Serial.print((control + 256) * 2);
        Serial.println();
    }
}

// ----------------------------- Control logic ------------------------------ //

template <uint8_t Idx>
void updateController(uint16_t setpoint, int16_t adcval, bool touched) {
    auto &controller = controllers[Idx];

    // Prevent the motor from being turned off after being touched
    if (touched) controller.resetActivityCounter();

    // Set the target position
    if (Config::setpoint_sma_length > 0) {
        static SMA<Config::setpoint_sma_length, uint16_t, uint32_t> sma;
        uint16_t filtsetpoint = sma(setpoint);
        controller.setSetpoint(filtsetpoint);
    } else {
        controller.setSetpoint(setpoint);
    }

    // Update the PID controller to get the control action
    int16_t control = controller.update(adcval);

    // Apply the control action to the motor
    if (Config::enable_controller) {
        if (touched) // Turn off motor if knob is touched
            motors.setSpeed<Idx>(0);
        else
            motors.setSpeed<Idx>(control);
    }

    // Change the setpoint as we move
    if (Config::touch_to_current_position && touched)
        references[Idx].setMasterSetpoint(adcval);

#if WITH_MIDI
    sendMIDIMessages<Idx>(touched);
#else
    printControllerSignals<Idx>(controller.getSetpoint(), adcval, control);
#endif
}

template <uint8_t Idx>
void readAndUpdateController() {
    // Read the ADC value for the given fader:
    int16_t adcval = adc.read(Idx);
    // If the ADC value was updated by the ADC interrupt, run the control loop:
    if (adcval >= 0) {
        // Check if the fader knob is touched
        bool touched = touch.read(Idx);
        // Read the target position
        uint16_t setpoint = references[Idx].getNextSetpoint();
        // Run the control loop
        updateController<Idx>(setpoint, adcval, touched);
        // Write -1 so the controller doesn't run again until the next value is
        // available:
        adc.write(Idx, -1);
        if (Config::enable_overrun_indicator)
            cbi(PORTB, 5); // Clear overrun indicator
    }
}

// ------------------------------ Setup & Loop ------------------------------ //

void onRequest();
void onReceive(int);
void updateSerialIn();

void setup() {
    // Initialize some globals
    for (uint8_t i = 0; i < Config::num_faders; ++i) {
        // all fader positions for the control loop start of as -1 (no reading)
        adc.write(i, -1);
        // reset the filter to the current fader position to prevent transients
        adc.writeFiltered(i, analogRead(adc.channel_index_to_mux_address(i)));
        // after how many seconds of not touching the fader and not changing
        // the reference do we turn off the motor?
        controllers[i].setActivityTimeout(Config::timeout);
    }

    // Configure the hardware
    if (Config::enable_overrun_indicator) sbi(DDRB, 5); // Pin 13 output

#if WITH_MIDI
    initMIDI();
#else
    if (Config::print_frequencies || Config::print_controller_signals ||
        Config::serial_control)
        Serial.begin(Config::serial_baud_rate);
#endif

    adc.begin();
    touch.begin();
    motors.begin();

    // Print information to the serial monitor or legends to the serial plotter
    if (Config::print_frequencies) {
        Serial.println();
        Serial.print(F("Interrupt frequency (Hz): "));
        Serial.println(Config::interrupt_freq);
        Serial.print(F("Controller sampling time (µs): "));
        Serial.println(Config::Ts * 1e6);
        Serial.print(F("ADC clock rate (Hz): "));
        Serial.println(Config::adc_clock_freq);
        Serial.print(F("ADC sampling rate (Sps): "));
        Serial.println(adc.adc_rate);
    }
    if (Config::print_controller_signals) {
        Serial.println();
        Serial.println(F("Reference\tActual\tControl\t-"));
        Serial.println(F("0\t0\t0\t0\r\n0\t0\t0\t1024"));
    }

    // Initalize I²C slave and attach callbacks
    if (Config::i2c_address) {
        Wire.begin(Config::i2c_address);
        Wire.onRequest(onRequest);
        Wire.onReceive(onReceive);
    }

    // Enable Timer2 overflow interrupt, this starts reading the touch sensitive
    // knobs and sampling the ADC, which causes the controllers to run in the
    // main loop
    sbi(TIMSK2, TOIE2);
}

void loop() {
    if (Config::num_faders > 0) readAndUpdateController<0>();
    if (Config::num_faders > 1) readAndUpdateController<1>();
    if (Config::num_faders > 2) readAndUpdateController<2>();
    if (Config::num_faders > 3) readAndUpdateController<3>();
#if WITH_MIDI
    updateMIDI();
#else
    if (Config::serial_control) updateSerialIn();
#endif
}

// ------------------------------- Interrupts ------------------------------- //

// Fires at a constant rate of `interrupt_freq`.
ISR(TIMER2_OVF_vect) {
    // We don't have to take all actions at each interupt, so keep a counter to
    // know when to take what actions.
    static uint8_t counter = 0;

    adc.update(counter);
    touch.update(counter);

    ++counter;
    if (counter == Config::interrupt_divisor) counter = 0;
}

// Fires when the ADC measurement is complete. Stores the reading, both before
// and after filtering (for the controller and for user input respectively).
ISR(ADC_vect) { adc.complete(); }

// ---------------------------------- Wire ---------------------------------- //

// Send the touch status and filtered fader positions to the master.
void onRequest() {
    uint8_t touched = 0;
    for (uint8_t i = 0; i < Config::num_faders; ++i)
        touched |= touch.touched[i] << i;
    Wire.write(touched);
    for (uint8_t i = 0; i < Config::num_faders; ++i) {
        uint16_t filt_read = adc.readFiltered14ISR(i);
        Wire.write(reinterpret_cast<const uint8_t *>(&filt_read), 2);
    }
}

// Change the setpoint of the given fader based on the value in the message
// received from the master.
void onReceive(int count) {
    if (count < 2) return;
    if (Wire.available() < 2) return;
    uint16_t data = Wire.read();
    data |= uint16_t(Wire.read()) << 8;
    uint8_t idx = data >> 12;
    data &= 0x03FF;
    if (idx < Config::num_faders) references[idx].setMasterSetpoint(data);
}

// ---------------------------------- Serial -------------------------------- //

// Read SLIP messages from the serial port that allow dynamically updating the
// tuning of the controllers. This is used by the Python tuning script.
//
// Message format: <command> <fader> <value>
// Commands:
//   - p: proportional gain Kp
//   - i: integral gain Ki
//   - d: derivative gain Kd
//   - c: derivative filter cutoff frequency f_c (Hz)
//   - m: maximum absolute control output
//   - s: start an experiment, using getNextExperimentSetpoint
// Fader index: up to four faders are addressed using the characters '0' - '3'.
// Values: values are sent as 32-bit little Endian floating point numbers.
//
// For example the message 'c0\x00\x00\x20\x42' sets the derivative filter
// cutoff frequency of the first fader to 40.

void updateSerialIn() {
    static SLIPParser parser;
    static char cmd = '\0';
    static uint8_t fader_idx = 0;
    static uint8_t buf[4];
    static_assert(sizeof(buf) == sizeof(float), "");
    // This function is called if a new byte of the message arrives:
    auto on_char_receive = [&](char new_byte, size_t index_in_packet) {
        if (index_in_packet == 0)
            cmd = new_byte;
        else if (index_in_packet == 1)
            fader_idx = new_byte - '0';
        else if (index_in_packet < 6)
            buf[index_in_packet - 2] = new_byte;
    };
    // Convert the 4-byte buffer to a float:
    auto as_f32 = [&] {
        float f;
        memcpy(&f, buf, sizeof(float));
        return f;
    };
    // Read and parse incoming packets from Serial:
    while (Serial.available() > 0) {
        uint8_t c = Serial.read();
        auto msg_size = parser.parse(c, on_char_receive);
        // If a complete message of 6 bytes was received, and if it addresses
        // a valid fader:
        if (msg_size == 6 && fader_idx < Config::num_faders) {
            // Execute the command:
            switch (cmd) {
                case 'p': controllers[fader_idx].setKp(as_f32()); break;
                case 'i': controllers[fader_idx].setKi(as_f32()); break;
                case 'd': controllers[fader_idx].setKd(as_f32()); break;
                case 'c': controllers[fader_idx].setEMACutoff(as_f32()); break;
                case 'm': controllers[fader_idx].setMaxOutput(as_f32()); break;
                case 's':
                    references[fader_idx].startExperiment(as_f32());
                    controllers[fader_idx].resetIntegral();
                    break;
                default: break;
            }
        }
    }
}

#pragma once
#include "Registers.hpp"
#include <avr/interrupt.h>
#include <util/atomic.h>

#include <Arduino_Helpers.h> // EMA.hpp

#include <AH/Filters/EMA.hpp>           // EMA filter
#include <AH/Math/IncreaseBitDepth.hpp> // increaseBitDepth

template <class Config>
struct ADCManager {
    /// Evenly distribute the analog readings in the control loop period.
    constexpr static uint8_t adc_start_count =
        Config::interrupt_divisor / Config::num_faders;
    /// The rate at which we're sampling using the ADC.
    constexpr static float adc_rate = Config::interrupt_freq / adc_start_count;
    // Check that this doesn't take more time than the 13 ADC clock cycles it
    // takes to actually do the conversion. Use 14 instead of 13 just to be safe.
    static_assert(adc_rate <= Config::adc_clock_freq / 14, "ADC too slow");

    /// Enable the ADC with Vcc reference, with the given prescaler, auto
    /// trigger disabled, ADC interrupt enabled.
    /// Called from main program, with interrupts enabled.
    void begin();

    /// Start an ADC conversion on the given channel.
    /// Called inside an ISR.
    void startConversion(uint8_t channel);

    /// Start an ADC conversion at the right intervals.
    /// @param  counter
    ///         Counter that keeps track of how many times the timer interrupt
    ///         fired, between 0 and Config::interrupt_divisor - 1.
    /// Called inside an ISR.
    void update(uint8_t counter);

    /// Read the latest ADC result.
    /// Called inside an ISR.
    void complete();

    /// Get the latest ADC reading for the given index.
    /// Called from main program, with interrupts enabled.
    int16_t read(uint8_t idx);
    /// Get the latest filtered ADC reading for the given index.
    /// Called from main program, with interrupts enabled.
    /// @return (16 - Config::adc_ema_K)-bit filtered ADC value.
    uint16_t readFiltered(uint8_t idx);
    /// Get the latest filtered ADC reading for the given index.
    /// Called from main program, with interrupts enabled.
    /// @return 14-bit filtered ADC value.
    uint16_t readFiltered14(uint8_t idx);
    /// Get the latest filtered ADC reading for the given index.
    /// Called inside an ISR.
    /// @return 14-bit filtered ADC value.
    uint16_t readFiltered14ISR(uint8_t idx);

    /// Write the ADC reading for the given index.
    /// Called from main program, with interrupts enabled.
    void write(uint8_t idx, int16_t val);
    /// Write the filtered ADC reading for the given index.
    /// Called only before ADC interrupts are enabled.
    /// @param  val 10-bit ADC value.
    void writeFiltered(uint8_t idx, uint16_t val);

    /// Convert a 10-bit ADC reading to the largest possible range for the given
    /// value of Config::adc_ema_K.
    uint16_t shiftValue10(uint16_t val);
    /// Convert the given shifted filtered value to a 14-bit range.
    uint16_t unShiftValue14(uint16_t val);

    /// Convert the channel index between 0 and Config::num_faders - 1 to the
    /// actual ADC multiplexer address.
    constexpr static inline uint8_t
    channel_index_to_mux_address(uint8_t adc_mux_idx) {
        return Config::use_A6_A7
                   ? (adc_mux_idx < 2 ? adc_mux_idx : adc_mux_idx + 4)
                   : adc_mux_idx;
    }

    /// Index of the ADC channel currently being read.
    uint8_t channel_index = Config::num_faders;
    /// Latest 10-bit ADC reading of each fader (updated in ADC ISR). Used for
    /// the control loop.
    volatile int16_t readings[Config::num_faders];
    /// Filters for ADC readings.
    EMA<Config::adc_ema_K, uint16_t> filters[Config::num_faders];
    /// Filtered (shifted) ADC readings. Used to output over MIDI etc. but not
    /// for the control loop.
    volatile uint16_t filtered_readings[Config::num_faders];
};

template <class Config>
inline void ADCManager<Config>::begin() {
    constexpr auto prescaler = factorToADCPrescaler(Config::adc_prescaler_fac);
    static_assert(prescaler != ADCPrescaler::Invalid, "Invalid prescaler");

    ATOMIC_BLOCK(ATOMIC_FORCEON) {
        cbi(ADCSRA, ADEN); // Disable ADC

        cbi(ADMUX, REFS1); // Vcc reference
        sbi(ADMUX, REFS0); // Vcc reference

        cbi(ADMUX, ADLAR); // 8 least significant bits in ADCL

        setADCPrescaler(prescaler);

        cbi(ADCSRA, ADATE); // Auto trigger disable
        sbi(ADCSRA, ADIE);  // ADC Interrupt Enable
        sbi(ADCSRA, ADEN);  // Enable ADC
    }
}

template <class Config>
inline void ADCManager<Config>::update(uint8_t counter) {
    if (Config::num_faders > 0 && counter == 0 * adc_start_count)
        startConversion(0);
    else if (Config::num_faders > 1 && counter == 1 * adc_start_count)
        startConversion(1);
    else if (Config::num_faders > 2 && counter == 2 * adc_start_count)
        startConversion(2);
    else if (Config::num_faders > 3 && counter == 3 * adc_start_count)
        startConversion(3);
}

template <class Config>
inline void ADCManager<Config>::startConversion(uint8_t channel) {
    channel_index = channel;
    ADMUX &= 0xF0;
    ADMUX |= channel_index_to_mux_address(channel);
    sbi(ADCSRA, ADSC); // ADC Start Conversion
}

template <class Config>
inline void ADCManager<Config>::complete() {
    if (Config::enable_overrun_indicator && readings[channel_index] >= 0)
        sbi(PORTB, 5);    // Set overrun indicator
    uint16_t value = ADC; // Store ADC reading
    readings[channel_index] = value;
    // Filter the reading
    auto &filter = filters[channel_index];
    filtered_readings[channel_index] = filter(shiftValue10(value));
}

template <class Config>
inline int16_t ADCManager<Config>::read(uint8_t idx) {
    int16_t val;
    ATOMIC_BLOCK(ATOMIC_FORCEON) { val = readings[idx]; }
    return val;
}

template <class Config>
inline void ADCManager<Config>::write(uint8_t idx, int16_t val) {
    ATOMIC_BLOCK(ATOMIC_FORCEON) { readings[idx] = val; }
}

template <class Config>
inline uint16_t ADCManager<Config>::shiftValue10(uint16_t val) {
    return AH::increaseBitDepth<16 - Config::adc_ema_K, 10, uint16_t>(val);
}

template <class Config>
inline uint16_t ADCManager<Config>::unShiftValue14(uint16_t val) {
    const int shift = 6 - Config::adc_ema_K - 4;
    return shift >= 0 ? val >> shift : val << -shift;
}

template <class Config>
inline uint16_t ADCManager<Config>::readFiltered14ISR(uint8_t idx) {
    return unShiftValue14(filtered_readings[idx]);
}

template <class Config>
inline uint16_t ADCManager<Config>::readFiltered(uint8_t idx) {
    uint16_t val;
    ATOMIC_BLOCK(ATOMIC_FORCEON) { val = filtered_readings[idx]; }
    return val;
}

template <class Config>
inline uint16_t ADCManager<Config>::readFiltered14(uint8_t idx) {
    return unShiftValue14(readFiltered(idx));
}

template <class Config>
inline void ADCManager<Config>::writeFiltered(uint8_t idx, uint16_t val) {
    filters[idx].reset(shiftValue10(val));
    filtered_readings[idx] = shiftValue10(val);
}

#pragma once
#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/atomic.h>

template <class Config>
struct TouchSense {

    /// The actual threshold as a number of interrupts instead of seconds:
    static constexpr uint8_t touch_sense_thres =
        Config::interrupt_freq * Config::touch_rc_time_threshold;
    /// Ignore mains noise by making the “touched” status stick for longer than
    /// the mains period:
    static constexpr float period_50Hz = 1. / 50;
    /// Keep the “touched” status active for this many periods (see below):
    static constexpr uint8_t touch_sense_stickiness =
        Config::interrupt_freq * period_50Hz * 4 / Config::interrupt_divisor;
    /// Check that the threshold is smaller than the control loop period:
    static_assert(touch_sense_thres < Config::interrupt_divisor,
                  "Touch sense threshold too high");

    /// The combined bit mask for all touch GPIO pins.
    static constexpr uint8_t gpio_mask =
        (Config::num_faders > 0 ? Config::touch_masks[0] : 0) |
        (Config::num_faders > 1 ? Config::touch_masks[1] : 0) |
        (Config::num_faders > 2 ? Config::touch_masks[2] : 0) |
        (Config::num_faders > 3 ? Config::touch_masks[3] : 0);

    /// Initialize the GPIO pins for capacitive sensing.
    /// Called from main program, with interrupts enabled.
    void begin();

    /// Check which touch sensing knobs are being touched.
    /// @param  counter
    ///         Counter that keeps track of how many times the timer interrupt
    ///         fired, between 0 and Config::interrupt_divisor - 1.
    /// Called inside an ISR.
    void update(uint8_t counter);

    /// Get the touch status for the given index.
    /// Called from main program, with interrupts enabled.
    bool read(uint8_t idx);

    // Timers to take into account the stickiness.
    uint8_t touch_timers[Config::num_faders] {};
    // Whether the knobs are being touched.
    volatile bool touched[Config::num_faders] {};
};

template <class Config>
void TouchSense<Config>::begin() {
    ATOMIC_BLOCK(ATOMIC_FORCEON) {
        PORTB &= ~gpio_mask; // low
        DDRB |= gpio_mask;   // output mode
    }
}

// 0. The pin mode is “output”, the value is “low”.
// 1. Set the pin mode to “input”, touch_timer = 0.
// 2. The pin will start charging through the external pull-up resistor.
// 3. After a fixed amount of time, check whether the pin became “high”:
//    if this is the case, the RC-time of the knob/pull-up resistor circuit
//    was smaller than the given threshold. Since R is fixed, this can be used
//    to infer C, the capacitance of the knob: if the capacitance is lower than
//    the threshold (i.e. RC-time is lower), this means the knob was not touched.
// 5. Set the pin mode to “output”, to start discharging the pin to 0V again.
// 6. Some time later, the pin has discharged, so switch to “input” mode and
//    start charging again for the next RC-time measurement.
//
// The “touched” status is sticky: it will remain set for at least
// touch_sense_stickiness ticks. If the pin never resulted in another “touched”
// measurement during that period, the “touched” status for that pin is cleared.

template <class Config>
void TouchSense<Config>::update(uint8_t counter) {
    if (gpio_mask == 0)
        return;
    if (counter == 0) {
        DDRB &= ~gpio_mask; // input mode, start charging
    } else if (counter == touch_sense_thres) {
        uint8_t touched_bits = PINB;
        DDRB |= gpio_mask; // output mode, start discharging
        for (uint8_t i = 0; i < Config::num_faders; ++i) {
            if (Config::touch_masks[i] == 0)
                continue;
            bool touch_i = (touched_bits & Config::touch_masks[i]) == 0;
            if (touch_i) {
                touch_timers[i] = touch_sense_stickiness;
                touched[i] = true;
            } else if (touch_timers[i] > 0) {
                --touch_timers[i];
                if (touch_timers[i] == 0) touched[i] = false;
            }
        }
    }
}

template <class Config>
bool TouchSense<Config>::read(uint8_t idx) {
    bool t;
    ATOMIC_BLOCK(ATOMIC_FORCEON) { t = touched[idx]; }
    return t;
}

#pragma once

#include <stddef.h>
#include <stdint.h>

/// @see @ref horner(float,float,const float(&)[N])
constexpr inline float horner_impl(float xa, const float *p, size_t count,
                                   float t) {
    return count == 0 ? p[count] + xa * t
                      : horner_impl(xa, p, count - 1, p[count] + xa * t);
}

/// Evaluate a polynomial using
/// [Horner's method](https://en.wikipedia.org/wiki/Horner%27s_method).
template <size_t N>
constexpr inline float horner(float x, float a, const float (&p)[N]) {
    return horner_impl(x - a, p, N - 2, p[N - 1]);
}

/// Compute the weight factor of a exponential moving average filter
/// with the given cutoff frequency.
/// @see https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Exponential%20Moving%20Average/Exponential-Moving-Average.html#cutoff-frequency
///      for the formula.
inline float calcAlphaEMA(float f_n) {
    // Taylor coefficients of
    // α(fₙ) = cos(2πfₙ) - 1 + √( cos(2πfₙ)² - 4 cos(2πfₙ) + 3 )
    // at fₙ = 0.25
    constexpr static float coeff[] {
        +7.3205080756887730e-01, +9.7201214975728490e-01,
        -3.7988125051760377e+00, +9.5168450173968860e+00,
        -2.0829320344443730e+01, +3.0074306603814595e+01,
        -1.6446172139457754e+01, -8.0756002564633450e+01,
        +3.2420501524111750e+02, -6.5601870948443250e+02,
    };
    return horner(f_n, 0.25, coeff);
}

/// Standard PID (proportional, integral, derivative) controller. Derivative
/// component is filtered using an exponential moving average filter.
class PID {
  public:
    PID() = default;
    /// @param  kp
    ///         Proportional gain
    /// @param  ki
    ///         Integral gain
    /// @param  kd
    ///         Derivative gain
    /// @param  Ts
    ///         Sampling time (seconds)
    /// @param  fc
    ///         Cutoff frequency of derivative EMA filter (Hertz),
    ///         zero to disable the filter entirely
    PID(float kp, float ki, float kd, float Ts, float f_c = 0,
        float maxOutput = 255)
        : Ts(Ts), maxOutput(maxOutput) {
        setKp(kp);
        setKi(ki);
        setKd(kd);
        setEMACutoff(f_c);
    }

    /// Update the controller: given the current position, compute the control
    /// action.
    float update(uint16_t input) {
        // The error is the difference between the reference (setpoint) and the
        // actual position (input)
        int16_t error = setpoint - input;
        // The integral or sum of current and previous errors
        int32_t newIntegral = integral + error;
        // Compute the difference between the current and the previous input,
        // but compute a weighted average using a factor α ∊ (0,1]
        float diff = emaAlpha * (prevInput - input);
        // Update the average
        prevInput -= diff;

        // Check if we can turn off the motor
        if (activityCount >= activityThres && activityThres) {
            float filtError = setpoint - prevInput;
            if (filtError >= -errThres && filtError <= errThres) {
                errThres = 2; // hysteresis
                return 0;
            } else {
                errThres = 1;
            }
        } else {
            ++activityCount;
            errThres = 1;
        }

        bool backward = false;
        int32_t calcIntegral = backward ? newIntegral : integral;

        // Standard PID rule
        float output = kp * error + ki_Ts * calcIntegral + kd_Ts * diff;

        // Clamp and anti-windup
        if (output > maxOutput)
            output = maxOutput;
        else if (output < -maxOutput)
            output = -maxOutput;
        else
            integral = newIntegral;

        return output;
    }

    void setKp(float kp) { this->kp = kp; }               ///< Proportional gain
    void setKi(float ki) { this->ki_Ts = ki * this->Ts; } ///< Integral gain
    void setKd(float kd) { this->kd_Ts = kd / this->Ts; } ///< Derivative gain

    float getKp() const { return kp; }         ///< Proportional gain
    float getKi() const { return ki_Ts / Ts; } ///< Integral gain
    float getKd() const { return kd_Ts * Ts; } ///< Derivative gain

    /// Set the cutoff frequency (-3 dB point) of the exponential moving average
    /// filter that is applied to the input before taking the difference for
    /// computing the derivative term.
    void setEMACutoff(float f_c) {
        float f_n = f_c * Ts; // normalized sampling frequency
        this->emaAlpha = f_c == 0 ? 1 : calcAlphaEMA(f_n);
    }

    /// Set the reference/target/setpoint of the controller.
    void setSetpoint(uint16_t setpoint) {
        if (this->setpoint != setpoint) this->activityCount = 0;
        this->setpoint = setpoint;
    }
    /// @see @ref setSetpoint(int16_t)
    uint16_t getSetpoint() const { return setpoint; }

    /// Set the maximum control output magnitude. Default is 255, which clamps
    /// the control output in [-255, +255].
    void setMaxOutput(float maxOutput) { this->maxOutput = maxOutput; }
    /// @see @ref setMaxOutput(float)
    float getMaxOutput() const { return maxOutput; }

    /// Reset the activity counter to prevent the motor from turning off.
    void resetActivityCounter() { this->activityCount = 0; }
    /// Set the number of seconds after which the motor is turned off, zero to
    /// keep it on indefinitely.
    void setActivityTimeout(float s) {
        if (s == 0)
            activityThres = 0;
        else
            activityThres = uint16_t(s / Ts) == 0 ? 1 : s / Ts;
    }

    /// Reset the sum of the previous errors to zero.
    void resetIntegral() { integral = 0; }

  private:
    float Ts = 1;               ///< Sampling time (seconds)
    float maxOutput = 255;      ///< Maximum control output magnitude
    float kp = 1;               ///< Proportional gain
    float ki_Ts = 0;            ///< Integral gain times Ts
    float kd_Ts = 0;            ///< Derivative gain divided by Ts
    float emaAlpha = 1;         ///< Weight factor of derivative EMA filter.
    float prevInput = 0;        ///< (Filtered) previous input for derivative.
    uint16_t activityCount = 0; ///< How many ticks since last setpoint change.
    uint16_t activityThres = 0; ///< Threshold for turning off the output.
    uint8_t errThres = 1;       ///< Threshold with hysteresis.
    int32_t integral = 0;       ///< Sum of previous errors for integral.
    uint16_t setpoint = 0;      ///< Position reference.
};

#pragma once
#include "Registers.hpp"
#include <avr/interrupt.h>
#include <util/atomic.h>

/// Configure Timer0 in either phase correct or fast PWM mode with the given
/// prescaler, enable output compare B.
inline void setupMotorTimer0(bool phase_correct, Timer0Prescaler prescaler) {
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
        setTimer0WGMode(phase_correct ? Timer0WGMode::PWM
                                      : Timer0WGMode::FastPWM);
        setTimer0Prescaler(prescaler);
        sbi(TCCR0A, COM0B1); // Table 14-6, 14-7 Compare Output Mode
        sbi(TCCR0A, COM0A1); // Table 14-6, 14-7 Compare Output Mode
    }
}

/// Configure Timer2 in either phase correct or fast PWM mode with the given
/// prescaler, enable output compare B.
inline void setupMotorTimer2(bool phase_correct, Timer2Prescaler prescaler) {
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
        setTimer2WGMode(phase_correct ? Timer2WGMode::PWM
                                      : Timer2WGMode::FastPWM);
        setTimer2Prescaler(prescaler);
        sbi(TCCR2A, COM2B1); // Table 14-6, 14-7 Compare Output Mode
        sbi(TCCR2A, COM2A1); // Table 14-6, 14-7 Compare Output Mode
    }
}

/// Configure the timers for the PWM outputs.
template <class Config>
inline void setupMotorTimers() {
    constexpr auto prescaler0 = factorToTimer0Prescaler(Config::prescaler_fac);
    static_assert(prescaler0 != Timer0Prescaler::Invalid, "Invalid prescaler");
    constexpr auto prescaler2 = factorToTimer2Prescaler(Config::prescaler_fac);
    static_assert(prescaler2 != Timer2Prescaler::Invalid, "Invalid prescaler");

    if (Config::num_faders > 0)
        setupMotorTimer2(Config::phase_correct_pwm, prescaler2);
    if (Config::num_faders > 2)
        setupMotorTimer0(Config::phase_correct_pwm, prescaler0);
}

/// Class for driving up to 4 DC motors using PWM.
template <class Config>
struct Motors {
    void begin();
    template <uint8_t Idx>
    void setSpeed(int16_t speed);

    template <uint8_t Idx>
    void setupGPIO();
    template <uint8_t Idx>
    void forward(uint8_t speed);
    template <uint8_t Idx>
    void backward(uint8_t speed);
};

template <class Config>
inline void Motors<Config>::begin() {
    setupMotorTimers<Config>();

    if (Config::num_faders > 0) {
        sbi(DDRD, 2);
        sbi(DDRD, 3);
    }
    if (Config::num_faders > 1) {
        sbi(DDRD, 7);
        sbi(DDRB, 3);
    }
    if (Config::num_faders > 2) {
        sbi(DDRD, 4);
        sbi(DDRD, 5);
    }
    if (Config::num_faders > 3) {
        if (Config::fader_3_A2)
            sbi(DDRC, 2);
        else
            sbi(DDRB, 5);

        sbi(DDRD, 6);
    }
}

// Fast PWM (Table 14-6):
//   Clear OC0B on Compare Match, set OC0B at BOTTOM (non-inverting mode).
// Phase Correct PWM (Table 14-7):
//   Clear OC0B on compare match when up-counting. Set OC0B on compare match
//   when down-counting.
template <class Config>
template <uint8_t Idx>
inline void Motors<Config>::forward(uint8_t speed) {
    if (Idx >= Config::num_faders)
        return;
    else if (Idx == 0)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            cbi(TCCR2A, COM2B0);
            cbi(PORTD, 2);
            OCR2B = speed;
        }
    else if (Idx == 1)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            cbi(TCCR2A, COM2A0);
            cbi(PORTD, 7);
            OCR2A = speed;
        }
    else if (Idx == 2)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            cbi(TCCR0A, COM0B0);
            cbi(PORTD, 4);
            OCR0B = speed;
        }
    else if (Idx == 3)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            cbi(TCCR0A, COM0A0);
            if (Config::fader_3_A2)
                cbi(PORTC, 2);
            else
                cbi(PORTB, 5);
            OCR0A = speed;
        }
}

// Fast PWM (Table 14-6):
//   Set OC0B on Compare Match, clear OC0B at BOTTOM (inverting mode).
// Phase Correct PWM (Table 14-7):
//   Set OC0B on compare match when up-counting. Clear OC0B on compare match
//   when down-counting.
template <class Config>
template <uint8_t Idx>
inline void Motors<Config>::backward(uint8_t speed) {
    if (Idx >= Config::num_faders)
        return;
    else if (Idx == 0)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            sbi(TCCR2A, COM2B0);
            sbi(PORTD, 2);
            OCR2B = speed;
        }
    else if (Idx == 1)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            sbi(TCCR2A, COM2A0);
            sbi(PORTD, 7);
            OCR2A = speed;
        }
    else if (Idx == 2)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            sbi(TCCR0A, COM0B0);
            sbi(PORTD, 4);
            OCR0B = speed;
        }
    else if (Idx == 3)
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
            sbi(TCCR0A, COM0A0);
            if (Config::fader_3_A2)
                sbi(PORTC, 2);
            else
                sbi(PORTB, 5);
            OCR0A = speed;
        }
}

template <class Config>
template <uint8_t Idx>
inline void Motors<Config>::setSpeed(int16_t speed) {
    if (speed >= 0)
        forward<Idx>(speed);
    else
        backward<Idx>(-speed);
}