http://wonderfl.net/c/k6M8

上のサイトのプログラムで、
スタートからゴールまでの総コスト（1マスの移動コストを1として、沼の移動コストは2）を
カウントして表示させることはできるのでしょうか？
for文をうまいこと使ってできないかと思ってるのですがなかなかうまくいきません。
どなたかわかる方、よければどのように記述すれば良いのか教えていただけるとうれしいです。