     moveJoints(2, leanRight);
     moveJoints(2, rotateFromLeftToCenter);
 
-    if (getDistance() <= STOP_DISTANCE)
+    distance = getDistance();
+
+    if (distance > 0 && distance <= STOP_DISTANCE)
     {
         Move stepDown[] = {
             Move(&rightAnkle, Joint::POS_MAX, Joint::POS_MIDDLE),

    moveJoints(2, leanLeft);
    moveJoints(2, rotateFromRightToCenter);

    if (getDistance() <= STOP_DISTANCE)
    {
        Move stepDown[] = {
            Move(&rightAnkle, Joint::POS_MIN, Joint::POS_MIDDLE),

}

void stepForward() {
    int distance = 0;
    
    Move stepLeft[] = {
        Move(&rightAnkle, Joint::POS_MIN, Joint::POS_MIDDLE),
        Move(&leftAnkle, Joint::POS_MIN, Joint::POS_MIDDLE),
