Ese es otro problema y algo que tu puedes resolver, asi que pelea un poco con el código.
Es algo en las sumas y restas así que presta atención a eso, y por como esta hecho tu código lo haces varias veces lo cual es un error.
Las sumas y restas de los sitios deben controlarse en un solo sitio.
Tu loop debe comandar las rutinas adecuadas y tu desde ahi debes poder VISUALIZAR el control del programa.
Tal como está es dificil verlo.
Ahora no me es posible pero luego le pongo unos cuantos minutos y lo rehago.