     moveJoints(4, stepLeft);
     moveJoints(2, leanRight);
     moveJoints(2, rotateFromLeftToCenter);
+
+    if (getDistance() <= STOP_DISTANCE)
+    {
+        Move stepDown[] = {
+            Move(&rightAnkle, Joint::POS_MAX, Joint::POS_MIDDLE),
+            Move(&leftAnkle, Joint::POS_MAX, Joint::POS_MIDDLE),
+        };
+
+        moveJoints(2, stepDown);
+        
+        stop = true;
+        return;
+    }
+    
     moveJoints(4, stepRight);
     moveJoints(2, leanLeft);
     moveJoints(2, rotateFromRightToCenter);
+
+    if (getDistance() <= STOP_DISTANCE)
+    {
+        Move stepDown[] = {
+            Move(&rightAnkle, Joint::POS_MIN, Joint::POS_MIDDLE),
+            Move(&leftAnkle, Joint::POS_MIN, Joint::POS_MIDDLE),
+        };
+
+        moveJoints(2, stepDown);
+
+        stop = true;
+        return;
+    }
 }
 
 void setup() {
+    pinMode(SR04_TRIGGER_PIN, OUTPUT);
+    pinMode(SR04_ECHO_PIN, INPUT);
+    
     rightAnkle.init();
     leftAnkle.init();
     rightHip.init();
     leftHip.init();
 }
 
-bool started = false;
-
 void loop()
 {
     int i = 0;

        started = true;
    }

    stepForward();
}


#include <limits.h>
#include <Servo.h>

#define RIGHT_ANKLE_POS_MIN     1050
#define RIGHT_ANKLE_POS_MIDDLE  1510
#define RIGHT_ANKLE_POS_MAX     1860

#define LEFT_ANKLE_POS_MIN      1140
#define LEFT_ANKLE_POS_MIDDLE   1490
#define LEFT_ANKLE_POS_MAX      2190

#define RIGHT_HIP_POS_MIN       950
#define RIGHT_HIP_POS_MIDDLE    1290
#define RIGHT_HIP_POS_MAX       1600

#define LEFT_HIP_POS_MIN        1100
#define LEFT_HIP_POS_MIDDLE     1500
#define LEFT_HIP_POS_MAX        1930

#define RIGHT_ANKLE_PIN         5
#define LEFT_ANKLE_PIN          4
#define RIGHT_HIP_PIN           3
#define LEFT_HIP_PIN            2
#define SR04_TRIGGER_PIN        8
#define SR04_ECHO_PIN           9

#define DELAY 2
#define STOP_DISTANCE           10

struct Joint
{
