У вас проблема потому, что вы на самом деле получаете координаты, а не пройденные расстояния. А думаете, что расстояния. Можно считать через расстояния, но там надо учитывать развороты тел... там сложно и с модулями. Считайте через координаты.Формулы для y1 и y2 (у вас это S1 и S2) - верные.S = |y2–y1|