Solved a minor bug in the stairs example.

Stairs algorithm tested in the real robot.

src/examples/darwin_stairs_test.cpp

@@ -23,15 +23,15 @@ int main(int argc, char *argv[])
     darwin.mm_enable_power();
     sleep(1);
     // printf parameters
-    std::cout << darwin.stairs_get_phase(SHIFT_WEIGHT_LEFT) << std::endl;
-    std::cout << darwin.stairs_get_phase(RISE_RIGHT_FOOT) << std::endl;
-    std::cout << darwin.stairs_get_phase(ADVANCE_RIGHT_FOOT) << std::endl;
-    std::cout << darwin.stairs_get_phase(CONTACT_RIGHT_FOOT) << std::endl;
-    std::cout << darwin.stairs_get_phase(SHIFT_WEIGHT_RIGHT) << std::endl;
-    std::cout << darwin.stairs_get_phase(RISE_LEFT_FOOT) << std::endl;
-    std::cout << darwin.stairs_get_phase(ADVANCE_LEFT_FOOT) << std::endl;
-    std::cout << darwin.stairs_get_phase(CONTACT_LEFT_FOOT) << std::endl;
-    std::cout << darwin.stairs_get_phase(CENTER_WEIGHT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(SHIFT_WEIGHT_LEFT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(RISE_RIGHT_FOOT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(ADVANCE_RIGHT_FOOT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(CONTACT_RIGHT_FOOT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(SHIFT_WEIGHT_RIGHT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(RISE_LEFT_FOOT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(ADVANCE_LEFT_FOOT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(CONTACT_LEFT_FOOT) << std::endl;
+    std::cout << darwin.stairs_get_phase_time(CENTER_WEIGHT) << std::endl;
     for(i=0;i<MAX_NUM_SERVOS;i++)
     {
       darwin.mm_enable_servo(i);
@@ -58,7 +58,14 @@ int main(int argc, char *argv[])
     darwin.stairs_go_up();
     while(darwin.stairs_is_active())
     {
-      std::cout << "Going up stairs ..." << std::endl;
+      std::cout << "Going up stairs ... phase" << std::hex << darwin.stairs_get_phase() << std::endl;
+      usleep(100000);
+    }
+    std::cout << "Go downstairs ..." << std::endl;
+    darwin.stairs_go_down();
+    while(darwin.stairs_is_active())
+    {
+      std::cout << "Going down stairs ... phase" << std::hex << darwin.stairs_get_phase() << std::endl;
       usleep(100000);
     }
     darwin.mm_stop();