diff --git a/examples/Controlling_Manually_Braccio/AppState.cpp b/examples/Controlling_Manually_Braccio/AppState.cpp new file mode 100644 index 0000000..13cd03b --- /dev/null +++ b/examples/Controlling_Manually_Braccio/AppState.cpp @@ -0,0 +1,192 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include "AppState.h" + +#include + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +enum BUTTONS { + BTN_UP = 1, + BTN_DOWN = 7, + BTN_LEFT = 3, + BTN_RIGHT = 5, +}; + +/************************************************************************************** + * EXTERN + **************************************************************************************/ + +extern lv_obj_t * label; +extern lv_obj_t * direction_btnm; + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +static auto gripper = Braccio.get(1); +static auto wristRoll = Braccio.get(2); +static auto wristPitch = Braccio.get(3); +static auto elbow = Braccio.get(4); +static auto shoulder = Braccio.get(5); +static auto base = Braccio.get(6); + +/************************************************************************************** + * ShoulderState + **************************************************************************************/ + +ShoulderState::ShoulderState() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Shoulder"); + Braccio.lvgl_unlock(); +} + +State * ShoulderState::handle_OnEnter() +{ + return new ElbowState(); +} + +State * ShoulderState::handle_OnUp() +{ + shoulder.move().to(shoulder.position() + 5.0f); + return this; +} + +State * ShoulderState::handle_OnDown() +{ + shoulder.move().to(shoulder.position() - 5.0f); + return this; +} + +State * ShoulderState::handle_OnLeft() +{ + base.move().to(base.position() + 5.0f); + return this; +} + +State * ShoulderState::handle_OnRight() +{ + base.move().to(base.position() - 5.0f); + return this; +} + +/************************************************************************************** + * ElbowState + **************************************************************************************/ + +ElbowState::ElbowState() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Elbow"); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN); + Braccio.lvgl_unlock(); +} + +ElbowState::~ElbowState() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN); + Braccio.lvgl_unlock(); +} + +State * ElbowState::handle_OnEnter() +{ + return new WristState(); +} + +State * ElbowState::handle_OnUp() +{ + elbow.move().to(elbow.position() + 5.0f); + return this; +} + +State * ElbowState::handle_OnDown() +{ + elbow.move().to(elbow.position() - 5.0f); + return this; +} + +/************************************************************************************** + * WristState + **************************************************************************************/ + +WristState::WristState() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Wrist"); + Braccio.lvgl_unlock(); +} + +State * WristState::handle_OnEnter() +{ + return new PinchState(); +} + +State * WristState::handle_OnUp() +{ + wristPitch.move().to(wristPitch.position() + 5.0f); + return this; +} + +State * WristState::handle_OnDown() +{ + wristPitch.move().to(wristPitch.position() - 5.0f); + return this; +} + +State * WristState::handle_OnLeft() +{ + wristRoll.move().to(wristRoll.position() + 10.0f); + return this; +} + +State * WristState::handle_OnRight() +{ + wristRoll.move().to(wristRoll.position() - 10.0f); + return this; +} + +/************************************************************************************** + * PinchState + **************************************************************************************/ + +PinchState::PinchState() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Pinch"); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_HIDDEN); + Braccio.lvgl_unlock(); +} + +PinchState::~PinchState() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_HIDDEN); + Braccio.lvgl_unlock(); +} + +State * PinchState::handle_OnEnter() +{ + return new ShoulderState(); +} + +State * PinchState::handle_OnLeft() +{ + gripper.move().to(gripper.position() + 5.0f); + return this; +} + +State * PinchState::handle_OnRight() +{ + gripper.move().to(gripper.position() - 5.0f); + return this; +} diff --git a/examples/Controlling_Manually_Braccio/AppState.h b/examples/Controlling_Manually_Braccio/AppState.h new file mode 100644 index 0000000..11dba57 --- /dev/null +++ b/examples/Controlling_Manually_Braccio/AppState.h @@ -0,0 +1,132 @@ +#ifndef APP_STATE_H_ +#define APP_STATE_H_ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +enum class Button +{ + None, Enter, Home, Up, Down, Left, Right +}; + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class State +{ +public: + virtual ~State() { } + State * update(Button const button) + { + State * next_state = this; + switch (button) + { + case Button::Home: + case Button::Enter: next_state = handle_OnEnter(); break; + case Button::Up: next_state = handle_OnUp(); break; + case Button::Down: next_state = handle_OnDown(); break; + case Button::Left: next_state = handle_OnLeft(); break; + case Button::Right: next_state = handle_OnRight(); break; + default: break; + } + return next_state; + } + +protected: + virtual State * handle_OnEnter() = 0; + virtual State * handle_OnUp () { return this; } + virtual State * handle_OnDown () { return this; } + virtual State * handle_OnLeft () { return this; } + virtual State * handle_OnRight() { return this; } +}; + +class ShoulderState : public State +{ +public: + ShoulderState(); + virtual ~ShoulderState() { } +protected: + virtual State * handle_OnEnter() override; + virtual State * handle_OnUp () override; + virtual State * handle_OnDown () override; + virtual State * handle_OnLeft () override; + virtual State * handle_OnRight() override; +}; + + +class ElbowState : public State +{ +public: + ElbowState(); + virtual ~ElbowState(); +protected: + virtual State * handle_OnEnter() override; + virtual State * handle_OnUp () override; + virtual State * handle_OnDown () override; +}; + +class WristState : public State +{ +public: + WristState(); + virtual ~WristState() { } +protected: + virtual State * handle_OnEnter() override; + virtual State * handle_OnUp () override; + virtual State * handle_OnDown () override; + virtual State * handle_OnLeft () override; + virtual State * handle_OnRight() override; +}; + + +class PinchState : public State +{ +public: + PinchState(); + virtual ~PinchState(); +protected: + virtual State * handle_OnEnter() override; + virtual State * handle_OnLeft () override; + virtual State * handle_OnRight() override; +}; + +class ManualControlApp +{ +public: + ManualControlApp() + : _state{nullptr} + , _mtx{} + { } + + void update(Button const button) + { + mbed::ScopedLock lock(_mtx); + + if (!_state) + { + _state = new ShoulderState(); + return; + } + + State * next_state = _state->update(button); + + if (next_state != _state) { + delete _state; + _state = next_state; + } + } + +private: + State * _state; + rtos::Mutex _mtx; +}; + +#endif /* APP_STATE_H_ */ diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino new file mode 100644 index 0000000..4d2f5ea --- /dev/null +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -0,0 +1,277 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +#include "AppState.h" + +/************************************************************************************** + * DEFINES + **************************************************************************************/ + +#define COLOR_TEAL 0x00878F +#define COLOR_LIGHT_TEAL 0x62AEB2 +#define COLOR_YELLOW 0xE5AD24 + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +// IDs of the displayed directional UI buttons +enum BUTTONS { + BTN_UP = 1, + BTN_DOWN = 7, + BTN_LEFT = 3, + BTN_RIGHT = 5, +}; + +/************************************************************************************** + * CONSTANTS + **************************************************************************************/ + +static float const HOME_POS[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; +static const char *DIRECTION_BTNM_MAP[] = {" ", LV_SYMBOL_UP, " ", "\n", + LV_SYMBOL_LEFT, " ", LV_SYMBOL_RIGHT, "\n", + " ", LV_SYMBOL_DOWN, " ", "\0"}; + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +lv_obj_t * direction_btnm; +lv_obj_t * label; + +ManualControlApp app; + +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ + +void setup() +{ + Serial.begin(115200); + + if (Braccio.begin(directionScreen)) + { + /* Configure Braccio. */ + Braccio.speed(speed_grade_t(120)/*MEDIUM*/); + /* Move to home position. */ + Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); + delay(500); + /* Init state. */ + app.update(Button::None); + /* Enable buttons. */ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, 3, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, 5, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, 7, LV_BTNMATRIX_CTRL_DISABLED); + Braccio.lvgl_unlock(); + } +} + +void loop() +{ + /* Execute every 10 ms. */ + { + static auto prev = millis(); + auto const now = millis(); + if ((now - prev) > 10) + { + prev = now; + handle_ButtonPressedReleased(); + } + } + + /* Execute every 50 ms. */ + { + static auto prev = millis(); + auto const now = millis(); + if ((now - prev) > 50) + { + prev = now; + + if (Braccio.isJoystickPressed_UP()) + app.update(Button::Up); + if (Braccio.isJoystickPressed_DOWN()) + app.update(Button::Down); + if (Braccio.isJoystickPressed_LEFT()) + app.update(Button::Left); + if (Braccio.isJoystickPressed_RIGHT()) + app.update(Button::Right); + } + } +} + +/************************************************************************************** + * FUNCTIONS + **************************************************************************************/ + +void directionScreen(void) +{ + Braccio.lvgl_lock(); + + static lv_style_t style_bg; + lv_style_init(&style_bg); + lv_style_set_bg_color(&style_bg, lv_color_white()); + + 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()); + + direction_btnm = lv_btnmatrix_create(lv_scr_act()); + lv_obj_set_size(direction_btnm, 240, 240); + lv_btnmatrix_set_map(direction_btnm, DIRECTION_BTNM_MAP); + lv_obj_align(direction_btnm, LV_ALIGN_CENTER, 0, 0); + + lv_obj_add_style(direction_btnm, &style_bg, 0); + lv_obj_add_style(direction_btnm, &style_btn, LV_PART_ITEMS); + + lv_btnmatrix_set_btn_ctrl(direction_btnm, 0, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 1, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 2, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 3, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 4, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 5, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 6, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 7, LV_BTNMATRIX_CTRL_DISABLED); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 8, LV_BTNMATRIX_CTRL_HIDDEN); + + lv_btnmatrix_set_one_checked(direction_btnm, true); + lv_btnmatrix_set_selected_btn(direction_btnm, 1); + + label = lv_label_create(lv_scr_act()); + lv_obj_set_width(label, 240); + lv_obj_set_style_text_align(label, LV_TEXT_ALIGN_CENTER, 0); + lv_obj_align(label, LV_ALIGN_CENTER, 0, 0); + lv_label_set_text(label, ""); + + Braccio.lvgl_unlock(); +} + +void handle_ButtonPressedReleased() +{ + /* ENTER */ + + static bool prev_joystick_pressed_enter = false; + bool const curr_joystick_pressed_enter = Braccio.isButtonPressed_ENTER(); + if (!prev_joystick_pressed_enter && curr_joystick_pressed_enter) { + app.update(Button::Enter); + } + prev_joystick_pressed_enter = curr_joystick_pressed_enter; + + /* SELECT */ + + static bool prev_joystick_pressed_select = false; + bool const curr_joystick_pressed_select = Braccio.isJoystickPressed_SELECT(); + if (!prev_joystick_pressed_select && curr_joystick_pressed_select) { + app.update(Button::Enter); + } + prev_joystick_pressed_select = curr_joystick_pressed_select; + + /* DOWN */ + + static bool prev_joystick_pressed_down = false; + bool const curr_joystick_pressed_down = Braccio.isJoystickPressed_DOWN(); + if (!prev_joystick_pressed_down && curr_joystick_pressed_down) { + handle_OnButtonDownPressed(); + } + if (prev_joystick_pressed_down && !curr_joystick_pressed_down) { + handle_OnButtonDownReleased(); + } + prev_joystick_pressed_down = curr_joystick_pressed_down; + + /* UP */ + + static bool prev_joystick_pressed_up = false; + bool const curr_joystick_pressed_up = Braccio.isJoystickPressed_UP(); + if (!prev_joystick_pressed_up && curr_joystick_pressed_up) { + handle_OnButtonUpPressed(); + } + if (prev_joystick_pressed_up && !curr_joystick_pressed_up) { + handle_OnButtonUpReleased(); + } + prev_joystick_pressed_up = curr_joystick_pressed_up; + + /* LEFT */ + + static bool prev_joystick_pressed_left = false; + bool const curr_joystick_pressed_left = Braccio.isJoystickPressed_LEFT(); + if (!prev_joystick_pressed_left && curr_joystick_pressed_left) { + handle_OnButtonLeftPressed(); + } + if (prev_joystick_pressed_left && !curr_joystick_pressed_left) { + handle_OnButtonLeftReleased(); + } + prev_joystick_pressed_left = curr_joystick_pressed_left; + + /* RIGHT */ + + static bool prev_joystick_pressed_right = false; + bool const curr_joystick_pressed_right = Braccio.isJoystickPressed_RIGHT(); + if (!prev_joystick_pressed_right && curr_joystick_pressed_right) { + handle_OnButtonRightPressed(); + } + if (prev_joystick_pressed_right && !curr_joystick_pressed_right) { + handle_OnButtonRightReleased(); + } + prev_joystick_pressed_right = curr_joystick_pressed_right; +} + +void handle_OnButtonDownPressed() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +} + +void handle_OnButtonDownReleased() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +} + +void handle_OnButtonUpPressed() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +} + +void handle_OnButtonUpReleased() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +} + +void handle_OnButtonLeftPressed() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +} + +void handle_OnButtonLeftReleased() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +} + +void handle_OnButtonRightPressed() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +} + +void handle_OnButtonRightReleased() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); +}