Updated final code (edit 2)
Updated Gyro Code