 #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
 {

    moveJoints(4, stepLeft);
    moveJoints(2, leanRight);
    moveJoints(2, rotateFromLeftToCenter);
    moveJoints(4, stepRight);
    moveJoints(2, leanLeft);
    moveJoints(2, rotateFromRightToCenter);
}

void setup() {
    rightAnkle.init();
    leftAnkle.init();
    rightHip.init();
    leftHip.init();
}

bool started = false;

void loop()
{
    int i = 0;

        started = true;
    }

    if (!stop)
    {
        stepForward();
    }
}

