From 2c828d1f32dc509d8242bb0e420c59e00c100f90 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 17 May 2022 13:32:48 +0200 Subject: [PATCH 1/4] Introduce a small delay before immediately issueing the recording commands within the Record state. --- examples/Braccio_Record_and_Replay/AppState.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/Braccio_Record_and_Replay/AppState.cpp b/examples/Braccio_Record_and_Replay/AppState.cpp index 10108da..1385059 100644 --- a/examples/Braccio_Record_and_Replay/AppState.cpp +++ b/examples/Braccio_Record_and_Replay/AppState.cpp @@ -152,6 +152,7 @@ void RecordState::onEnter() lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); Braccio.disengage(); + delay(100); sample_cnt = 0; } From c4182dd2f568c47a1ca9c997a2904ef97b0ca4bb Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 17 May 2022 13:41:52 +0200 Subject: [PATCH 2/4] Instead of a blanket delay regularly check if the home position has been reached. This way we can be sure that the arm has indeed reached the home position. --- .../Braccio_Record_and_Replay/AppState.cpp | 31 ++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/examples/Braccio_Record_and_Replay/AppState.cpp b/examples/Braccio_Record_and_Replay/AppState.cpp index 1385059..803452c 100644 --- a/examples/Braccio_Record_and_Replay/AppState.cpp +++ b/examples/Braccio_Record_and_Replay/AppState.cpp @@ -271,7 +271,36 @@ void ZeroState::onEnter() Braccio.engage(); delay(100); Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); - delay(500); + + auto isInHomePos = []() -> bool + { + float current_angles[SmartServoClass::NUM_MOTORS] = {0}; + Braccio.positions(current_angles); + + float total_angle_err = 0.0; + for (size_t i = 0; i < SmartServoClass::NUM_MOTORS; i++) + total_angle_err += fabs(current_angles[i] - HOME_POS[i]); + + static float const TOTAL_EPSILON = 10.0f; + bool const is_in_home_pos = (total_angle_err < TOTAL_EPSILON); + return is_in_home_pos; + }; + auto isTimeout = [](unsigned long const start) -> bool + { + /* Timeout of one second. */ + auto const now = millis(); + if ((now - start) > 1000) + return true; + else + return false; + }; + + /* Wait until we've returned to the home position + * with a timeout (i.e. we leave this function) + * after one second even if we can't fully reach + * the home position. + */ + for(auto start = millis(); !isInHomePos() && !isTimeout(start); delay(100)) { } } void ZeroState::onExit() From 6a54383d0ccf23bebdeaabb18db1627c89902c3b Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 17 May 2022 13:47:01 +0200 Subject: [PATCH 3/4] Only allow to press the zero position button from the idle state. --- .../Braccio_Record_and_Replay/AppState.cpp | 18 +++++++++--------- examples/Braccio_Record_and_Replay/AppState.h | 7 ++++--- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/examples/Braccio_Record_and_Replay/AppState.cpp b/examples/Braccio_Record_and_Replay/AppState.cpp index 803452c..b592780 100644 --- a/examples/Braccio_Record_and_Replay/AppState.cpp +++ b/examples/Braccio_Record_and_Replay/AppState.cpp @@ -108,15 +108,6 @@ void custom_main_menu() Braccio.connectJoystickTo(btnm); } -/************************************************************************************** - * State - **************************************************************************************/ - -State * State::handle_OnZeroPosition() -{ - return new ZeroState(); -} - /************************************************************************************** * IdleState **************************************************************************************/ @@ -141,6 +132,11 @@ State * IdleState::handle_OnReplay() return new ReplayState(); } +State * IdleState::handle_OnZeroPosition() +{ + return new ZeroState(); +} + /************************************************************************************** * RecordState **************************************************************************************/ @@ -150,6 +146,7 @@ void RecordState::onEnter() btnm_map[0] = "STOP"; lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_CHECKED); lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); Braccio.disengage(); delay(100); @@ -161,6 +158,7 @@ void RecordState::onExit() btnm_map[0] = "RECORD"; lv_btnmatrix_clear_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_CHECKED); lv_btnmatrix_clear_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_clear_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); lv_label_set_text_fmt(counter, "Counter: %d" , 0); Braccio.engage(); @@ -218,6 +216,7 @@ void ReplayState::onEnter() btnm_map[2] = "STOP"; lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_DISABLED); lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_CHECKED); + lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); } void ReplayState::onExit() @@ -225,6 +224,7 @@ void ReplayState::onExit() btnm_map[2] = "REPLAY"; lv_btnmatrix_clear_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_DISABLED); lv_btnmatrix_clear_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_CHECKED); + lv_btnmatrix_clear_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); lv_label_set_text_fmt(counter, "Counter: %d" , 0); } diff --git a/examples/Braccio_Record_and_Replay/AppState.h b/examples/Braccio_Record_and_Replay/AppState.h index 036a827..a682d32 100644 --- a/examples/Braccio_Record_and_Replay/AppState.h +++ b/examples/Braccio_Record_and_Replay/AppState.h @@ -54,7 +54,7 @@ class State protected: virtual State * handle_OnRecord() { return this; } virtual State * handle_OnReplay() { return this; } - virtual State * handle_OnZeroPosition(); + virtual State * handle_OnZeroPosition() { return this; } virtual State * handle_OnTimerTick() { return this; } }; @@ -67,8 +67,9 @@ class IdleState : public State virtual void onExit() override; protected: - virtual State * handle_OnRecord() override; - virtual State * handle_OnReplay() override; + virtual State * handle_OnRecord () override; + virtual State * handle_OnReplay () override; + virtual State * handle_OnZeroPosition() override; }; class RecordState : public State From b346e4db8d0b9121bf852636daa770211e437f58 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 18 May 2022 06:35:59 +0200 Subject: [PATCH 4/4] Ensure a mininum delay between two consecutive Braccio++ actions. --- examples/Braccio_Record_and_Replay/AppState.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/Braccio_Record_and_Replay/AppState.cpp b/examples/Braccio_Record_and_Replay/AppState.cpp index b592780..d2b1317 100644 --- a/examples/Braccio_Record_and_Replay/AppState.cpp +++ b/examples/Braccio_Record_and_Replay/AppState.cpp @@ -148,6 +148,7 @@ void RecordState::onEnter() lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); + delay(100); Braccio.disengage(); delay(100); sample_cnt = 0; @@ -161,7 +162,9 @@ void RecordState::onExit() lv_btnmatrix_clear_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); lv_label_set_text_fmt(counter, "Counter: %d" , 0); + delay(100); Braccio.engage(); + delay(100); } State * RecordState::handle_OnRecord() @@ -268,6 +271,7 @@ void ZeroState::onEnter() lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); + delay(100); Braccio.engage(); delay(100); Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);