     delayMicroseconds(10);
     digitalWrite(SR04_TRIGGER_PIN, LOW);
 
-    duration = pulseIn(SR04_ECHO_PIN, HIGH);
+    duration = pulseIn(SR04_ECHO_PIN, HIGH, 3000);
 
     return duration * 0.01716;
 }

}

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

    moveJoints(2, leanRight);
    moveJoints(2, rotateFromLeftToCenter);

    distance = getDistance();

    if (distance > 0 && distance <= STOP_DISTANCE)
    {
        Move stepDown[] = {
            Move(&rightAnkle, Joint::POS_MAX, Joint::POS_MIDDLE),
