fspwm: StartPWM = StartPWM{},
    bspwm: StartPWM = StartPWM{},
    femc_offset: u12 = 0, // ADC is 12 bits
    bemc_offset: u12 = 0, // ADC is 12 bits

    }
    pub fn measureADCOffset(self: *@This()) !void {
        const ITERATIONS = 50;
        self.stop();
        rp2xxx.time.sleep_ms(1000);
        for (0..1) |i| {
            // Prepare ADC
            rp2xxx.adc.apply(.{
                .sample_frequency = null,
                .round_robin = null,
                .temp_sensor_enabled = false,
                .fifo = .{ .dreq_enabled = false, .irq_enabled = false, .shift = false },
            }); // apply calls enable
            rp2xxx.adc.select_input(switch (@as(u1, @intCast(i))) {
                0 => main.pins.m1emf,
                1 => main.pins.m2emf,
            });
            rp2xxx.adc.start(.free_running);
            var sum: usize = 0;
            for (0..ITERATIONS) |_| {
                while (rp2xxx.adc.fifo.is_empty()) {
                    asm volatile ("" ::: "memory");
                }
                sum += try rp2xxx.adc.fifo.pop();
            }
            switch (@as(u1, @intCast(i))) {
                0 => self.femc_offset = @truncate(sum / ITERATIONS),
                1 => self.bemc_offset = @truncate(sum / ITERATIONS),
            }
        }
        log.debug("EMC offset measurement done. Fwd: {} Bwd: {}", .{ self.femc_offset, self.bemc_offset });

pub fn tester(comptime driver: MotorDriverType) !void {
    const md = MotorDriver(driver){
        .fwd = main.pins.mot1,
        .bwd = main.pins.mot2,
        .speed = main.pins.speed,
    };
    const pwm_period = 125_000_000 / (@as(usize, main.cv.PWM_period) * 100 + 10_000);

const StartPWM = struct {
    const START_PWM_SIZE = 10;
    // TODO: Initial values could be 2/3 of pwm_wrap
    history: [START_PWM_SIZE]u16 = [_]u16{0} ** START_PWM_SIZE, // PWM wrap is u16
    next: u8 = 0, // CV is u8

    fn save(self: *@This(), val: u16) void {
        self.history[self.next] = val;
        self.next +%= 1;
    }
    fn average(self: @This()) u16 {
        var sum: usize = 0;
        for (0..START_PWM_SIZE) |i| {
            sum += self.history[i];
        }
        return @truncate(sum / START_PWM_SIZE);
    }
};
pub fn tester(md: anytype, pwm_period: u16) !void {

    // try motor.tester(board.motor_driver);

    var md = motor.MotorDriver(board.motor_driver){
        .fwd = pins.mot1,
        .bwd = pins.mot2,
        .speed = pins.speed,
    };
    try md.measureADCOffset();
    // try motor.tester(md, @intCast(125_000_000 / (@as(usize, cv.PWM_period) * 100 + 10_000)));