 
     if (!started)
     {
-        Move startLeanLeft[] = { 
-            Move(&rightAnkle, Joint::POS_MIDDLE, Joint::POS_MIN), 
-            Move(&leftAnkle, Joint::POS_MIDDLE, Joint::POS_MIN)
-        };
-
         delay(5000);
-
-        moveJoints(2, startLeanLeft);
-
+        startStepFoward();
         started = true;
     }
 
-    if (!stop)
-    {
-        stepForward();
-    }
-}
-
+    stepForward();
+}
+

        };

        moveJoints(2, stepDown);
        
        stop = true;
        return;
    }
    

#define LEFT_HIP_PIN            2
#define SR04_TRIGGER_PIN        8
#define SR04_ECHO_PIN           9
#define RANDOM_SEED_PIN         A0      // Take noise from unconnected analog pin to seed RNG

#define DELAY                   1500
#define STOP_DISTANCE           20

struct Joint
{
