Fix the Timer 2 interrupt priority issue
Integrate the angle velocity from the gyroscope