diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 62f3e82c692ba07e83e45a7835cbaa14d0b38336..ea57e49244a2fb75427ba2343d565d99cadf03bf 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -326,12 +326,11 @@ int main()
       high = inp(ADCH);
       pos = 0; // reset position
       angleOffset =  ((int16_t) ((high<<8) | low) - 512);
-      //angleOffset = -150;
-      sei();
       oldPos = 0;
       velEst = 0; // reset velocity estimate
       I = 0; // reset integral part of controller
       u = 0; // reset ctrl signal
+      sei();
     } 
   }
 }