diff --git a/controller/src/sensor_manager.rs b/controller/src/sensor_manager.rs
index 329ccd6..7622946 100644
--- a/controller/src/sensor_manager.rs
+++ b/controller/src/sensor_manager.rs
@@ -34,9 +34,7 @@ static POLL_STATE: AtomicU8 = AtomicU8::new(PollState::None as u8);
 #[repr(u8)]
 enum PollState {
     None,
-    TofL,
-    TofR,
-    TofS,
+    Tof,
     Gyro,
 }
 
@@ -45,18 +43,24 @@ async fn init_sensors(mut hardware: SensorHardware) {
     let bus = RefCell::new(hardware.bus_tof);
 
     Timer::after_millis(1).await;
-    let mut tof_s = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
-    tof_s.set_address(0x32).unwrap();
+    let mut tof_s = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
+    if let Some(ref mut tof_s) = tof_s {
+        tof_s.set_address(0x32).unwrap();
+    }
 
     hardware.tof_l_enable.set_high();
     Timer::after_micros(1200).await; // DS11555 3.2
-    let mut tof_l = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
-    tof_l.set_address(0x33).unwrap();
+    let mut tof_l = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
+    if let Some(ref mut tof_l) = tof_l {
+        tof_l.set_address(0x33).unwrap();
+    }
 
     hardware.tof_r_enable.set_high();
     Timer::after_micros(1200).await; // DS11555 3.2
-    let mut tof_r = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
-    tof_r.set_address(0x34).unwrap();
+    let mut tof_r = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
+    if let Some(ref mut tof_r) = tof_r {
+        tof_r.set_address(0x34).unwrap();
+    }
 
     //let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
     //gyro.init(&mut Delay).unwrap();
@@ -71,30 +75,20 @@ async fn init_sensors(mut hardware: SensorHardware) {
         gyro: None,
         accel: None,
     };
-    let mut updated_ts = Instant::now();
 
     loop {
-        let mut updated = false;
-        POLL_STATE.store(PollState::TofS as u8, Ordering::SeqCst);
-        if let Ok(dist) = tof_s.read_range_single_millimeters_blocking() {
-            data.tof_s = Some(dist);
-            updated = true;
+        POLL_STATE.store(PollState::Tof as u8, Ordering::SeqCst);
+        for (tof, store) in 
+            [(&mut tof_s, &mut data.tof_s), (&mut tof_l, &mut data.tof_l), (&mut tof_r, &mut data.tof_r)] {
+            if let Some(ref mut tof) = tof {
+                if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
+                    *store = Some(dist);
+                } else {
+                    *store = None;
+                }
+                Timer::after_millis(3).await;
+            }
         }
-        Timer::after_millis(3).await;
-
-        POLL_STATE.store(PollState::TofL as u8, Ordering::SeqCst);
-        if let Ok(dist) = tof_l.read_range_single_millimeters_blocking() {
-            data.tof_l = Some(dist);
-            updated = true;
-        }
-        Timer::after_millis(3).await;
-
-        POLL_STATE.store(PollState::TofR as u8, Ordering::SeqCst);
-        if let Ok(dist) = tof_r.read_range_single_millimeters_blocking() {
-            data.tof_r = Some(dist);
-            updated = true;
-        }
-        Timer::after_millis(3).await;
 
         //POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
         //if let Ok(gyro) = gyro.get_gyro() {
@@ -106,12 +100,9 @@ async fn init_sensors(mut hardware: SensorHardware) {
         //    data.accel = Some(accel);
         //    updated = true;
         //}
-        if updated {
-            updated_ts = Instant::now();
-        }
         POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
 
         info!("sensors: {data:?}");
-        //CHANNEL.send((data.clone(), updated_ts.clone())).await;
+        CHANNEL.send((data.clone(), Instant::now())).await;
     }
 }