diff --git a/src/mpu/sensor.rs b/src/mpu/sensor.rs
index d64c214a71a8193db99657b9ca45ff0f225ec54c..376c2d7e8fb8cc8feb18cfcc48efd62bda71e645 100644
--- a/src/mpu/sensor.rs
+++ b/src/mpu/sensor.rs
@@ -7,6 +7,7 @@ use crate::time::delay_ms_assembly;
 use core::marker::PhantomData;
 use core::time::Duration;
 use embedded_hal::blocking::i2c::{Write, WriteRead};
+use crate::led::Green;
 use crate::twi::TwiWrapper;
 
 const MPU6050_ADDRESS: u8 = 0x68;
diff --git a/src/twi.rs b/src/twi.rs
index b5dcfb4d8f99ecf1e87d6b028ee02447b282cc48..7660050d6d166cc10650244c5ed6c4a6316d156b 100644
--- a/src/twi.rs
+++ b/src/twi.rs
@@ -3,7 +3,7 @@ use crate::once_cell::OnceCell;
 use core::sync::atomic::{AtomicBool, Ordering};
 use cortex_m::peripheral::NVIC;
 use nrf51_hal::gpio::p0::{P0_02, P0_04};
-use nrf51_hal::gpio::{Disconnected, Level, Pin};
+use nrf51_hal::gpio::{Disconnected, Pin};
 use nrf51_pac::interrupt;
 use nrf51_pac::twi0::frequency::FREQUENCY_A;
 use nrf51_pac::{Interrupt, GPIO, TWI0};
@@ -68,7 +68,7 @@ impl TwiWrapper {
             self.twi.shorts.write(|w| w.bb_suspend().set_bit())
         }
 
-        self.twi.tasks_starttx.write(|w| unsafe{w.bits(1)});
+        self.twi.tasks_startrx.write(|w| unsafe{w.bits(1)});
 
         let mut bytes_left = data.len();
         let mut write_ptr = 0;
@@ -179,10 +179,9 @@ pub(crate) fn initialize(
             .set_bit()
             .error()
             .set_bit()
-            .stopped()
-            .set_bit()
     });
 
+    twi.shorts.reset();
     twi.enable.write(|w| w.enable().enabled());
 
     // Initialize oncecell
@@ -207,8 +206,6 @@ pub(crate) fn initialize(
 
 #[interrupt]
 unsafe fn SPI0_TWI0() {
-    let _ = unsafe { nrf51_hal::gpio::p0::Parts::new(nrf51_pac::Peripherals::steal().GPIO).p0_19.into_push_pull_output(Level::High) };
-
     // Safety: interrupts are already turned off here, since we are inside an interrupt
     // We might be accessing the hardware while the interrupted code also wants to, this is fine since we're only touching the EVENT registers which are not touched by the other code
     let twi = unsafe { TWI.no_critical_section_lock_mut() };
@@ -229,7 +226,4 @@ unsafe fn SPI0_TWI0() {
             .write(|w| w.anack().clear_bit().overrun().clear_bit()); // Clear error source
         twi.twi.events_error.reset();
     }
-
-    let _ = unsafe { nrf51_hal::gpio::p0::Parts::new(nrf51_pac::Peripherals::steal().GPIO).p0_19.into_push_pull_output(Level::Low) };
-
 }