diff --git a/examples/Braccio_Learn_and_Repeat/AppState.cpp b/examples/Braccio_Record_and_Replay/AppState.cpp similarity index 100% rename from examples/Braccio_Learn_and_Repeat/AppState.cpp rename to examples/Braccio_Record_and_Replay/AppState.cpp diff --git a/examples/Braccio_Learn_and_Repeat/AppState.h b/examples/Braccio_Record_and_Replay/AppState.h similarity index 100% rename from examples/Braccio_Learn_and_Repeat/AppState.h rename to examples/Braccio_Record_and_Replay/AppState.h diff --git a/examples/Braccio_Learn_and_Repeat/Braccio_Learn_and_Repeat.ino b/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino similarity index 100% rename from examples/Braccio_Learn_and_Repeat/Braccio_Learn_and_Repeat.ino rename to examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino diff --git a/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino b/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino new file mode 100644 index 0000000..a7e99a7 --- /dev/null +++ b/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino @@ -0,0 +1,37 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +#include "AppState.h" + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +LearnAndRepeatApp app; + +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ + +void setup() +{ + if (Braccio.begin(custom_main_menu)) { + app.enableButtons(); + } +} + +void loop() +{ + /* Only execute every 50 ms. */ + static auto prev = millis(); + auto const now = millis(); + + if ((now - prev) > 50) + { + prev = now; + app.update(EventSource::TimerTick); + } +} diff --git a/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/AppState.cpp b/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/AppState.cpp new file mode 100644 index 0000000..d8317d2 --- /dev/null +++ b/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/AppState.cpp @@ -0,0 +1,256 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include "AppState.h" + +/************************************************************************************** + * DEFINE + **************************************************************************************/ + +#define COLOR_TEAL 0x00878F +#define COLOR_LIGHT_TEAL 0x62AEB2 +#define COLOR_ORANGE 0xE47128 + +#define BUTTON_ENTER 6 + +/************************************************************************************** + * CONSTANT + **************************************************************************************/ + +static int const SAMPLE_BUF_SIZE = 6*200*2; /* 20 seconds. */ +static float const HOME_POS[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +lv_obj_t * counter; +lv_obj_t * btnm; +const char * btnm_map[] = { "RECORD", "\n", "REPLAY", "\n", "ZERO_POSITION", "\n", "\0" }; + +static float sample_buf[SAMPLE_BUF_SIZE]; +static int sample_cnt; + +extern LearnAndRepeatApp app; + +/************************************************************************************** + * FUNCTION DEFINITION + **************************************************************************************/ + +static void event_handler_menu(lv_event_t * e) +{ + lv_event_code_t code = lv_event_get_code(e); + + if (code == LV_EVENT_CLICKED || (code == LV_EVENT_KEY && Braccio.getKey() == BUTTON_ENTER)) + { + lv_obj_t * obj = lv_event_get_target(e); + uint32_t const id = lv_btnmatrix_get_selected_btn(obj); + + switch (id) + { + case 0: app.update(EventSource::Button_Record); break; + case 1: app.update(EventSource::Button_Replay); break; + case 2: app.update(EventSource::Button_ZeroPosition); break; + } + } +} + +void custom_main_menu() +{ + Braccio.lvgl_lock(); + static lv_style_t style_focus; + lv_style_init(&style_focus); + lv_style_set_outline_color(&style_focus, lv_color_hex(COLOR_ORANGE)); + lv_style_set_outline_width(&style_focus, 4); + + static lv_style_t style_btn; + lv_style_init(&style_btn); + lv_style_set_bg_color(&style_btn, lv_color_hex(COLOR_LIGHT_TEAL)); + lv_style_set_text_color(&style_btn, lv_color_white()); + + btnm = lv_btnmatrix_create(lv_scr_act()); + lv_obj_set_size(btnm, 240, 240); + lv_btnmatrix_set_map(btnm, btnm_map); + lv_obj_align(btnm, LV_ALIGN_CENTER, 0, 0); + + lv_obj_add_style(btnm, &style_btn, LV_PART_ITEMS); + lv_obj_add_style(btnm, &style_focus, LV_PART_ITEMS | LV_STATE_FOCUS_KEY); + + lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); + + lv_btnmatrix_set_one_checked(btnm, true); + lv_btnmatrix_set_selected_btn(btnm, 0); + lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); + + counter = lv_label_create(btnm); + lv_label_set_text_fmt(counter, "Counter: %d" , 0); + lv_obj_align(counter, LV_ALIGN_CENTER, 0, 80); + + lv_obj_add_event_cb(btnm, event_handler_menu, LV_EVENT_ALL, NULL); + Braccio.lvgl_unlock(); + + Braccio.connectJoystickTo(btnm); +} + +/************************************************************************************** + * State + **************************************************************************************/ + +State * State::handle_OnZeroPosition() +{ + return new ZeroState(); +} + +/************************************************************************************** + * IdleState + **************************************************************************************/ + +void IdleState::onEnter() +{ + lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); +} + +void IdleState::onExit() +{ + lv_btnmatrix_clear_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); +} + +State * IdleState::handle_OnRecord() +{ + return new RecordState(); +} + +State * IdleState::handle_OnReplay() +{ + return new ReplayState(); +} + +/************************************************************************************** + * RecordState + **************************************************************************************/ + +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); + + Braccio.disengage(); + sample_cnt = 0; +} + +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_label_set_text_fmt(counter, "Counter: %d" , 0); + + Braccio.engage(); +} + +State * RecordState::handle_OnRecord() +{ + return new IdleState(); +} + +State * RecordState::handle_OnTimerTick() +{ + /* The sample buffer is full. */ + if (sample_cnt >= SAMPLE_BUF_SIZE) { + return new IdleState(); + } + + /* We still have space, let's sample some data. */ + Braccio.positions(sample_buf + sample_cnt); + sample_cnt += 6; + + /* Update sample counter. */ + lv_label_set_text_fmt(counter, "Counter: %d" , (sample_cnt / 6)); + + return this; +} + +/************************************************************************************** + * ReplayState + **************************************************************************************/ + +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); +} + +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_label_set_text_fmt(counter, "Counter: %d" , 0); +} + +State * ReplayState::handle_OnReplay() +{ + return new IdleState(); +} + +State * ReplayState::handle_OnTimerTick() +{ + /* All samples have been replayed. */ + if (_replay_cnt >= sample_cnt) { + return new IdleState(); + } + + /* Replay recorded movements. */ + Braccio.moveTo(sample_buf[_replay_cnt + 0], + sample_buf[_replay_cnt + 1], + sample_buf[_replay_cnt + 2], + sample_buf[_replay_cnt + 3], + sample_buf[_replay_cnt + 4], + sample_buf[_replay_cnt + 5]); + _replay_cnt += 6; + + lv_label_set_text_fmt(counter, "Counter: %d" , (_replay_cnt / 6)); + + return this; +} + +/************************************************************************************** + * ReplayState + **************************************************************************************/ + +State * ZeroState::handle_OnTimerTick() +{ + return new IdleState(); +} + +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); + + 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); +} + +void ZeroState::onExit() +{ + lv_btnmatrix_clear_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); +} + +/************************************************************************************** + * LearnAndRepeatApp + **************************************************************************************/ + +void LearnAndRepeatApp::enableButtons() +{ + /* Enable buttons once init is complete. */ + lv_btnmatrix_clear_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_clear_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_DISABLED); +} \ No newline at end of file diff --git a/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/AppState.h b/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/AppState.h new file mode 100644 index 0000000..036a827 --- /dev/null +++ b/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_Record_and_Replay/AppState.h @@ -0,0 +1,153 @@ +#ifndef LEARN_AND_REPEAT_APP_STATE_H_ +#define LEARN_AND_REPEAT_APP_STATE_H_ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +enum class EventSource +{ + Button_Record, Button_Replay, Button_ZeroPosition, TimerTick +}; + +enum class StateName +{ + Record, Replay, Idle, Zero +}; + +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +void custom_main_menu(); + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class State +{ +public: + virtual ~State() { } + virtual void onEnter() { Serial.println("State::onEnter"); } + virtual void onExit() { Serial.println("State::onExit"); } + virtual StateName name() = 0; + State * update(EventSource const evt_src) + { + State * next_state = this; + switch (evt_src) + { + case EventSource::Button_Record: next_state = handle_OnRecord(); break; + case EventSource::Button_Replay: next_state = handle_OnReplay(); break; + case EventSource::Button_ZeroPosition: next_state = handle_OnZeroPosition(); break; + case EventSource::TimerTick: next_state = handle_OnTimerTick(); break; + } + return next_state; + } + +protected: + virtual State * handle_OnRecord() { return this; } + virtual State * handle_OnReplay() { return this; } + virtual State * handle_OnZeroPosition(); + virtual State * handle_OnTimerTick() { return this; } +}; + +class IdleState : public State +{ +public: + virtual ~IdleState() { } + virtual StateName name() override { return StateName::Idle; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnRecord() override; + virtual State * handle_OnReplay() override; +}; + +class RecordState : public State +{ +public: + virtual ~RecordState() { } + virtual StateName name() override { return StateName::Record; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnRecord() override; + virtual State * handle_OnTimerTick() override; +}; + +class ReplayState : public State +{ +public: + ReplayState() : _replay_cnt{0} { } + virtual ~ReplayState() { } + virtual StateName name() override { return StateName::Replay; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnReplay() override; + virtual State * handle_OnTimerTick() override; + +private: + int _replay_cnt; +}; + +class ZeroState : public State +{ +public: + virtual ~ZeroState() { } + virtual StateName name() override { return StateName::Zero; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnTimerTick() override; +}; + +class LearnAndRepeatApp +{ +public: + LearnAndRepeatApp() + : _state{nullptr} + , _mtx{} + { } + + void enableButtons(); + + void update(EventSource const evt_src) + { + mbed::ScopedLock lock(_mtx); + + if (!_state) + { + _state = new ZeroState(); + _state->onEnter(); + return; + } + + State * next_state = _state->update(evt_src); + + if (next_state->name() != _state->name()) + { + _state->onExit(); + delete _state; + _state = next_state; + _state->onEnter(); + } + } + +private: + State * _state; + rtos::Mutex _mtx; +}; + +#endif /* LEARN_AND_REPEAT_APP_STATE_H_ */ diff --git a/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_learning_mode/01_Braccio_learning_mode.ino b/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_learning_mode/01_Braccio_learning_mode.ino deleted file mode 100644 index f9b5564..0000000 --- a/examples/Platform_Tutorials/projects/p03-learning-mode/01_Braccio_learning_mode/01_Braccio_learning_mode.ino +++ /dev/null @@ -1,165 +0,0 @@ -#include - -// Colors -#define COLOR_TEAL 0x00878F -#define COLOR_LIGHT_TEAL 0x62AEB2 -#define COLOR_ORANGE 0xE47128 - -enum states { - RECORD, - REPLAY, - ZERO_POSITION -}; - -int state = ZERO_POSITION; - -float values[10000]; -float* idx = values; -float* final_idx = 0; -float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; - -static lv_obj_t * counter; -static lv_obj_t * btnm; - -static const char * btnm_map[] = { "RECORD", "\n", "REPLAY", "\n", "ZERO_POSITION", "\n", "\0" }; - - -static void eventHandlerMenu(lv_event_t * e) { - lv_event_code_t code = lv_event_get_code(e); - lv_obj_t * obj = lv_event_get_target(e); - - if (code == LV_EVENT_KEY && lv_indev_get_key(lv_indev_get_act()) == LV_KEY_HOME) { - state = ZERO_POSITION; - return; - } - - if (code == LV_EVENT_CLICKED) { - uint32_t id = lv_btnmatrix_get_selected_btn(obj); - const char * txt = lv_btnmatrix_get_btn_text(obj, id); - - if (state == RECORD) { - final_idx = idx; - } - - idx = values; - - switch (id) { - case 0: // if the button pressed is the first one - if (txt == "RECORD") { - state = RECORD; - Braccio.disengage(); // allow the user to freely move the braccio - lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_CHECKED); - Serial.println("RECORD"); - lv_btnmatrix_clear_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); // remove disabled state of the replay button - btnm_map[0] = "STOP"; // change the label of the first button to "STOP" - } - else if (txt == "STOP") { - state = ZERO_POSITION; - Braccio.engage(); // enable the steppers so that the braccio stands still - lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); - btnm_map[0] = "RECORD"; // reset the label of the first button back to "RECORD" - } - break; - case 1: - btnm_map[0] = "RECORD"; // reset the label of the first button back to "RECORD" - if (txt == "REPLAY"){ - state = REPLAY; - btnm_map[2] = "STOP"; // change the label of the second button to "STOP" - Braccio.engage(); - lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_CHECKED); - Serial.println("REPLAY"); - } - else if (txt=="STOP"){ - state = ZERO_POSITION; - Braccio.engage(); // enable the steppers so that the braccio stands still - lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); - btnm_map[2] = "REPLAY"; // reset the label of the first button back to "REPLAY" - } - - break; - - default: - state = ZERO_POSITION; - btnm_map[0] = "RECORD"; // reset the label of the first button back to "RECORD" - btnm_map[2] = "REPLAY"; // reset the label of the first button back to "REPLAY" - Braccio.engage(); - delay(500); - Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); - lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); - Serial.println("ZERO_POSITION"); - break; - } - } -} - -void mainMenu() { - static lv_style_t style_focus; - lv_style_init(&style_focus); - lv_style_set_outline_color(&style_focus, lv_color_hex(COLOR_ORANGE)); - lv_style_set_outline_width(&style_focus, 4); - - static lv_style_t style_btn; - lv_style_init(&style_btn); - lv_style_set_bg_color(&style_btn, lv_color_hex(COLOR_LIGHT_TEAL)); - lv_style_set_text_color(&style_btn, lv_color_white()); - - btnm = lv_btnmatrix_create(lv_scr_act()); - lv_obj_set_size(btnm, 240, 240); - lv_btnmatrix_set_map(btnm, btnm_map); - lv_obj_align(btnm, LV_ALIGN_CENTER, 0, 0); - - lv_obj_add_style(btnm, &style_btn, LV_PART_ITEMS); - lv_obj_add_style(btnm, &style_focus, LV_PART_ITEMS | LV_STATE_FOCUS_KEY); - - lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); - lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKABLE); - - lv_btnmatrix_set_one_checked(btnm, true); - lv_btnmatrix_set_selected_btn(btnm, 0); - lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); - - counter = lv_label_create(btnm); - lv_label_set_text_fmt(counter, "Counter: %d" , 0); - lv_obj_align(counter, LV_ALIGN_CENTER, 0, 80); - - lv_obj_add_event_cb(btnm, eventHandlerMenu, LV_EVENT_ALL, NULL); - - Braccio.connectJoystickTo(btnm); -} - -void setup() { - Braccio.begin(mainMenu); - delay(500); - - Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); - delay(500); - - Serial.begin(115200); - Serial.println("Replicate a movement"); -} - -void loop() { - if (state == RECORD) { - Braccio.positions(idx); - idx += 6; - } - if (state == REPLAY) { - Braccio.moveTo(idx[0], idx[1], idx[2], idx[3], idx[4], idx[5]); - idx += 6; - if (idx >= final_idx) { - Serial.println("REPLAY done"); - state = ZERO_POSITION; - btnm_map[2] = "REPLAY"; // reset the label of the first button back to "REPLAY" - lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED); - } - } - if (idx - values >= sizeof(values)) { - Serial.println("ZERO_POSITION"); - state = ZERO_POSITION; - } - delay(100); - if (state != ZERO_POSITION) { - lv_label_set_text_fmt(counter, "Counter: %d" , idx - values); - } -}