diff --git a/Src/control.c b/Src/control.c
index c3ec8df..9ffdfd2 100644
--- a/Src/control.c
+++ b/Src/control.c
@@ -105,8 +105,6 @@ void Nunchuck_Read() {
   HAL_Delay(5);
   if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 100) == HAL_OK) {
     timeout = 0;
-  } else {
-    timeout++;
   }
 
   if (timeout > 3) {
diff --git a/Src/main.c b/Src/main.c
index 96b2bef..4cc2624 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -226,11 +226,10 @@ int main(void) {
     #endif
 
     #ifdef CONTROL_MOTOR_TEST
-      // if (main_loop_counter % 2 == 0) {
-        if (motor_test_direction == 1) cmd2 += 1;
-        else cmd2 -= 1;
-        if (abs(cmd2) > CONTROL_MOTOR_TEST_MAX_SPEED) motor_test_direction = -motor_test_direction;
-      // }
+      if (motor_test_direction == 1) cmd2 += 1;
+      else cmd2 -= 1;
+      if (abs(cmd2) > CONTROL_MOTOR_TEST_MAX_SPEED) motor_test_direction = -motor_test_direction;
+
       timeout = 0;
     #endif
 
@@ -327,6 +326,7 @@ int main(void) {
     }
     
     main_loop_counter += 1;
+    timeout++;
   }
 }