From e7f7ac6a7fba3a0d29eeebaa326990defdb7b74a Mon Sep 17 00:00:00 2001
From: ferranmafe <darwin@darwin.users.iri.prv>
Date: Thu, 31 Aug 2017 22:47:20 +0200
Subject: [PATCH] Solved a minor bug in the stairs example. Stairs algorithm
tested in the real robot.
---
src/examples/darwin_stairs_test.cpp | 27 +++++++++++++++++----------
1 file changed, 17 insertions(+), 10 deletions(-)
diff --git a/src/examples/darwin_stairs_test.cpp b/src/examples/darwin_stairs_test.cpp
index b31f59b..29111e2 100644
--- a/src/examples/darwin_stairs_test.cpp
+++ b/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();
--
GitLab