Kodlamadan anlayan arkadaşlar delirmek üzereyim bi türlü halledemedim lütfen yardım edin!

Arkadaşlar servonun 20 cm den yakın engel gördüğü andaki açıda durmasını istediğim bir program yazmaya çalışıyorum ama bi türlü olmadı. Servo 20 cm den küçük engel gördüğü anda taramaya başladığı andaki pozisyonuna geri dönüyor. Lütfen yardım edin. Kodum burda http://pastebin.com/nMTRvP30 Nerde hata yapıyorum anlamadım tara fonksiyonum sk verisini döndürüyor ama sk=i yaptığımda for döngüsündeki i sayacının o anki değerini sk ile dışarı çıkartmaya çalışıyorum ama denemediğim şey kalmadı sk’yı i ye eşitleyip dışarı çıkaramıyorum. Lütfen düzeltmeme yardım edin.