Rewrite examples/Braccio_Learn_and_Repeat using State design pattern

examples/Braccio_Learn_and_Repeat/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);
+}

examples/Braccio_Learn_and_Repeat/AppState.h

@@ -0,0 +1,153 @@
+#ifndef LEARN_AND_REPEAT_APP_STATE_H_
+#define LEARN_AND_REPEAT_APP_STATE_H_
+
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
+#include <Braccio++.h>
+
+/**************************************************************************************
+ * 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<rtos::Mutex> 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_ */

examples/Braccio_Learn_and_Repeat/Braccio_Learn_and_Repeat.ino

@@ -1,164 +1,37 @@
-#include <Braccio++.h>
-
-// Colors
-#define COLOR_TEAL       0x00878F
-#define COLOR_LIGHT_TEAL 0x62AEB2
-#define COLOR_ORANGE     0xE47128
-
-// ENTER button
-#define BUTTON_ENTER     6
-
-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;
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
 
-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);
+#include <Braccio++.h>
 
+#include "AppState.h"
 
-  if (code == LV_EVENT_CLICKED || (code == LV_EVENT_KEY && Braccio.getKey() == BUTTON_ENTER)) {
-    uint32_t id = lv_btnmatrix_get_selected_btn(obj);
-    const char * txt = lv_btnmatrix_get_btn_text(obj, id);
+/**************************************************************************************
+ * GLOBAL VARIABLES
+ **************************************************************************************/
 
-    if (state == RECORD) {
-      final_idx = idx;
-    }
+LearnAndRepeatApp app;
 
-    idx = values;
+/**************************************************************************************
+ * SETUP/LOOP
+ **************************************************************************************/
 
-    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 setup()
+{
+  if (Braccio.begin(custom_main_menu)) {
+    app.enableButtons();
   }
 }
 
-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);
+void loop()
+{
+  /* Only execute every 50 ms. */
+  static auto prev = millis();
+  auto const now = millis();
 
-  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);
+  if ((now - prev) > 50)
+  {
+    prev = now;
+    app.update(EventSource::TimerTick);
   }
 }