diff --git a/bbb_cape/src/cape/fill_packet.c b/bbb_cape/src/cape/fill_packet.c
index 29ea095..97f9d8a 100644
--- a/bbb_cape/src/cape/fill_packet.c
+++ b/bbb_cape/src/cape/fill_packet.c
@@ -86,6 +86,8 @@
   led_set(LED_ERR, 0);
   gyro_init();
 
+  robot_init();
+
   uart_common_configure(750000);
   uart_dma_configure(DATA_STRUCT_SEND_SIZE, buffer1, buffer2);
 }
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index 57e3d66..aabd8d3 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -93,21 +93,20 @@
 
 void TIM1_TRG_COM_TIM11_IRQHandler(void) {
   TIM11->SR = ~TIM_SR_CC1IF;
-  TIM11->CR1 = 0;
-  ultrasonic_pulse_length = TIM11->CCR1;
-  ultrasonic_pulse_length = 1;
-  TIM11->CNT = 0;
+  if (digital_read(6)) {
+    TIM11->EGR = TIM_EGR_UG;
+  } else {
+    ultrasonic_pulse_length = TIM11->CCR1;
+  }
 }
 
 void robot_init(void) {
   gpio_setup_alt(GPIOB, 9, 3);
   RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
   TIM11->CR1 = TIM_CR1_URS;
-  TIM11->SMCR = 5 << 4 /* TI1 */ |
-      6 << 0 /* start on rising edge */;
   TIM11->DIER = TIM_DIER_CC1IE;
   TIM11->CCMR1 = TIM_CCMR1_CC1S_0 /* input pin 1 -> timer input 1 */;
-  TIM11->CCER = TIM_CCER_CC1P /* go on falling edge */ | TIM_CCER_CC1E;
+  TIM11->CCER = TIM_CCER_CC1P | TIM_CCER_CC1NP | TIM_CCER_CC1E;
   TIM11->EGR = TIM_EGR_UG;
   TIM11->PSC = 1200 - 1;  // 100kHZ timer
   TIM11->CR1 |= TIM_CR1_CEN;