diff --git a/Pedestal/MCDU/firmware/src/main.rs b/Pedestal/MCDU/firmware/src/main.rs
index a8fc697..129f1d0 100644
--- a/Pedestal/MCDU/firmware/src/main.rs
+++ b/Pedestal/MCDU/firmware/src/main.rs
@@ -37,12 +37,11 @@ macro_rules! define_output_states {
 	};
 }
 
-// TODO compiler throws error when initializing pa15 and pb4
 struct MyPins {
 	// Input Col select
 	pa4: Pin<'A', 4, Output<PushPull>>,
 	pa5: Pin<'A', 5, Output<PushPull>>,
-//	pa15: Pin<'A', 15, Output<PushPull>>,
+	pa15: Pin<'A', 15, Output<PushPull>>,
 	pb6: Pin<'B', 6, Output<PushPull>>,
 	pb7: Pin<'B', 7, Output<PushPull>>,
 	pb15: Pin<'B', 15, Output<PushPull>>,
@@ -57,7 +56,7 @@ struct MyPins {
 	pa1: Pin<'A', 1, Input<PullDown>>,
 	pa2: Pin<'A', 2, Input<PullDown>>,
 	pa8: Pin<'A', 8, Input<PullDown>>,
-//	pb4: Pin<'B', 4, Input<PullDown>>,
+	pb4: Pin<'B', 4, Input<PullDown>>,
 	pb5: Pin<'B', 5, Input<PullDown>>,
 	pb12: Pin<'B', 12, Input<PullDown>>,
 	pb13: Pin<'B', 13, Input<PullDown>>,
@@ -100,6 +99,9 @@ fn main() -> ! {
 		.pclk1(24.MHz())
 		.freeze(&mut flash.acr);
 
+	let mut afio = p.AFIO.constrain();
+	let (pa15, pb3, pb4) = afio.mapr.disable_jtag(gpioa.pa15, gpiob.pb3, gpiob.pb4); // To allow us to use pa15 and pb4
+
 	// ====================== USB setup =================
 	assert!(clocks.usbclk_valid());
 	let mut usb_dp = gpioa.pa12.into_push_pull_output(&mut gpioa.crh);
@@ -143,7 +145,7 @@ fn main() -> ! {
 		// Inputs Col Select
 		pa4: gpioa.pa4.into_push_pull_output(&mut gpioa.crl),
 		pa5: gpioa.pa5.into_push_pull_output(&mut gpioa.crl),
-//		pa15: gpioa.pa15.into_push_pull_output(&mut gpioa.crh),
+		pa15: pa15.into_push_pull_output(&mut gpioa.crh),
 		pb6: gpiob.pb6.into_push_pull_output(&mut gpiob.crl),
 		pb7: gpiob.pb7.into_push_pull_output(&mut gpiob.crl),
 		pb15: gpiob.pb15.into_push_pull_output(&mut gpiob.crh),
@@ -158,7 +160,7 @@ fn main() -> ! {
 		pa1: gpioa.pa1.into_pull_down_input(&mut gpioa.crl),
 		pa2: gpioa.pa2.into_pull_down_input(&mut gpioa.crl),
 		pa8: gpioa.pa8.into_pull_down_input(&mut gpioa.crh),
-//		pb4: gpiob.pb4.into_pull_down_input(&mut gpiob.crl),
+		pb4: pb4.into_pull_down_input(&mut gpiob.crl),
 		pb5: gpiob.pb5.into_pull_down_input(&mut gpiob.crl),
 		pb12: gpiob.pb12.into_pull_down_input(&mut gpiob.crh),
 		pb13: gpiob.pb13.into_pull_down_input(&mut gpiob.crh),