diff --git a/firmware/src/board.rs b/firmware/src/board.rs
new file mode 100644
index 0000000..3ef6f01
--- /dev/null
+++ b/firmware/src/board.rs
@@ -0,0 +1,274 @@
+use cortex_m;
+use tm4c129x;
+
+
+const LED1: u8 = 0x10; // PF1
+const LED2: u8 = 0x40; // PF3
+
+const HV_PWM: u8 = 0x01;  // PF0
+const FV_PWM: u8 = 0x04;  // PF2
+const FBV_PWM: u8 = 0x01; // PD5
+
+const FD_ADC: u8 = 0x01;  // PE0
+const FV_ADC: u8 = 0x02;  // PE1
+const FBI_ADC: u8 = 0x04; // PE2
+const IC_ADC: u8 = 0x08;  // PE3
+const FBV_ADC: u8 = 0x20; // PD5
+const AV_ADC: u8 = 0x40;  // PD6
+
+const FV_ERRN: u8 = 0x01;    // PL0
+const FBV_ERRN: u8 = 0x02;   // PL1
+const FBI_ERRN: u8 = 0x04;   // PL2
+const AV_ERRN: u8 = 0x08;    // PL3
+const AI_ERRN: u8 = 0x10;    // PL4
+const ERR_LATCHN: u8 = 0x20; // PL5
+const ERR_RESN: u8 = 0x01;   // PQ0
+
+const PWM_LOAD: u16 = (/*pwmclk*/16_000_000u32 / /*freq*/100_000) as u16;
+const UART_DIV_16P6: u32 = /*altclk*/16_000_000 * (1 << /*len(divfrac)*/6) /
+                           (/*clkdiv*/16 * /*baud*/115200);
+
+
+pub const AV_ADC_GAIN: f32 = 6.792703150912105;
+
+
+pub fn set_led(nr: u8, state: bool) {
+    let bit = match nr {
+        1 => LED1,
+        2 => LED2,
+        _ => panic!("unknown LED")
+    };
+    cortex_m::interrupt::free(|cs| {
+        let gpio_k = tm4c129x::GPIO_PORTK.borrow(cs);
+        if state {
+            gpio_k.data.modify(|r, w| w.data().bits(r.data().bits() | bit))
+        } else {
+            gpio_k.data.modify(|r, w| w.data().bits(r.data().bits() & !bit))
+        }
+    });
+}
+
+pub fn set_hv_pwm(duty: u16) {
+    cortex_m::interrupt::free(|cs| {
+        let pwm0 = tm4c129x::PWM0.borrow(cs);
+        pwm0._0_cmpa.write(|w| w.compa().bits(duty));
+    });
+}
+
+pub fn set_fv_pwm(duty: u16) {
+    cortex_m::interrupt::free(|cs| {
+        let pwm0 = tm4c129x::PWM0.borrow(cs);
+        pwm0._1_cmpa.write(|w| w.compa().bits(duty));
+    });
+}
+
+pub fn set_fbv_pwm(duty: u16) {
+    cortex_m::interrupt::free(|cs| {
+        let pwm0 = tm4c129x::PWM0.borrow(cs);
+        pwm0._2_cmpa.write(|w| w.compa().bits(duty));
+    });
+}
+
+#[allow(dead_code)]
+pub enum EmissionRange {
+    Low,  // 22K
+    Med,  // 22K//(200Ω + compensated diode)
+    High  // 22K//(39Ω + uncompensated diode)
+}
+
+pub fn set_emission_range(range: EmissionRange) {
+    cortex_m::interrupt::free(|cs| {
+        let gpio_p = tm4c129x::GPIO_PORTP.borrow(cs);
+        gpio_p.data.modify(|r, w| {
+            let value = r.data().bits() & 0b100111;
+            match range {
+                EmissionRange::Low  => w.data().bits(value | 0b000000),
+                EmissionRange::Med  => w.data().bits(value | 0b001000),
+                EmissionRange::High => w.data().bits(value | 0b010000),
+            }
+        });
+    });
+}
+
+fn reset_error() {
+    cortex_m::interrupt::free(|cs| {
+        let gpio_l = tm4c129x::GPIO_PORTL.borrow(cs);
+        let gpio_q = tm4c129x::GPIO_PORTQ.borrow(cs);
+        gpio_q.data.modify(|r, w| w.data().bits(r.data().bits() & !ERR_RESN));
+        while gpio_l.data.read().bits() as u8 & ERR_LATCHN == 0 {}
+        gpio_q.data.modify(|r, w| w.data().bits(r.data().bits() | ERR_RESN));
+    });
+}
+
+pub fn process_errors() {
+    cortex_m::interrupt::free(|cs| {
+        let gpio_l = tm4c129x::GPIO_PORTL.borrow(cs);
+        let errors_n = gpio_l.data.read().bits() as u8;
+        if errors_n & FV_ERRN == 0 {
+            hprintln!("Filament overvolt");
+        }
+        if errors_n & FBV_ERRN == 0 {
+            hprintln!("Filament bias overvolt");
+        }
+        if errors_n & FBI_ERRN == 0 {
+            hprintln!("Filament bias overcurrent");
+        }
+        if errors_n & AV_ERRN == 0 {
+            hprintln!("Anode overvolt");
+        }
+        if errors_n & AI_ERRN == 0 {
+            hprintln!("Anode overcurrent");
+        }
+    });
+}
+
+pub fn init() {
+    cortex_m::interrupt::free(|cs| {
+        let sysctl = tm4c129x::SYSCTL.borrow(cs);
+
+        // Set up main oscillator
+        sysctl.moscctl.write(|w| w.noxtal().bit(false));
+        sysctl.moscctl.modify(|_, w| w.pwrdn().bit(false).oscrng().bit(true));
+
+        // Set up PLL with fVCO=320 MHz
+        sysctl.pllfreq1.write(|w| w.q().bits(0).n().bits(4));
+        sysctl.pllfreq0.write(|w| w.mint().bits(64).pllpwr().bit(true));
+        sysctl.rsclkcfg.modify(|_, w| w.pllsrc().mosc().newfreq().bit(true));
+        while !sysctl.pllstat.read().lock().bit() {}
+
+        // Bring up GPIO ports A, D, E, F, G, K, L, P, Q
+        sysctl.rcgcgpio.modify(|_, w| {
+            w.r0().bit(true)
+             .r3().bit(true)
+             .r4().bit(true)
+             .r5().bit(true)
+             .r6().bit(true)
+             .r9().bit(true)
+             .r10().bit(true)
+             .r13().bit(true)
+             .r14().bit(true)
+        });
+        while !sysctl.prgpio.read().r0().bit() {}
+        while !sysctl.prgpio.read().r3().bit() {}
+        while !sysctl.prgpio.read().r4().bit() {}
+        while !sysctl.prgpio.read().r5().bit() {}
+        while !sysctl.prgpio.read().r6().bit() {}
+        while !sysctl.prgpio.read().r9().bit() {}
+        while !sysctl.prgpio.read().r10().bit() {}
+        while !sysctl.prgpio.read().r13().bit() {}
+        while !sysctl.prgpio.read().r14().bit() {}
+
+        // Set up UART0 at 115200
+        let gpio_a = tm4c129x::GPIO_PORTA_AHB.borrow(cs);
+        gpio_a.dir.write(|w| w.dir().bits(0b11));
+        gpio_a.den.write(|w| w.den().bits(0b11));
+        gpio_a.afsel.write(|w| w.afsel().bits(0b11));
+        gpio_a.pctl.write(|w| unsafe { w.pmc0().bits(1).pmc1().bits(1) });
+
+        sysctl.rcgcuart.modify(|_, w| w.r0().bit(true));
+        while !sysctl.pruart.read().r0().bit() {}
+
+        let uart_0 = tm4c129x::UART0.borrow(cs);
+        uart_0.cc.write(|w| w.cs().altclk());
+        uart_0.ibrd.write(|w| w.divint().bits((UART_DIV_16P6 >> 6) as u16));
+        uart_0.fbrd.write(|w| w.divfrac().bits(UART_DIV_16P6 as u8));
+        uart_0.lcrh.write(|w| w.wlen()._8().fen().bit(true));
+        uart_0.ctl.write(|w| w.rxe().bit(true).txe().bit(true).uarten().bit(true));
+
+        // Set up LEDs
+        let gpio_k = tm4c129x::GPIO_PORTK.borrow(cs);
+        gpio_k.dir.write(|w| w.dir().bits(LED1|LED2));
+        gpio_k.den.write(|w| w.den().bits(LED1|LED2));
+
+        // Set up gain and emission range control pins
+        let gpio_p = tm4c129x::GPIO_PORTP.borrow(cs);
+        gpio_p.dir.write(|w| w.dir().bits(0b111111));
+        gpio_p.den.write(|w| w.den().bits(0b111111));
+
+        // Set up error pins
+        let gpio_l = tm4c129x::GPIO_PORTL.borrow(cs);
+        let gpio_q = tm4c129x::GPIO_PORTQ.borrow(cs);
+        gpio_l.pur.write(|w| w.pue().bits(FV_ERRN|FBV_ERRN|FBI_ERRN|AV_ERRN|AI_ERRN));
+        gpio_l.den.write(|w| w.den().bits(FV_ERRN|FBV_ERRN|FBI_ERRN|AV_ERRN|AI_ERRN|ERR_LATCHN));
+        gpio_q.dir.write(|w| w.dir().bits(ERR_RESN));
+        gpio_q.den.write(|w| w.den().bits(ERR_RESN));
+        reset_error();
+
+        // Set up PWMs
+        let gpio_f = tm4c129x::GPIO_PORTF_AHB.borrow(cs);
+        gpio_f.dir.write(|w| w.dir().bits(HV_PWM|FV_PWM));
+        gpio_f.den.write(|w| w.den().bits(HV_PWM|FV_PWM));
+        gpio_f.afsel.write(|w| w.afsel().bits(HV_PWM|FV_PWM));
+        gpio_f.pctl.write(|w| unsafe { w.pmc0().bits(6).pmc2().bits(6) });
+
+        let gpio_g = tm4c129x::GPIO_PORTG_AHB.borrow(cs);
+        gpio_g.dir.write(|w| w.dir().bits(FBV_PWM));
+        gpio_g.den.write(|w| w.den().bits(FBV_PWM));
+        gpio_g.afsel.write(|w| w.afsel().bits(FBV_PWM));
+        gpio_g.pctl.write(|w| unsafe { w.pmc0().bits(6) });
+
+        sysctl.rcgcpwm.modify(|_, w| w.r0().bit(true));
+        while !sysctl.prpwm.read().r0().bit() {}
+
+        let pwm0 = tm4c129x::PWM0.borrow(cs);
+        // HV_PWM
+        pwm0._0_gena.write(|w| w.actload().zero().actcmpad().one());
+        pwm0._0_load.write(|w| w.load().bits(PWM_LOAD));
+        pwm0._0_cmpa.write(|w| w.compa().bits(0));
+        pwm0._0_ctl.write(|w| w.enable().bit(true));
+        // FV_PWM
+        pwm0._1_gena.write(|w| w.actload().zero().actcmpad().one());
+        pwm0._1_load.write(|w| w.load().bits(PWM_LOAD));
+        pwm0._1_cmpa.write(|w| w.compa().bits(0));
+        pwm0._1_ctl.write(|w| w.enable().bit(true));
+        // FBV_PWM
+        pwm0._2_gena.write(|w| w.actload().zero().actcmpad().one());
+        pwm0._2_load.write(|w| w.load().bits(PWM_LOAD));
+        pwm0._2_cmpa.write(|w| w.compa().bits(0));
+        pwm0._2_ctl.write(|w| w.enable().bit(true));
+        // Enable all at once
+        pwm0.enable.write(|w| {
+            w.pwm0en().bit(true)
+             .pwm2en().bit(true)
+             .pwm4en().bit(true)
+        });
+
+        // Set up ADC
+        let gpio_d = tm4c129x::GPIO_PORTD_AHB.borrow(cs);
+        let gpio_e = tm4c129x::GPIO_PORTE_AHB.borrow(cs);
+        gpio_d.afsel.write(|w| w.afsel().bits(FBV_ADC|AV_ADC));
+        gpio_d.amsel.write(|w| w.amsel().bits(FBV_ADC|AV_ADC));
+        gpio_e.afsel.write(|w| w.afsel().bits(FD_ADC|FV_ADC|FBI_ADC|IC_ADC));
+        gpio_e.amsel.write(|w| w.amsel().bits(FD_ADC|FV_ADC|FBI_ADC|IC_ADC));
+
+        sysctl.rcgcadc.modify(|_, w| w.r0().bit(true));
+        while !sysctl.pradc.read().r0().bit() {}
+
+        let adc0 = tm4c129x::ADC0.borrow(cs);
+        // Due to silicon erratum, this HAS to use PLL. PIOSC is not a suitable source.
+        // fADC=32 MHz
+        adc0.cc.write(|w| w.cs().syspll().clkdiv().bits(10));
+        adc0.im.write(|w| w.mask0().bit(true));
+        adc0.emux.write(|w| w.em0().always());
+        adc0.ssmux0.write(|w| {
+            w.mux0().bits(0) // IC_ADC
+             .mux1().bits(1) // FBI_ADC
+             .mux2().bits(2) // FV_ADC
+             .mux3().bits(3) // FD_ADC
+             .mux4().bits(5) // AV_ADC
+             .mux5().bits(6) // FBV_ADC
+        });
+        adc0.ssctl0.write(|w| w.ie5().bit(true).end5().bit(true));
+        adc0.sstsh0.write(|w| {
+            w.tsh0()._4()
+             .tsh1()._4()
+             .tsh2()._4()
+             .tsh3()._4()
+             .tsh4()._4()
+             .tsh5()._4()
+        });
+        adc0.sac.write(|w| w.avg()._64x());
+        adc0.ctl.write(|w| w.vref().bit(true));
+        adc0.actss.write(|w| w.asen0().bit(true));
+    });
+}
diff --git a/firmware/src/loop_anode.rs b/firmware/src/loop_anode.rs
new file mode 100644
index 0000000..fa8ce41
--- /dev/null
+++ b/firmware/src/loop_anode.rs
@@ -0,0 +1,39 @@
+use board;
+use pid;
+
+const PID_PARAMETERS: pid::Parameters = pid::Parameters {
+    kp: 0.004,
+    ki: 0.002,
+    kd: 0.0,
+    output_min: 0.0,
+    output_max: 30.0,
+    integral_min: -5000.0,
+    integral_max: 5000.0
+};
+
+
+pub struct Controller {
+    pid: pid::Controller
+}
+
+
+impl Controller {
+    pub const fn new() -> Controller {
+        Controller {
+            pid: pid::Controller::new(PID_PARAMETERS)
+        }
+    }
+
+    pub fn adc_input(&mut self, av_sample: u16) {
+        let pid_out = self.pid.update(av_sample as f32);
+        board::set_hv_pwm(pid_out as u16)
+    }
+
+    pub fn set_target(&mut self, volts: f32) {
+        self.pid.set_target(volts*board::AV_ADC_GAIN)
+    }
+
+    pub fn ready(&mut self) -> bool {
+        self.pid.is_within(1.0*board::AV_ADC_GAIN)
+    }
+}
diff --git a/firmware/src/loop_cathode.rs b/firmware/src/loop_cathode.rs
new file mode 100644
index 0000000..e552244
--- /dev/null
+++ b/firmware/src/loop_cathode.rs
@@ -0,0 +1,28 @@
+use board;
+use pid;
+
+const PID_PARAMETERS: pid::Parameters = pid::Parameters {
+    kp: 0.004,
+    ki: 0.002,
+    kd: 0.0,
+    output_min: 0.0,
+    output_max: 30.0,
+    integral_min: -5000.0,
+    integral_max: 5000.0
+};
+
+pub struct Controller {
+    pid: pid::Controller
+}
+
+
+impl Controller {
+    pub const fn new() -> Controller {
+        Controller {
+            pid: pid::Controller::new(PID_PARAMETERS)
+        }
+    }
+
+    pub fn adc_input(&mut self, _fbi_sample: u16, _fd_sample: u16, _fv_sample: u16, _fbv_sample: u16) {
+    }
+}
diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index 779f438..37de701 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -1,4 +1,4 @@
-#![feature(used, const_fn)]
+#![feature(used, const_fn, core_float)]
 #![no_std]
 
 #[macro_use]
@@ -14,49 +14,17 @@ use cortex_m::interrupt::Mutex;
 use tm4c129x::interrupt::Interrupt;
 use tm4c129x::interrupt::Handlers as InterruptHandlers;
 
+mod board;
 mod pid;
+mod loop_anode;
+mod loop_cathode;
 
 
-const HV_PID_PARAMETERS: pid::Parameters = pid::Parameters {
-    kp: 0.004,
-    ki: 0.002,
-    kd: 0.0,
-    output_min: 0.0,
-    output_max: 30.0,
-    integral_min: -5000.0,
-    integral_max: 5000.0
-};
+static LOOP_ANODE: Mutex<RefCell<loop_anode::Controller>> = Mutex::new(RefCell::new(
+    loop_anode::Controller::new()));
 
-static HV_PID: Mutex<RefCell<pid::Controller>> = Mutex::new(RefCell::new(
-    pid::Controller::new(HV_PID_PARAMETERS)));
-
-
-const LED1: u8 = 0x10; // PF1
-const LED2: u8 = 0x40; // PF3
-
-const HV_PWM: u8 = 0x01;  // PF0
-const FV_PWM: u8 = 0x04;  // PF2
-const FBV_PWM: u8 = 0x01; // PD5
-
-const FD_ADC: u8 = 0x01;  // PE0
-const FV_ADC: u8 = 0x02;  // PE1
-const FBI_ADC: u8 = 0x04; // PE2
-const IC_ADC: u8 = 0x08;  // PE3
-const FBV_ADC: u8 = 0x20; // PD5
-const AV_ADC: u8 = 0x40;  // PD6
-
-const FV_ERRN: u8 = 0x01;    // PL0
-const FBV_ERRN: u8 = 0x02;   // PL1
-const FBI_ERRN: u8 = 0x04;   // PL2
-const AV_ERRN: u8 = 0x08;    // PL3
-const AI_ERRN: u8 = 0x10;    // PL4
-const ERR_LATCHN: u8 = 0x20; // PL5
-const ERR_RESN: u8 = 0x01;   // PQ0
-
-const PWM_LOAD: u16 = (/*pwmclk*/16_000_000u32 / /*freq*/100_000) as u16;
-
-const UART_DIV_16P6: u32 = /*altclk*/16_000_000 * (1 << /*len(divfrac)*/6) /
-                           (/*clkdiv*/16 * /*baud*/115200);
+static LOOP_CATHODE: Mutex<RefCell<loop_cathode::Controller>> = Mutex::new(RefCell::new(
+    loop_cathode::Controller::new()));
 
 
 pub struct UART0;
@@ -89,283 +57,35 @@ macro_rules! println {
 }
 
 
-fn set_led(nr: u8, state: bool) {
-    cortex_m::interrupt::free(|cs| {
-        let gpio_k = tm4c129x::GPIO_PORTK.borrow(cs);
-        if state {
-            gpio_k.data.modify(|r, w| w.data().bits(r.data().bits() | nr))
-        } else {
-            gpio_k.data.modify(|r, w| w.data().bits(r.data().bits() & !nr))
-        }
-    });
-}
-
-fn set_hv_pwm(duty: u16) {
-    cortex_m::interrupt::free(|cs| {
-        let pwm0 = tm4c129x::PWM0.borrow(cs);
-        pwm0._0_cmpa.write(|w| w.compa().bits(duty));
-    });
-}
-
-fn set_fv_pwm(duty: u16) {
-    cortex_m::interrupt::free(|cs| {
-        let pwm0 = tm4c129x::PWM0.borrow(cs);
-        pwm0._1_cmpa.write(|w| w.compa().bits(duty));
-    });
-}
-
-fn set_fbv_pwm(duty: u16) {
-    cortex_m::interrupt::free(|cs| {
-        let pwm0 = tm4c129x::PWM0.borrow(cs);
-        pwm0._2_cmpa.write(|w| w.compa().bits(duty));
-    });
-}
-
-#[allow(dead_code)]
-enum EmissionRange {
-    Low,  // 22K
-    Med,  // 22K//(200Ω + compensated diode)
-    High  // 22K//(39Ω + uncompensated diode)
-}
-
-fn set_emission_range(range: EmissionRange) {
-    cortex_m::interrupt::free(|cs| {
-        let gpio_p = tm4c129x::GPIO_PORTP.borrow(cs);
-        gpio_p.data.modify(|r, w| {
-            let value = r.data().bits() & 0b100111;
-            match range {
-                EmissionRange::Low  => w.data().bits(value | 0b000000),
-                EmissionRange::Med  => w.data().bits(value | 0b001000),
-                EmissionRange::High => w.data().bits(value | 0b010000),
-            }
-        });
-    });
-}
-
-fn error_reset() {
-    cortex_m::interrupt::free(|cs| {
-        let gpio_l = tm4c129x::GPIO_PORTL.borrow(cs);
-        let gpio_q = tm4c129x::GPIO_PORTQ.borrow(cs);
-        gpio_q.data.modify(|r, w| w.data().bits(r.data().bits() & !ERR_RESN));
-        while gpio_l.data.read().bits() as u8 & ERR_LATCHN == 0 {}
-        gpio_q.data.modify(|r, w| w.data().bits(r.data().bits() | ERR_RESN));
-    });
-}
-
-
 fn main() {
-    cortex_m::interrupt::free(|cs| {
-        let sysctl = tm4c129x::SYSCTL.borrow(cs);
-        let nvic = tm4c129x::NVIC.borrow(cs);
+    board::init();
 
+    cortex_m::interrupt::free(|cs| {
         // Enable FPU
         let scb = tm4c129x::SCB.borrow(cs);
         scb.enable_fpu();
 
-        // Set up main oscillator
-        sysctl.moscctl.write(|w| w.noxtal().bit(false));
-        sysctl.moscctl.modify(|_, w| w.pwrdn().bit(false).oscrng().bit(true));
-
-        // Set up PLL with fVCO=320 MHz
-        sysctl.pllfreq1.write(|w| w.q().bits(0).n().bits(4));
-        sysctl.pllfreq0.write(|w| w.mint().bits(64).pllpwr().bit(true));
-        sysctl.rsclkcfg.modify(|_, w| w.pllsrc().mosc().newfreq().bit(true));
-        while !sysctl.pllstat.read().lock().bit() {}
-
-        // Set up system timer
-        let systick = tm4c129x::SYST.borrow(cs);
-        systick.set_reload(systick.get_ticks_per_10ms());
-        systick.enable_counter();
-        systick.enable_interrupt();
-
-        // Bring up GPIO ports A, D, E, F, G, K, L, P, Q
-        sysctl.rcgcgpio.modify(|_, w| {
-            w.r0().bit(true)
-             .r3().bit(true)
-             .r4().bit(true)
-             .r5().bit(true)
-             .r6().bit(true)
-             .r9().bit(true)
-             .r10().bit(true)
-             .r13().bit(true)
-             .r14().bit(true)
-        });
-        while !sysctl.prgpio.read().r0().bit() {}
-        while !sysctl.prgpio.read().r3().bit() {}
-        while !sysctl.prgpio.read().r4().bit() {}
-        while !sysctl.prgpio.read().r5().bit() {}
-        while !sysctl.prgpio.read().r6().bit() {}
-        while !sysctl.prgpio.read().r9().bit() {}
-        while !sysctl.prgpio.read().r10().bit() {}
-        while !sysctl.prgpio.read().r13().bit() {}
-        while !sysctl.prgpio.read().r14().bit() {}
-
-        // Set up UART0 at 115200
-        let gpio_a = tm4c129x::GPIO_PORTA_AHB.borrow(cs);
-        gpio_a.dir.write(|w| w.dir().bits(0b11));
-        gpio_a.den.write(|w| w.den().bits(0b11));
-        gpio_a.afsel.write(|w| w.afsel().bits(0b11));
-        gpio_a.pctl.write(|w| unsafe { w.pmc0().bits(1).pmc1().bits(1) });
-
-        sysctl.rcgcuart.modify(|_, w| w.r0().bit(true));
-        while !sysctl.pruart.read().r0().bit() {}
-
-        let uart_0 = tm4c129x::UART0.borrow(cs);
-        uart_0.cc.write(|w| w.cs().altclk());
-        uart_0.ibrd.write(|w| w.divint().bits((UART_DIV_16P6 >> 6) as u16));
-        uart_0.fbrd.write(|w| w.divfrac().bits(UART_DIV_16P6 as u8));
-        uart_0.lcrh.write(|w| w.wlen()._8().fen().bit(true));
-        uart_0.ctl.write(|w| w.rxe().bit(true).txe().bit(true).uarten().bit(true));
-
-        // Set up LEDs
-        let gpio_k = tm4c129x::GPIO_PORTK.borrow(cs);
-        gpio_k.dir.write(|w| w.dir().bits(LED1|LED2));
-        gpio_k.den.write(|w| w.den().bits(LED1|LED2));
-
-        // Set up gain and emission range control pins
-        let gpio_p = tm4c129x::GPIO_PORTP.borrow(cs);
-        gpio_p.dir.write(|w| w.dir().bits(0b111111));
-        gpio_p.den.write(|w| w.den().bits(0b111111));
-
-        // Set up error pins
-        let gpio_l = tm4c129x::GPIO_PORTL.borrow(cs);
-        let gpio_q = tm4c129x::GPIO_PORTQ.borrow(cs);
-        gpio_l.pur.write(|w| w.pue().bits(FV_ERRN|FBV_ERRN|FBI_ERRN|AV_ERRN|AI_ERRN));
-        gpio_l.den.write(|w| w.den().bits(FV_ERRN|FBV_ERRN|FBI_ERRN|AV_ERRN|AI_ERRN|ERR_LATCHN));
-        gpio_q.dir.write(|w| w.dir().bits(ERR_RESN));
-        gpio_q.den.write(|w| w.den().bits(ERR_RESN));
-        error_reset();
-
-        // Set up PWMs
-        let gpio_f = tm4c129x::GPIO_PORTF_AHB.borrow(cs);
-        gpio_f.dir.write(|w| w.dir().bits(HV_PWM|FV_PWM));
-        gpio_f.den.write(|w| w.den().bits(HV_PWM|FV_PWM));
-        gpio_f.afsel.write(|w| w.afsel().bits(HV_PWM|FV_PWM));
-        gpio_f.pctl.write(|w| unsafe { w.pmc0().bits(6).pmc2().bits(6) });
-
-        let gpio_g = tm4c129x::GPIO_PORTG_AHB.borrow(cs);
-        gpio_g.dir.write(|w| w.dir().bits(FBV_PWM));
-        gpio_g.den.write(|w| w.den().bits(FBV_PWM));
-        gpio_g.afsel.write(|w| w.afsel().bits(FBV_PWM));
-        gpio_g.pctl.write(|w| unsafe { w.pmc0().bits(6) });
-
-        sysctl.rcgcpwm.modify(|_, w| w.r0().bit(true));
-        while !sysctl.prpwm.read().r0().bit() {}
-
-        let pwm0 = tm4c129x::PWM0.borrow(cs);
-        // HV_PWM
-        pwm0._0_gena.write(|w| w.actload().zero().actcmpad().one());
-        pwm0._0_load.write(|w| w.load().bits(PWM_LOAD));
-        pwm0._0_cmpa.write(|w| w.compa().bits(0));
-        pwm0._0_ctl.write(|w| w.enable().bit(true));
-        // FV_PWM
-        pwm0._1_gena.write(|w| w.actload().zero().actcmpad().one());
-        pwm0._1_load.write(|w| w.load().bits(PWM_LOAD));
-        pwm0._1_cmpa.write(|w| w.compa().bits(0));
-        pwm0._1_ctl.write(|w| w.enable().bit(true));
-        // FBV_PWM
-        pwm0._2_gena.write(|w| w.actload().zero().actcmpad().one());
-        pwm0._2_load.write(|w| w.load().bits(PWM_LOAD));
-        pwm0._2_cmpa.write(|w| w.compa().bits(0));
-        pwm0._2_ctl.write(|w| w.enable().bit(true));
-        // Enable all at once
-        pwm0.enable.write(|w| {
-            w.pwm0en().bit(true)
-             .pwm2en().bit(true)
-             .pwm4en().bit(true)
-        });
-
-        // Set up ADC
-        let gpio_d = tm4c129x::GPIO_PORTD_AHB.borrow(cs);
-        let gpio_e = tm4c129x::GPIO_PORTE_AHB.borrow(cs);
-        gpio_d.afsel.write(|w| w.afsel().bits(FBV_ADC|AV_ADC));
-        gpio_d.amsel.write(|w| w.amsel().bits(FBV_ADC|AV_ADC));
-        gpio_e.afsel.write(|w| w.afsel().bits(FD_ADC|FV_ADC|FBI_ADC|IC_ADC));
-        gpio_e.amsel.write(|w| w.amsel().bits(FD_ADC|FV_ADC|FBI_ADC|IC_ADC));
-
-        sysctl.rcgcadc.modify(|_, w| w.r0().bit(true));
-        while !sysctl.pradc.read().r0().bit() {}
-
-        let adc0 = tm4c129x::ADC0.borrow(cs);
-        // Due to silicon erratum, this HAS to use PLL. PIOSC is not a suitable source.
-        // fADC=32 MHz
-        adc0.cc.write(|w| w.cs().syspll().clkdiv().bits(10));
-        adc0.im.write(|w| w.mask0().bit(true));
-        adc0.emux.write(|w| w.em0().always());
-        adc0.ssmux0.write(|w| {
-            w.mux0().bits(0) // IC_ADC
-             .mux1().bits(1) // FBI_ADC
-             .mux2().bits(2) // FV_ADC
-             .mux3().bits(3) // FD_ADC
-             .mux4().bits(5) // AV_ADC
-             .mux5().bits(6) // FBV_ADC
-        });
-        adc0.ssctl0.write(|w| w.ie5().bit(true).end5().bit(true));
-        adc0.sstsh0.write(|w| {
-            w.tsh0()._4()
-             .tsh1()._4()
-             .tsh2()._4()
-             .tsh3()._4()
-             .tsh4()._4()
-             .tsh5()._4()
-        });
-        adc0.sac.write(|w| w.avg()._64x());
-        adc0.ctl.write(|w| w.vref().bit(true));
-        adc0.actss.write(|w| w.asen0().bit(true));
-
+        let nvic = tm4c129x::NVIC.borrow(cs);
         nvic.enable(Interrupt::ADC0SS0);
 
-
-        set_emission_range(EmissionRange::Med);
-        HV_PID.borrow(cs).borrow_mut().set_target(200.0);
-        set_fv_pwm(PWM_LOAD/16);
-        set_fbv_pwm(PWM_LOAD/8);
+        board::set_emission_range(board::EmissionRange::High);
+        LOOP_ANODE.borrow(cs).borrow_mut().set_target(30.0+12.0);
+        //set_fv_pwm(10);
+        board::set_fbv_pwm(20);
     });
 
     println!("ready");
 
     loop {
-        cortex_m::interrupt::free(|cs| {
-            let gpio_l = tm4c129x::GPIO_PORTL.borrow(cs);
-            let errors_n = gpio_l.data.read().bits() as u8;
-            if errors_n & FV_ERRN == 0 {
-                hprintln!("Filament overvolt");
-            }
-            if errors_n & FBV_ERRN == 0 {
-                hprintln!("Filament bias overvolt");
-            }
-            if errors_n & FBI_ERRN == 0 {
-                hprintln!("Filament bias overcurrent");
-            }
-            if errors_n & AV_ERRN == 0 {
-                hprintln!("Anode overvolt");
-            }
-            if errors_n & AI_ERRN == 0 {
-                hprintln!("Anode overcurrent");
-            }
-        });
-    }
-}
-
-
-use cortex_m::exception::SysTick;
-extern fn sys_tick(ctxt: SysTick) {
-    static ELAPSED: Local<Cell<u32>, SysTick> = Local::new(Cell::new(0));
-    let elapsed = ELAPSED.borrow(&ctxt);
-
-    elapsed.set(elapsed.get() + 1);
-    if elapsed.get() % 100 == 0 {
-        set_led(LED1, true);
-        set_led(LED2, false);
-    }
-    if elapsed.get() % 100 == 50 {
-        set_led(LED1, false);
-        set_led(LED2, true);
+        board::process_errors();
     }
 }
 
 use tm4c129x::interrupt::ADC0SS0;
-extern fn adc0_ss0(_ctxt: ADC0SS0) {
+extern fn adc0_ss0(ctxt: ADC0SS0) {
+    static ELAPSED: Local<Cell<u32>, ADC0SS0> = Local::new(Cell::new(0));
+    let elapsed = ELAPSED.borrow(&ctxt);
+
     cortex_m::interrupt::free(|cs| {
         let adc0 = tm4c129x::ADC0.borrow(cs);
         if adc0.ostat.read().ov0().bit() {
@@ -373,22 +93,32 @@ extern fn adc0_ss0(_ctxt: ADC0SS0) {
         }
         adc0.isc.write(|w| w.in0().bit(true));
 
-        let _ic_sample  = adc0.ssfifo0.read().data().bits();
-        let _fbi_sample = adc0.ssfifo0.read().data().bits();
-        let _fv_sample  = adc0.ssfifo0.read().data().bits();
-        let _fd_sample  = adc0.ssfifo0.read().data().bits();
-        let av_sample   = adc0.ssfifo0.read().data().bits();
-        let _fbv_sample = adc0.ssfifo0.read().data().bits();
+        let _ic_sample = adc0.ssfifo0.read().data().bits();
+        let fbi_sample = adc0.ssfifo0.read().data().bits();
+        let fv_sample  = adc0.ssfifo0.read().data().bits();
+        let fd_sample  = adc0.ssfifo0.read().data().bits();
+        let av_sample  = adc0.ssfifo0.read().data().bits();
+        let fbv_sample = adc0.ssfifo0.read().data().bits();
 
-        let mut hv_pid = HV_PID.borrow(cs).borrow_mut();
-        set_hv_pwm(hv_pid.update(av_sample as f32) as u16);
+        let mut loop_anode = LOOP_ANODE.borrow(cs).borrow_mut();
+        let mut loop_cathode = LOOP_CATHODE.borrow(cs).borrow_mut();
+        loop_anode.adc_input(av_sample);
+        loop_cathode.adc_input(fbi_sample, fd_sample, fv_sample, fbv_sample);
+
+        if elapsed.get() % 100 == 0 {
+            board::set_led(1, true);
+            board::set_led(2, false);
+        }
+        if elapsed.get() % 100 == 50 {
+            board::set_led(1, false);
+            board::set_led(2, true);
+        }
     })
 }
 
 #[used]
 #[link_section = ".rodata.exceptions"]
 pub static EXCEPTIONS: ExceptionHandlers = ExceptionHandlers {
-    sys_tick: sys_tick,
     ..cortex_m::exception::DEFAULT_HANDLERS
 };
 
diff --git a/firmware/src/pid.rs b/firmware/src/pid.rs
index 10e15df..3b0921b 100644
--- a/firmware/src/pid.rs
+++ b/firmware/src/pid.rs
@@ -1,3 +1,5 @@
+use core::num::Float;
+
 #[derive(Clone, Copy)]
 pub struct Parameters {
     pub kp: f32,
@@ -60,6 +62,14 @@ impl Controller {
         self.target = target
     }
 
+    #[allow(dead_code)]
+    pub fn is_within(&mut self, tolerance: f32) -> bool {
+        match self.last_input {
+            None => false,
+            Some(last_input) => (last_input - self.target).abs() < tolerance
+        }
+    }
+
     #[allow(dead_code)]
     pub fn reset(&mut self) {
         self.integral = 0.0;