 #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 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 SR04_TRIGGER_PIN        8
+#define SR04_ECHO_PIN           9
 
 #define DELAY 2
+#define STOP_DISTANCE           10
 
 struct Joint
 {

        started = true;
    }

    stepForward();
}


Joint rightHip(RIGHT_HIP_PIN, RIGHT_HIP_POS_MIN, RIGHT_HIP_POS_MIDDLE, RIGHT_HIP_POS_MAX);
Joint leftHip(LEFT_HIP_PIN, LEFT_HIP_POS_MIN, LEFT_HIP_POS_MIDDLE, LEFT_HIP_POS_MAX);

bool started = false;
bool stop = false;

long getDistance()
{
    long duration = 0;
    
    digitalWrite(SR04_TRIGGER_PIN, LOW);
    delayMicroseconds(5);
    digitalWrite(SR04_TRIGGER_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(SR04_TRIGGER_PIN, LOW);

    duration = pulseIn(SR04_ECHO_PIN, HIGH);

    return duration * 0.01716;
}

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