From 50056247e60d216264d25cbab7e373e0b4213e24 Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 12:12:08 +0200 Subject: [PATCH 01/23] copy sketch to the root `example/` folder --- .../Controlling_Manually_Braccio.ino | 191 ++++++++++++++++++ 1 file changed, 191 insertions(+) create mode 100644 examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino 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..8fc3fc0 --- /dev/null +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -0,0 +1,191 @@ +#include + +#define BUTTON_ENTER 6 + +// Colors +#define COLOR_TEAL 0x00878F +#define COLOR_LIGHT_TEAL 0x62AEB2 +#define COLOR_YELLOW 0xE5AD24 + +// Variables +String selectedJoints; +float homePos[6] = {160.0, 150.0, 220.0, 220.0, 100.0, 180.0}; +float angles[6]; // All motors current angles + +// Braccio ++ joints +auto gripper = Braccio.get(1); +auto wristRoll = Braccio.get(2); +auto wristPitch = Braccio.get(3); +auto elbow = Braccio.get(4); +auto shoulder = Braccio.get(5); +auto base = Braccio.get(6); + +String jointsPair[] = {"Shoulder", "Elbow", "Wrist"}; + +static const char * btnm_map[] = {"Shoulder", "\n", + "Elbow", "\n", + "Wrist", "\0" + }; + +static const char * directional_map[] = {" ", LV_SYMBOL_UP, " ", "\n", + LV_SYMBOL_LEFT, " ", LV_SYMBOL_RIGHT, "\n", + " ", LV_SYMBOL_DOWN, " ", "\0" + }; + +lv_obj_t * btnm; // Joints button matrix +lv_obj_t * directional; // Direction button matrix + + +// Function +void moveJoints(uint32_t btnID) { + if (selectedJoints == "Shoulder") { + switch (btnID) { + case 1: shoulder.move().to(angles[4] - 10.0); break; + case 3: base.move().to(angles[5] - 10.0); break; + case 5: base.move().to(angles[5] + 10.0); break; + case 7: shoulder.move().to(angles[4] + 10.0); break; + default: break; + } + } + + if (selectedJoints == "Elbow") { + switch (btnID) { + case 1: elbow.move().to(angles[3] - 10.0); break; + case 7: elbow.move().to(angles[3] + 10.0); break; + default: break; + } + } + + if (selectedJoints == "Wrist") { + switch (btnID) { + case 1: wristPitch.move().to(angles[2] - 10.0); break; + case 3: wristRoll.move().to(angles[1] - 10.0); break; + case 5: wristRoll.move().to(angles[1] + 10.0); break; + case 7: wristPitch.move().to(angles[2] + 10.0); break; + default: break; + } + } +} + +// Event Handlers +static void eventHandlerMenu(lv_event_t * e) { + lv_obj_t * obj = lv_event_get_target(e); + uint32_t id = lv_btnmatrix_get_selected_btn(obj); + + selectedJoints = jointsPair[id]; + + directionScreen(); + lv_obj_del(btnm); +} + +static void eventHandlerDirectional(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) { + uint32_t pressed_key = Braccio.getKey(); + + if (pressed_key == BUTTON_ENTER) { + mainMenu(); // Load motor menu screen + lv_obj_del(directional); // Delete the object + } + } + if (code == LV_EVENT_PRESSING) { + uint32_t id = lv_btnmatrix_get_selected_btn(obj); + Braccio.positions(angles); + delay(5); + moveJoints(id); + } +} + +// Screens functions +void mainMenu() { + 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_YELLOW)); + lv_style_set_border_color(&style_btn, lv_color_hex(COLOR_LIGHT_TEAL)); + lv_style_set_border_width(&style_btn, 2); + lv_style_set_text_color(&style_btn, lv_color_hex(COLOR_TEAL)); + lv_style_set_text_letter_space(&style_btn, 8); + + 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_bg, 0); + lv_obj_add_style(btnm, &style_btn, LV_PART_ITEMS); + + lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(btnm, 3, LV_BTNMATRIX_CTRL_CHECKABLE); + + lv_btnmatrix_set_one_checked(btnm, true); + lv_btnmatrix_set_selected_btn(btnm, 0); + + lv_obj_add_event_cb(btnm, eventHandlerMenu, LV_EVENT_PRESSED, NULL); + + Braccio.connectJoystickTo(btnm); +} + +void directionScreen(void) +{ + 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()); + + directional = lv_btnmatrix_create(lv_scr_act()); + lv_obj_set_size(directional, 240, 240); + lv_btnmatrix_set_map(directional, directional_map); + lv_obj_align(directional, LV_ALIGN_CENTER, 0, 0); + + lv_obj_add_style(directional, &style_bg, 0); + lv_obj_add_style(directional, &style_btn, LV_PART_ITEMS); + + lv_btnmatrix_set_btn_ctrl(directional, 0, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(directional, 1, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(directional, 2, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(directional, 4, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(directional, 6, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(directional, 7, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(directional, 8, LV_BTNMATRIX_CTRL_HIDDEN); + + if (selectedJoints == "Elbow") { + lv_btnmatrix_set_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_HIDDEN); + } + + lv_btnmatrix_set_one_checked(directional, true); + lv_btnmatrix_set_selected_btn(directional, 1); + + lv_obj_add_event_cb(directional, eventHandlerDirectional, LV_EVENT_ALL, NULL); + + delay(50); + Braccio.connectJoystickTo(btnm); +} + +void setup() { + Braccio.begin(mainMenu); + delay(500); // Waits for the Braccio initialization + + Braccio.speed(SLOW); + + Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); + delay(500); +} + +void loop() { + +} From 155e6f920ef21ac8ad08d4335fe32f5e8d5af83f Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 12:15:57 +0200 Subject: [PATCH 02/23] change home position --- .../Controlling_Manually_Braccio.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 8fc3fc0..e793c98 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -9,7 +9,7 @@ // Variables String selectedJoints; -float homePos[6] = {160.0, 150.0, 220.0, 220.0, 100.0, 180.0}; +float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; float angles[6]; // All motors current angles // Braccio ++ joints From eef357bff15ca563053dc2a881551a09a37a9c61 Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 12:45:22 +0200 Subject: [PATCH 03/23] use joystick directional keys to directly move the braccio --- .../Controlling_Manually_Braccio.ino | 31 +++++++++---------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index e793c98..33193c2 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -40,28 +40,28 @@ lv_obj_t * directional; // Direction button matrix void moveJoints(uint32_t btnID) { if (selectedJoints == "Shoulder") { switch (btnID) { - case 1: shoulder.move().to(angles[4] - 10.0); break; - case 3: base.move().to(angles[5] - 10.0); break; - case 5: base.move().to(angles[5] + 10.0); break; - case 7: shoulder.move().to(angles[4] + 10.0); break; + case 4: shoulder.move().to(angles[4] - 10.0); break; + case 1: base.move().to(angles[5] - 10.0); break; + case 2: base.move().to(angles[5] + 10.0); break; + case 5: shoulder.move().to(angles[4] + 10.0); break; default: break; } } if (selectedJoints == "Elbow") { switch (btnID) { - case 1: elbow.move().to(angles[3] - 10.0); break; - case 7: elbow.move().to(angles[3] + 10.0); break; + case 4: elbow.move().to(angles[3] - 10.0); break; + case 5: elbow.move().to(angles[3] + 10.0); break; default: break; } } if (selectedJoints == "Wrist") { switch (btnID) { - case 1: wristPitch.move().to(angles[2] - 10.0); break; - case 3: wristRoll.move().to(angles[1] - 10.0); break; - case 5: wristRoll.move().to(angles[1] + 10.0); break; - case 7: wristPitch.move().to(angles[2] + 10.0); break; + case 4: wristPitch.move().to(angles[2] - 10.0); break; + case 1: wristRoll.move().to(angles[1] - 10.0); break; + case 2: wristRoll.move().to(angles[1] + 10.0); break; + case 5: wristPitch.move().to(angles[2] + 10.0); break; default: break; } } @@ -83,19 +83,16 @@ static void eventHandlerDirectional(lv_event_t * e) { lv_obj_t * obj = lv_event_get_target(e); if (code == LV_EVENT_KEY) { - uint32_t pressed_key = Braccio.getKey(); + uint32_t pressed_key = Braccio.getKey(); + Braccio.positions(angles); + delay(5); + moveJoints(pressed_key); if (pressed_key == BUTTON_ENTER) { mainMenu(); // Load motor menu screen lv_obj_del(directional); // Delete the object } } - if (code == LV_EVENT_PRESSING) { - uint32_t id = lv_btnmatrix_get_selected_btn(obj); - Braccio.positions(angles); - delay(5); - moveJoints(id); - } } // Screens functions From 0a24af5a2a84e155931c76c6ced8c9acf72da646 Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 15:39:32 +0200 Subject: [PATCH 04/23] add switch between states (SHOULDER, ELBOW, WRIST) using enter button --- .../Controlling_Manually_Braccio.ino | 73 +++++++++++-------- 1 file changed, 44 insertions(+), 29 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 33193c2..9067071 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -8,7 +8,6 @@ #define COLOR_YELLOW 0xE5AD24 // Variables -String selectedJoints; float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; float angles[6]; // All motors current angles @@ -21,6 +20,16 @@ auto shoulder = Braccio.get(5); auto base = Braccio.get(6); String jointsPair[] = {"Shoulder", "Elbow", "Wrist"}; +String selectedJoints = "Shoulder"; + +enum states { + SHOULDER, + ELBOW, + WRIST +}; + +int state = SHOULDER; + static const char * btnm_map[] = {"Shoulder", "\n", "Elbow", "\n", @@ -38,32 +47,35 @@ lv_obj_t * directional; // Direction button matrix // Function void moveJoints(uint32_t btnID) { - if (selectedJoints == "Shoulder") { - switch (btnID) { - case 4: shoulder.move().to(angles[4] - 10.0); break; - case 1: base.move().to(angles[5] - 10.0); break; - case 2: base.move().to(angles[5] + 10.0); break; - case 5: shoulder.move().to(angles[4] + 10.0); break; - default: break; - } - } + switch (state) { + case SHOULDER: + switch (btnID) { + case 4: shoulder.move().to(angles[4] - 10.0); break; + case 1: base.move().to(angles[5] - 10.0); break; + case 2: base.move().to(angles[5] + 10.0); break; + case 5: shoulder.move().to(angles[4] + 10.0); break; + default: break; + } + break; + case ELBOW: + switch (btnID) { + case 4: elbow.move().to(angles[3] - 10.0); break; + case 5: elbow.move().to(angles[3] + 10.0); break; + default: break; + } + break; + case WRIST: + switch (btnID) { + case 4: wristPitch.move().to(angles[2] - 10.0); break; + case 1: wristRoll.move().to(angles[1] - 10.0); break; + case 2: wristRoll.move().to(angles[1] + 10.0); break; + case 5: wristPitch.move().to(angles[2] + 10.0); break; + default: break; + } + break; + default: + break; - if (selectedJoints == "Elbow") { - switch (btnID) { - case 4: elbow.move().to(angles[3] - 10.0); break; - case 5: elbow.move().to(angles[3] + 10.0); break; - default: break; - } - } - - if (selectedJoints == "Wrist") { - switch (btnID) { - case 4: wristPitch.move().to(angles[2] - 10.0); break; - case 1: wristRoll.move().to(angles[1] - 10.0); break; - case 2: wristRoll.move().to(angles[1] + 10.0); break; - case 5: wristPitch.move().to(angles[2] + 10.0); break; - default: break; - } } } @@ -89,8 +101,12 @@ static void eventHandlerDirectional(lv_event_t * e) { moveJoints(pressed_key); if (pressed_key == BUTTON_ENTER) { - mainMenu(); // Load motor menu screen - lv_obj_del(directional); // Delete the object + //mainMenu(); // Load motor menu screen + //lv_obj_del(directional); // Delete the object + state++; + if (state > WRIST) { + state = SHOULDER; // restart from the shoulder + } } } } @@ -184,5 +200,4 @@ void setup() { } void loop() { - } From 0debf06e6df280a9e3bcf3d0dab2723cc8fe9709 Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 16:47:13 +0200 Subject: [PATCH 05/23] remove `eventHandlerMenu` callback & optimizations --- .../Controlling_Manually_Braccio.ino | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 9067071..1f7cb47 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -80,15 +80,6 @@ void moveJoints(uint32_t btnID) { } // Event Handlers -static void eventHandlerMenu(lv_event_t * e) { - lv_obj_t * obj = lv_event_get_target(e); - uint32_t id = lv_btnmatrix_get_selected_btn(obj); - - selectedJoints = jointsPair[id]; - - directionScreen(); - lv_obj_del(btnm); -} static void eventHandlerDirectional(lv_event_t * e) { lv_event_code_t code = lv_event_get_code(e); @@ -141,8 +132,6 @@ void mainMenu() { lv_btnmatrix_set_one_checked(btnm, true); lv_btnmatrix_set_selected_btn(btnm, 0); - lv_obj_add_event_cb(btnm, eventHandlerMenu, LV_EVENT_PRESSED, NULL); - Braccio.connectJoystickTo(btnm); } @@ -175,7 +164,7 @@ void directionScreen(void) lv_btnmatrix_set_btn_ctrl(directional, 7, LV_BTNMATRIX_CTRL_CHECKABLE); lv_btnmatrix_set_btn_ctrl(directional, 8, LV_BTNMATRIX_CTRL_HIDDEN); - if (selectedJoints == "Elbow") { + if (state == ELBOW) { lv_btnmatrix_set_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_HIDDEN); lv_btnmatrix_set_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_HIDDEN); } @@ -185,12 +174,12 @@ void directionScreen(void) lv_obj_add_event_cb(directional, eventHandlerDirectional, LV_EVENT_ALL, NULL); - delay(50); - Braccio.connectJoystickTo(btnm); + // delay(50); + Braccio.connectJoystickTo(directional); } void setup() { - Braccio.begin(mainMenu); + Braccio.begin(directionScreen); delay(500); // Waits for the Braccio initialization Braccio.speed(SLOW); From 2651550702426a1dc2384ad3a227f077efce1d2b Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 17:52:53 +0200 Subject: [PATCH 06/23] fix braccio being slow responding to commands --- .../Controlling_Manually_Braccio.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 1f7cb47..c0cb341 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -174,7 +174,7 @@ void directionScreen(void) lv_obj_add_event_cb(directional, eventHandlerDirectional, LV_EVENT_ALL, NULL); - // delay(50); + delay(50); Braccio.connectJoystickTo(directional); } From cb0fe0b519380f62e94239bdf48c25485fa4ca2a Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 17:53:30 +0200 Subject: [PATCH 07/23] add pinch movement --- .../Controlling_Manually_Braccio.ino | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index c0cb341..805f283 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -8,7 +8,7 @@ #define COLOR_YELLOW 0xE5AD24 // Variables -float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; +float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 190.0}; float angles[6]; // All motors current angles // Braccio ++ joints @@ -25,7 +25,8 @@ String selectedJoints = "Shoulder"; enum states { SHOULDER, ELBOW, - WRIST + WRIST, + PINCH }; int state = SHOULDER; @@ -73,6 +74,12 @@ void moveJoints(uint32_t btnID) { default: break; } break; + case PINCH: + switch (btnID) { + case 1: gripper.move().to(angles[0] + 10.0); break; + case 2: gripper.move().to(angles[0] - 10.0); break; + default: break; + } default: break; @@ -95,7 +102,7 @@ static void eventHandlerDirectional(lv_event_t * e) { //mainMenu(); // Load motor menu screen //lv_obj_del(directional); // Delete the object state++; - if (state > WRIST) { + if (state > PINCH) { state = SHOULDER; // restart from the shoulder } } From 6fcd8c01e4a5e036b78876300c6df193bd62b908 Mon Sep 17 00:00:00 2001 From: Umberto Baldi Date: Fri, 29 Apr 2022 18:39:01 +0200 Subject: [PATCH 08/23] change buttons according to active state --- .../Controlling_Manually_Braccio.ino | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 805f283..e357f4f 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -196,4 +196,18 @@ void setup() { } void loop() { + if (state == ELBOW) { + lv_btnmatrix_set_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_HIDDEN); + } + else if (state == PINCH) { + lv_btnmatrix_set_btn_ctrl(directional, 1, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(directional, 7, LV_BTNMATRIX_CTRL_HIDDEN); + } + else { + lv_btnmatrix_clear_btn_ctrl(directional, 1, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_clear_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_clear_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_clear_btn_ctrl(directional, 7, LV_BTNMATRIX_CTRL_HIDDEN); + } } From 3816c47f34ebb06d4989e0c8690ce79de2711444 Mon Sep 17 00:00:00 2001 From: Ernesto Voltaggio Date: Tue, 3 May 2022 01:18:38 +0200 Subject: [PATCH 09/23] code cleanup --- .../Controlling_Manually_Braccio.ino | 38 ------------------- 1 file changed, 38 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index e357f4f..bbc2fcc 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -32,20 +32,13 @@ enum states { int state = SHOULDER; -static const char * btnm_map[] = {"Shoulder", "\n", - "Elbow", "\n", - "Wrist", "\0" - }; - static const char * directional_map[] = {" ", LV_SYMBOL_UP, " ", "\n", LV_SYMBOL_LEFT, " ", LV_SYMBOL_RIGHT, "\n", " ", LV_SYMBOL_DOWN, " ", "\0" }; -lv_obj_t * btnm; // Joints button matrix lv_obj_t * directional; // Direction button matrix - // Function void moveJoints(uint32_t btnID) { switch (state) { @@ -110,37 +103,6 @@ static void eventHandlerDirectional(lv_event_t * e) { } // Screens functions -void mainMenu() { - 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_YELLOW)); - lv_style_set_border_color(&style_btn, lv_color_hex(COLOR_LIGHT_TEAL)); - lv_style_set_border_width(&style_btn, 2); - lv_style_set_text_color(&style_btn, lv_color_hex(COLOR_TEAL)); - lv_style_set_text_letter_space(&style_btn, 8); - - 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_bg, 0); - lv_obj_add_style(btnm, &style_btn, LV_PART_ITEMS); - - lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm, 1, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm, 3, LV_BTNMATRIX_CTRL_CHECKABLE); - - lv_btnmatrix_set_one_checked(btnm, true); - lv_btnmatrix_set_selected_btn(btnm, 0); - - Braccio.connectJoystickTo(btnm); -} void directionScreen(void) { From 724e1b89b3645b17833d08f53878a1b086f0ab89 Mon Sep 17 00:00:00 2001 From: Ernesto Voltaggio Date: Tue, 3 May 2022 02:26:20 +0200 Subject: [PATCH 10/23] added checked state of buttons when pressed --- .../Controlling_Manually_Braccio.ino | 169 +++++++++++------- 1 file changed, 108 insertions(+), 61 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index bbc2fcc..3ed32a2 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -1,7 +1,5 @@ #include -#define BUTTON_ENTER 6 - // Colors #define COLOR_TEAL 0x00878F #define COLOR_LIGHT_TEAL 0x62AEB2 @@ -19,7 +17,7 @@ auto elbow = Braccio.get(4); auto shoulder = Braccio.get(5); auto base = Braccio.get(6); -String jointsPair[] = {"Shoulder", "Elbow", "Wrist"}; +char * jointsPair[] = {"Shoulder", "Elbow", "Wrist", "Pinch"}; String selectedJoints = "Shoulder"; enum states { @@ -31,13 +29,34 @@ enum states { int state = SHOULDER; +// Values of clicked input keys returned by Braccio.getKey() function (defined inside the library). +enum KEYS +{ + UP = 4, + DOWN = 5, + LEFT = 1, + RIGHT = 2, + CLICK = 3, // click of joystick + ENTER = 6, +}; + +uint32_t pressed_key; -static const char * directional_map[] = {" ", LV_SYMBOL_UP, " ", "\n", - LV_SYMBOL_LEFT, " ", LV_SYMBOL_RIGHT, "\n", - " ", LV_SYMBOL_DOWN, " ", "\0" - }; -lv_obj_t * directional; // Direction button matrix +// IDs of the displayed directional UI buttons +enum BUTTONS { + BTN_UP = 1, + BTN_DOWN = 7, + BTN_LEFT = 3, + BTN_RIGHT = 5, +}; + +static const char *direction_btnm_map[] = {" ", LV_SYMBOL_UP, " ", "\n", + LV_SYMBOL_LEFT, " ", LV_SYMBOL_RIGHT, "\n", + " ", LV_SYMBOL_DOWN, " ", "\0"}; + +lv_obj_t * direction_btnm; // Direction button matrix +static lv_obj_t * label; // Label // Function void moveJoints(uint32_t btnID) { @@ -75,8 +94,46 @@ void moveJoints(uint32_t btnID) { } default: break; + } +} +void updateButtons(uint32_t key) +{ + if (key == UP){ + lv_btnmatrix_set_selected_btn(direction_btnm, BTN_UP); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); + } + else if (key == DOWN){ + lv_btnmatrix_set_selected_btn(direction_btnm, BTN_DOWN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); + } + else if (key == LEFT) { + lv_btnmatrix_set_selected_btn(direction_btnm, BTN_LEFT); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); } + else if (key == RIGHT){ + lv_btnmatrix_set_selected_btn(direction_btnm, BTN_RIGHT); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); + } + else { + lv_btnmatrix_set_selected_btn(direction_btnm, NULL); + } + + if (state == 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); + } + else if (state == 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); + } + else{ + 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); + 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); + } + lv_label_set_text(label, jointsPair[state]); } // Event Handlers @@ -85,20 +142,20 @@ static void eventHandlerDirectional(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) { - uint32_t pressed_key = Braccio.getKey(); - Braccio.positions(angles); - delay(5); - moveJoints(pressed_key); - - if (pressed_key == BUTTON_ENTER) { - //mainMenu(); // Load motor menu screen - //lv_obj_del(directional); // Delete the object - state++; - if (state > PINCH) { + if (code == LV_EVENT_KEY){ + pressed_key = Braccio.getKey(); + + if (pressed_key == ENTER){ + state++; // Index the next joints in the states enum array + + if (state > PINCH){ state = SHOULDER; // restart from the shoulder } } + updateButtons(pressed_key); + Braccio.positions(angles); + delay(5); + moveJoints(pressed_key); } } @@ -115,39 +172,41 @@ void directionScreen(void) lv_style_set_bg_color(&style_btn, lv_color_hex(COLOR_LIGHT_TEAL)); lv_style_set_text_color(&style_btn, lv_color_white()); - directional = lv_btnmatrix_create(lv_scr_act()); - lv_obj_set_size(directional, 240, 240); - lv_btnmatrix_set_map(directional, directional_map); - lv_obj_align(directional, LV_ALIGN_CENTER, 0, 0); - - lv_obj_add_style(directional, &style_bg, 0); - lv_obj_add_style(directional, &style_btn, LV_PART_ITEMS); - - lv_btnmatrix_set_btn_ctrl(directional, 0, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_set_btn_ctrl(directional, 1, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(directional, 2, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_set_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(directional, 4, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_set_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(directional, 6, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_set_btn_ctrl(directional, 7, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(directional, 8, LV_BTNMATRIX_CTRL_HIDDEN); - - if (state == ELBOW) { - lv_btnmatrix_set_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_set_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_HIDDEN); - } + 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_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 2, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 3, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 4, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 5, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 6, LV_BTNMATRIX_CTRL_HIDDEN); + lv_btnmatrix_set_btn_ctrl(direction_btnm, 7, LV_BTNMATRIX_CTRL_CHECKABLE); + 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); - lv_btnmatrix_set_one_checked(directional, true); - lv_btnmatrix_set_selected_btn(directional, 1); + lv_obj_add_event_cb(direction_btnm, eventHandlerDirectional, LV_EVENT_ALL, NULL); - lv_obj_add_event_cb(directional, eventHandlerDirectional, LV_EVENT_ALL, NULL); + 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, jointsPair[state]); delay(50); - Braccio.connectJoystickTo(directional); + Braccio.connectJoystickTo(direction_btnm); } -void setup() { +void setup() +{ Braccio.begin(directionScreen); delay(500); // Waits for the Braccio initialization @@ -157,19 +216,7 @@ void setup() { delay(500); } -void loop() { - if (state == ELBOW) { - lv_btnmatrix_set_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_set_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_HIDDEN); - } - else if (state == PINCH) { - lv_btnmatrix_set_btn_ctrl(directional, 1, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_set_btn_ctrl(directional, 7, LV_BTNMATRIX_CTRL_HIDDEN); - } - else { - lv_btnmatrix_clear_btn_ctrl(directional, 1, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_clear_btn_ctrl(directional, 3, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_clear_btn_ctrl(directional, 5, LV_BTNMATRIX_CTRL_HIDDEN); - lv_btnmatrix_clear_btn_ctrl(directional, 7, LV_BTNMATRIX_CTRL_HIDDEN); - } +void loop() +{ + } From 83ac180276f71609a533977a6bff0c1b3d6d20ee Mon Sep 17 00:00:00 2001 From: Ernesto Voltaggio Date: Tue, 3 May 2022 02:32:33 +0200 Subject: [PATCH 11/23] added removal of checked state on press released --- .../Controlling_Manually_Braccio.ino | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 3ed32a2..d9479a5 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -41,6 +41,7 @@ enum KEYS }; uint32_t pressed_key; +uint32_t last_pressed_key; // IDs of the displayed directional UI buttons @@ -218,5 +219,16 @@ void setup() void loop() { + pressed_key= Braccio.getKey(); + if (pressed_key == 0) { +if(pressed_key != last_pressed_key){ + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); + delay(50); + } + } + last_pressed_key=pressed_key; } From 60562f2b39e7082c7f78a86dc728b6fb61ad5496 Mon Sep 17 00:00:00 2001 From: Ernesto Voltaggio Date: Tue, 3 May 2022 02:39:14 +0200 Subject: [PATCH 12/23] added joystick click as input to switch joints - same as enter button --- .../Controlling_Manually_Braccio.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index d9479a5..9ce78ef 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -146,7 +146,7 @@ static void eventHandlerDirectional(lv_event_t * e) { if (code == LV_EVENT_KEY){ pressed_key = Braccio.getKey(); - if (pressed_key == ENTER){ + if (pressed_key == ENTER || pressed_key == CLICK){ state++; // Index the next joints in the states enum array if (state > PINCH){ From 5ff8ddeebfa89c584120c2b0bf38457aeb3d8fbc Mon Sep 17 00:00:00 2001 From: Fernando Esquirio Torres <31143274+Esquirio@users.noreply.github.com> Date: Tue, 3 May 2022 15:00:43 +0200 Subject: [PATCH 13/23] Update Controlling_Manually_Braccio.ino In the code, it was missing the Braccio.lvgl_lock/unlock statements to avoid multi-threaded access to LVGL. --- .../Controlling_Manually_Braccio.ino | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 9ce78ef..66e5710 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -101,47 +101,70 @@ void moveJoints(uint32_t btnID) { void updateButtons(uint32_t key) { if (key == UP){ + Braccio.lvgl_lock(); lv_btnmatrix_set_selected_btn(direction_btnm, BTN_UP); lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); } else if (key == DOWN){ + Braccio.lvgl_lock(); lv_btnmatrix_set_selected_btn(direction_btnm, BTN_DOWN); lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); } else if (key == LEFT) { + Braccio.lvgl_lock(); lv_btnmatrix_set_selected_btn(direction_btnm, BTN_LEFT); lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); } else if (key == RIGHT){ + Braccio.lvgl_lock(); lv_btnmatrix_set_selected_btn(direction_btnm, BTN_RIGHT); lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); } else { + Braccio.lvgl_lock(); lv_btnmatrix_set_selected_btn(direction_btnm, NULL); + Braccio.lvgl_unlock(); } if (state == ELBOW){ + Braccio.lvgl_lock(); 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(); } else if (state == PINCH){ + Braccio.lvgl_lock(); 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(); } else{ + 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); 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(); } + + Braccio.lvgl_lock(); lv_label_set_text(label, jointsPair[state]); + Braccio.lvgl_unlock(); } // Event Handlers static void eventHandlerDirectional(lv_event_t * e) { + Braccio.lvgl_lock(); + lv_event_code_t code = lv_event_get_code(e); lv_obj_t * obj = lv_event_get_target(e); + + Braccio.lvgl_unlock(); if (code == LV_EVENT_KEY){ pressed_key = Braccio.getKey(); @@ -164,6 +187,8 @@ static void eventHandlerDirectional(lv_event_t * e) { 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()); @@ -201,6 +226,8 @@ void directionScreen(void) 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, jointsPair[state]); + + Braccio.lvgl_unlock(); delay(50); Braccio.connectJoystickTo(direction_btnm); @@ -221,11 +248,16 @@ void loop() { pressed_key= Braccio.getKey(); if (pressed_key == 0) { -if(pressed_key != last_pressed_key){ - lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); + if(pressed_key != last_pressed_key){ + Braccio.lvgl_lock(); + + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); + + Braccio.lvgl_unlock(); + delay(50); } From 7ceb29c400636b0f1eab3e6a5081e20d8097ea5f Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 9 May 2022 13:57:32 +0200 Subject: [PATCH 14/23] temp save. --- .../Controlling_Manually_Braccio/AppState.cpp | 202 ++++++++++++++++++ .../Controlling_Manually_Braccio/AppState.h | 147 +++++++++++++ .../Controlling_Manually_Braccio.ino | 93 ++++++-- 3 files changed, 419 insertions(+), 23 deletions(-) create mode 100644 examples/Controlling_Manually_Braccio/AppState.cpp create mode 100644 examples/Controlling_Manually_Braccio/AppState.h diff --git a/examples/Controlling_Manually_Braccio/AppState.cpp b/examples/Controlling_Manually_Braccio/AppState.cpp new file mode 100644 index 0000000..3bf10a2 --- /dev/null +++ b/examples/Controlling_Manually_Braccio/AppState.cpp @@ -0,0 +1,202 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include "AppState.h" + +#include + +extern lv_obj_t * label; +extern lv_obj_t * direction_btnm; + +enum BUTTONS { + BTN_UP = 1, + BTN_DOWN = 7, + BTN_LEFT = 3, + BTN_RIGHT = 5, +}; + +/************************************************************************************** + * State + **************************************************************************************/ + +State * State::handle_OnButtonDownPressed() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); + return this; +} + +State * State::handle_OnButtonDownReleased() +{ + Braccio.lvgl_lock(); + lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); + Braccio.lvgl_unlock(); + return this; +} + +/************************************************************************************** + * ShoulderState + **************************************************************************************/ + +void ShoulderState::onEnter() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Shoulder"); + Braccio.lvgl_unlock(); +} + +void ShoulderState::onExit() +{ + +} + +State * ShoulderState::handle_OnButtonDownPressed() +{ + State::handle_OnButtonDownPressed(); + return this; +} + +State * ShoulderState::handle_OnButtonDownReleased() +{ + State::handle_OnButtonDownReleased(); + return this; +} + +State * ShoulderState::handle_OnButtonUp() +{ + return this; +} + +State * ShoulderState::handle_OnButtonLeft() +{ + return this; +} + +State * ShoulderState::handle_OnButtonRight() +{ + return this; +} + +State * ShoulderState::handle_OnButtonEnter() +{ + return new ElbowState(); +} + +/************************************************************************************** + * ElbowState + **************************************************************************************/ + +void ElbowState::onEnter() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Elbow"); + Braccio.lvgl_unlock(); +} + +void ElbowState::onExit() +{ + +} + +State * ElbowState::handle_OnButtonDownPressed() +{ + State::handle_OnButtonDownPressed(); + return this; +} + +State * ElbowState::handle_OnButtonDownReleased() +{ + State::handle_OnButtonDownReleased(); + return this; +} + +State * ElbowState::handle_OnButtonUp() +{ + return new WristState(); +} + +State * ElbowState::handle_OnButtonEnter() +{ + return new WristState(); +} + +/************************************************************************************** + * WristState + **************************************************************************************/ + +void WristState::onEnter() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Wrist"); + Braccio.lvgl_unlock(); +} + +void WristState::onExit() +{ + +} + +State * WristState::handle_OnButtonDownPressed() +{ + State::handle_OnButtonDownPressed(); + return this; +} + +State * WristState::handle_OnButtonDownReleased() +{ + State::handle_OnButtonDownReleased(); + return this; +} + +State * WristState::handle_OnButtonUp() +{ + return this; +} + +State * WristState::handle_OnButtonLeft() +{ + return this; +} + +State * WristState::handle_OnButtonRight() +{ + return this; +} + +State * WristState::handle_OnButtonEnter() +{ + return new PinchState(); +} + +/************************************************************************************** + * PinchState + **************************************************************************************/ + +void PinchState::onEnter() +{ + Braccio.lvgl_lock(); + lv_label_set_text(label, "Pinch"); + Braccio.lvgl_unlock(); +} + +void PinchState::onExit() +{ + +} + +State * PinchState::handle_OnButtonLeft() +{ + return this; +} + +State * PinchState::handle_OnButtonRight() +{ + return this; +} + +State * PinchState::handle_OnButtonEnter() +{ + return new ShoulderState(); +} diff --git a/examples/Controlling_Manually_Braccio/AppState.h b/examples/Controlling_Manually_Braccio/AppState.h new file mode 100644 index 0000000..828cb14 --- /dev/null +++ b/examples/Controlling_Manually_Braccio/AppState.h @@ -0,0 +1,147 @@ +#ifndef APP_STATE_H_ +#define APP_STATE_H_ + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +enum class EventSource +{ + Button_DownPressed, Button_DownReleased, Button_Up, Button_Left, Button_Right, Button_Enter +}; + +enum class StateName +{ + Shoulder, Elbow, Wrist, Pinch +}; + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class State +{ +public: + virtual ~State() { } + virtual void onEnter() { } + virtual void onExit() { } + virtual StateName name() = 0; + State * update(EventSource const evt_src) + { + State * next_state = this; + switch (evt_src) + { + case EventSource::Button_DownPressed: next_state = handle_OnButtonDownPressed(); break; + case EventSource::Button_DownReleased: next_state = handle_OnButtonDownReleased(); break; + + case EventSource::Button_Up: next_state = handle_OnButtonUp(); break; + case EventSource::Button_Left: next_state = handle_OnButtonLeft(); break; + case EventSource::Button_Right: next_state = handle_OnButtonRight(); break; + case EventSource::Button_Enter: next_state = handle_OnButtonEnter(); break; + } + return next_state; + } + +protected: + virtual State * handle_OnButtonDownPressed(); + virtual State * handle_OnButtonDownReleased(); + virtual State * handle_OnButtonUp() { return this; } + virtual State * handle_OnButtonLeft() { return this; } + virtual State * handle_OnButtonRight() { return this; } + virtual State * handle_OnButtonEnter() { return this; } +}; + +class ShoulderState : public State +{ +public: + virtual ~ShoulderState() { } + virtual StateName name() override { return StateName::Shoulder; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnButtonDownPressed() override; + virtual State * handle_OnButtonDownReleased() override; + virtual State * handle_OnButtonUp() override; + virtual State * handle_OnButtonLeft() override; + virtual State * handle_OnButtonRight() override; + virtual State * handle_OnButtonEnter() override; +}; + +class ElbowState : public State +{ +public: + virtual ~ElbowState() { } + virtual StateName name() override { return StateName::Elbow; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnButtonDownPressed() override; + virtual State * handle_OnButtonDownReleased() override; + virtual State * handle_OnButtonUp() override; + virtual State * handle_OnButtonEnter() override; +}; + +class WristState : public State +{ +public: + virtual ~WristState() { } + virtual StateName name() override { return StateName::Wrist; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnButtonDownPressed() override; + virtual State * handle_OnButtonDownReleased() override; + virtual State * handle_OnButtonUp() override; + virtual State * handle_OnButtonLeft() override; + virtual State * handle_OnButtonRight() override; + virtual State * handle_OnButtonEnter() override; +}; + +class PinchState : public State +{ +public: + virtual ~PinchState() { } + virtual StateName name() override { return StateName::Pinch; } + virtual void onEnter() override; + virtual void onExit() override; + +protected: + virtual State * handle_OnButtonLeft() override; + virtual State * handle_OnButtonRight() override; + virtual State * handle_OnButtonEnter() override; +}; + +class ControlApp +{ +public: + ControlApp() + : _state{nullptr} + { } + + void update(EventSource const evt_src) + { + if (!_state) + { + _state = new ShoulderState(); + _state->onEnter(); + } + + 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; +}; + +#endif /* APP_STATE_H_ */ diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 66e5710..b623ecd 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -1,5 +1,7 @@ #include +#include "AppState.h" + // Colors #define COLOR_TEAL 0x00878F #define COLOR_LIGHT_TEAL 0x62AEB2 @@ -57,7 +59,7 @@ static const char *direction_btnm_map[] = {" ", LV_SYMBOL_UP, " ", "\n", " ", LV_SYMBOL_DOWN, " ", "\0"}; lv_obj_t * direction_btnm; // Direction button matrix -static lv_obj_t * label; // Label +lv_obj_t * label; // Label // Function void moveJoints(uint32_t btnID) { @@ -150,38 +152,62 @@ void updateButtons(uint32_t key) lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN); Braccio.lvgl_unlock(); } - + /* Braccio.lvgl_lock(); lv_label_set_text(label, jointsPair[state]); Braccio.lvgl_unlock(); + */ } // Event Handlers -static void eventHandlerDirectional(lv_event_t * e) { - Braccio.lvgl_lock(); - - lv_event_code_t code = lv_event_get_code(e); - lv_obj_t * obj = lv_event_get_target(e); - - Braccio.lvgl_unlock(); +ControlApp app; - if (code == LV_EVENT_KEY){ - pressed_key = Braccio.getKey(); +static void eventHandlerDirectional(lv_event_t * e) +{ + lv_event_code_t code = lv_event_get_code(e); - if (pressed_key == ENTER || pressed_key == CLICK){ - state++; // Index the next joints in the states enum array - - if (state > PINCH){ - state = SHOULDER; // restart from the shoulder - } + if (code == LV_EVENT_KEY) + { + lv_obj_t * obj = lv_event_get_target(e); + uint32_t const id = lv_btnmatrix_get_selected_btn(obj); + + Serial.println(id); + + switch (id) + { + case UP : app.update(EventSource::Button_Up); break; + //case DOWN : app.update(EventSource::Button_Down); break; + case LEFT : app.update(EventSource::Button_Left); break; + case RIGHT: app.update(EventSource::Button_Right); break; + case CLICK: + case ENTER: app.update(EventSource::Button_Enter); break; } - updateButtons(pressed_key); - Braccio.positions(angles); - delay(5); - moveJoints(pressed_key); } } + // Braccio.lvgl_lock(); + + // lv_event_code_t code = lv_event_get_code(e); + // lv_obj_t * obj = lv_event_get_target(e); + + // Braccio.lvgl_unlock(); + + // if (code == LV_EVENT_KEY){ + // pressed_key = Braccio.getKey(); + + // if (pressed_key == ENTER || pressed_key == CLICK){ + // state++; // Index the next joints in the states enum array + + // if (state > PINCH){ + // state = SHOULDER; // restart from the shoulder + // } + // } + // updateButtons(pressed_key); + // Braccio.positions(angles); + // delay(5); + // moveJoints(pressed_key); + // } +// } // Screens functions @@ -229,12 +255,14 @@ void directionScreen(void) Braccio.lvgl_unlock(); - delay(50); - Braccio.connectJoystickTo(direction_btnm); + //delay(50); + //Braccio.connectJoystickTo(direction_btnm); } void setup() { + Serial.begin(115200); + Braccio.begin(directionScreen); delay(500); // Waits for the Braccio initialization @@ -246,6 +274,24 @@ void setup() void loop() { + if (Braccio.isButtonPressed_ENTER()) { + static auto prev_enter_event = millis(); + auto const now = millis(); + if ((now - prev_enter_event) > 200 && Braccio.isButtonPressed_ENTER()) + { + prev_enter_event = now; + app.update(EventSource::Button_Enter); + } + } + + if (Braccio.isJoystickPressed_DOWN()) { + app.update(EventSource::Button_DownPressed); + Serial.println("Braccio.isJoystickPressed_DOWN()"); + } else { + app.update(EventSource::Button_DownReleased); + } + + /* pressed_key= Braccio.getKey(); if (pressed_key == 0) { if(pressed_key != last_pressed_key){ @@ -263,4 +309,5 @@ void loop() } } last_pressed_key=pressed_key; + */ } From 149c0464065eec8959bc1a41682e994faf2f27db Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 9 May 2022 14:04:41 +0200 Subject: [PATCH 15/23] One SW event per button event. --- .../Controlling_Manually_Braccio/AppState.cpp | 126 +++------ .../Controlling_Manually_Braccio/AppState.h | 123 ++++----- .../Controlling_Manually_Braccio.ino | 242 +++++++++++------- 3 files changed, 238 insertions(+), 253 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/AppState.cpp b/examples/Controlling_Manually_Braccio/AppState.cpp index 3bf10a2..c74c797 100644 --- a/examples/Controlling_Manually_Braccio/AppState.cpp +++ b/examples/Controlling_Manually_Braccio/AppState.cpp @@ -6,197 +6,145 @@ #include -extern lv_obj_t * label; -extern lv_obj_t * direction_btnm; +/************************************************************************************** + * EXTERN + **************************************************************************************/ -enum BUTTONS { - BTN_UP = 1, - BTN_DOWN = 7, - BTN_LEFT = 3, - BTN_RIGHT = 5, -}; +extern lv_obj_t * label; /************************************************************************************** - * State + * GLOBAL VARIABLES **************************************************************************************/ -State * State::handle_OnButtonDownPressed() -{ - Braccio.lvgl_lock(); - lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); - Braccio.lvgl_unlock(); - return this; -} - -State * State::handle_OnButtonDownReleased() -{ - Braccio.lvgl_lock(); - lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); - Braccio.lvgl_unlock(); - return this; -} +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 **************************************************************************************/ -void ShoulderState::onEnter() +ShoulderState::ShoulderState() { Braccio.lvgl_lock(); lv_label_set_text(label, "Shoulder"); Braccio.lvgl_unlock(); } -void ShoulderState::onExit() +State * ShoulderState::handle_OnEnter() { - -} - -State * ShoulderState::handle_OnButtonDownPressed() -{ - State::handle_OnButtonDownPressed(); - return this; + return new ElbowState(); } -State * ShoulderState::handle_OnButtonDownReleased() +State * ShoulderState::handle_OnUp() { - State::handle_OnButtonDownReleased(); return this; } -State * ShoulderState::handle_OnButtonUp() +State * ShoulderState::handle_OnDown() { return this; } -State * ShoulderState::handle_OnButtonLeft() +State * ShoulderState::handle_OnLeft() { return this; } -State * ShoulderState::handle_OnButtonRight() +State * ShoulderState::handle_OnRight() { return this; } -State * ShoulderState::handle_OnButtonEnter() -{ - return new ElbowState(); -} - /************************************************************************************** * ElbowState **************************************************************************************/ -void ElbowState::onEnter() +ElbowState::ElbowState() { Braccio.lvgl_lock(); lv_label_set_text(label, "Elbow"); Braccio.lvgl_unlock(); } -void ElbowState::onExit() +State * ElbowState::handle_OnEnter() { - + return new WristState(); } -State * ElbowState::handle_OnButtonDownPressed() +State * ElbowState::handle_OnUp() { - State::handle_OnButtonDownPressed(); return this; } -State * ElbowState::handle_OnButtonDownReleased() +State * ElbowState::handle_OnDown() { - State::handle_OnButtonDownReleased(); return this; } -State * ElbowState::handle_OnButtonUp() -{ - return new WristState(); -} - -State * ElbowState::handle_OnButtonEnter() -{ - return new WristState(); -} - /************************************************************************************** * WristState **************************************************************************************/ -void WristState::onEnter() +WristState::WristState() { Braccio.lvgl_lock(); lv_label_set_text(label, "Wrist"); Braccio.lvgl_unlock(); } -void WristState::onExit() -{ - -} - -State * WristState::handle_OnButtonDownPressed() +State * WristState::handle_OnEnter() { - State::handle_OnButtonDownPressed(); - return this; + return new PinchState(); } -State * WristState::handle_OnButtonDownReleased() +State * WristState::handle_OnUp() { - State::handle_OnButtonDownReleased(); return this; } -State * WristState::handle_OnButtonUp() +State * WristState::handle_OnDown() { return this; } -State * WristState::handle_OnButtonLeft() +State * WristState::handle_OnLeft() { return this; } -State * WristState::handle_OnButtonRight() +State * WristState::handle_OnRight() { return this; } -State * WristState::handle_OnButtonEnter() -{ - return new PinchState(); -} - /************************************************************************************** * PinchState **************************************************************************************/ -void PinchState::onEnter() +PinchState::PinchState() { Braccio.lvgl_lock(); lv_label_set_text(label, "Pinch"); Braccio.lvgl_unlock(); } -void PinchState::onExit() +State * PinchState::handle_OnEnter() { - + return new ShoulderState(); } -State * PinchState::handle_OnButtonLeft() +State * PinchState::handle_OnLeft() { + gripper.move().to(gripper.position() + 10.0f).in(10ms); return this; } -State * PinchState::handle_OnButtonRight() +State * PinchState::handle_OnRight() { + gripper.move().to(gripper.position() - 10.0f).in(10ms); return this; } - -State * PinchState::handle_OnButtonEnter() -{ - return new ShoulderState(); -} diff --git a/examples/Controlling_Manually_Braccio/AppState.h b/examples/Controlling_Manually_Braccio/AppState.h index 828cb14..bde74ac 100644 --- a/examples/Controlling_Manually_Braccio/AppState.h +++ b/examples/Controlling_Manually_Braccio/AppState.h @@ -2,17 +2,18 @@ #define APP_STATE_H_ /************************************************************************************** - * TYPEDEF + * INCLUDE **************************************************************************************/ -enum class EventSource -{ - Button_DownPressed, Button_DownReleased, Button_Up, Button_Left, Button_Right, Button_Enter -}; +#include + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ -enum class StateName +enum class Button { - Shoulder, Elbow, Wrist, Pinch + None, Enter, Home, Up, Down, Left, Right }; /************************************************************************************** @@ -23,125 +24,109 @@ class State { public: virtual ~State() { } - virtual void onEnter() { } - virtual void onExit() { } - virtual StateName name() = 0; - State * update(EventSource const evt_src) + State * update(Button const button) { State * next_state = this; - switch (evt_src) + switch (button) { - case EventSource::Button_DownPressed: next_state = handle_OnButtonDownPressed(); break; - case EventSource::Button_DownReleased: next_state = handle_OnButtonDownReleased(); break; - - case EventSource::Button_Up: next_state = handle_OnButtonUp(); break; - case EventSource::Button_Left: next_state = handle_OnButtonLeft(); break; - case EventSource::Button_Right: next_state = handle_OnButtonRight(); break; - case EventSource::Button_Enter: next_state = handle_OnButtonEnter(); break; + 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_OnButtonDownPressed(); - virtual State * handle_OnButtonDownReleased(); - virtual State * handle_OnButtonUp() { return this; } - virtual State * handle_OnButtonLeft() { return this; } - virtual State * handle_OnButtonRight() { return this; } - virtual State * handle_OnButtonEnter() { return this; } + 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() { } - virtual StateName name() override { return StateName::Shoulder; } - virtual void onEnter() override; - virtual void onExit() override; - protected: - virtual State * handle_OnButtonDownPressed() override; - virtual State * handle_OnButtonDownReleased() override; - virtual State * handle_OnButtonUp() override; - virtual State * handle_OnButtonLeft() override; - virtual State * handle_OnButtonRight() override; - virtual State * handle_OnButtonEnter() override; + 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() { } - virtual StateName name() override { return StateName::Elbow; } - virtual void onEnter() override; - virtual void onExit() override; - protected: - virtual State * handle_OnButtonDownPressed() override; - virtual State * handle_OnButtonDownReleased() override; - virtual State * handle_OnButtonUp() override; - virtual State * handle_OnButtonEnter() override; + virtual State * handle_OnEnter() override; + virtual State * handle_OnUp () override; + virtual State * handle_OnDown () override; }; class WristState : public State { public: + WristState(); virtual ~WristState() { } - virtual StateName name() override { return StateName::Wrist; } - virtual void onEnter() override; - virtual void onExit() override; - protected: - virtual State * handle_OnButtonDownPressed() override; - virtual State * handle_OnButtonDownReleased() override; - virtual State * handle_OnButtonUp() override; - virtual State * handle_OnButtonLeft() override; - virtual State * handle_OnButtonRight() override; - virtual State * handle_OnButtonEnter() override; + 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() { } - virtual StateName name() override { return StateName::Pinch; } - virtual void onEnter() override; - virtual void onExit() override; - protected: - virtual State * handle_OnButtonLeft() override; - virtual State * handle_OnButtonRight() override; - virtual State * handle_OnButtonEnter() override; + virtual State * handle_OnEnter() override; + virtual State * handle_OnLeft () override; + virtual State * handle_OnRight() override; }; -class ControlApp +class ManualControlApp { public: - ControlApp() + ManualControlApp() : _state{nullptr} + , _mtx{} { } - void update(EventSource const evt_src) + void update(Button const button) { + mbed::ScopedLock lock(_mtx); + if (!_state) { _state = new ShoulderState(); - _state->onEnter(); + return; } - State * next_state = _state->update(evt_src); + State * next_state = _state->update(button); - if (next_state->name() != _state->name()) - { - _state->onExit(); + if (next_state != _state) { delete _state; _state = next_state; - _state->onEnter(); } } 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 index b623ecd..ada5652 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -1,26 +1,23 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include #include "AppState.h" +/************************************************************************************** + * DEFINES + **************************************************************************************/ + // Colors #define COLOR_TEAL 0x00878F #define COLOR_LIGHT_TEAL 0x62AEB2 #define COLOR_YELLOW 0xE5AD24 -// Variables -float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 190.0}; -float angles[6]; // All motors current angles - -// Braccio ++ joints -auto gripper = Braccio.get(1); -auto wristRoll = Braccio.get(2); -auto wristPitch = Braccio.get(3); -auto elbow = Braccio.get(4); -auto shoulder = Braccio.get(5); -auto base = Braccio.get(6); - -char * jointsPair[] = {"Shoulder", "Elbow", "Wrist", "Pinch"}; -String selectedJoints = "Shoulder"; +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ enum states { SHOULDER, @@ -29,8 +26,6 @@ enum states { PINCH }; -int state = SHOULDER; - // Values of clicked input keys returned by Braccio.getKey() function (defined inside the library). enum KEYS { @@ -42,10 +37,6 @@ enum KEYS ENTER = 6, }; -uint32_t pressed_key; -uint32_t last_pressed_key; - - // IDs of the displayed directional UI buttons enum BUTTONS { BTN_UP = 1, @@ -58,9 +49,31 @@ static const char *direction_btnm_map[] = {" ", LV_SYMBOL_UP, " ", "\n", LV_SYMBOL_LEFT, " ", LV_SYMBOL_RIGHT, "\n", " ", LV_SYMBOL_DOWN, " ", "\0"}; +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +// Variables +float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 190.0}; +float angles[6]; // All motors current angles + +// Braccio ++ joints +auto gripper = Braccio.get(1); +auto wristRoll = Braccio.get(2); +auto wristPitch = Braccio.get(3); +auto elbow = Braccio.get(4); +auto shoulder = Braccio.get(5); +auto base = Braccio.get(6); + +char * jointsPair[] = {"Shoulder", "Elbow", "Wrist", "Pinch"}; + +int state = SHOULDER; + lv_obj_t * direction_btnm; // Direction button matrix lv_obj_t * label; // Label +ManualControlApp app; + // Function void moveJoints(uint32_t btnID) { switch (state) { @@ -161,53 +174,7 @@ void updateButtons(uint32_t key) // Event Handlers -ControlApp app; - -static void eventHandlerDirectional(lv_event_t * e) -{ - lv_event_code_t code = lv_event_get_code(e); - - if (code == LV_EVENT_KEY) - { - lv_obj_t * obj = lv_event_get_target(e); - uint32_t const id = lv_btnmatrix_get_selected_btn(obj); - Serial.println(id); - - switch (id) - { - case UP : app.update(EventSource::Button_Up); break; - //case DOWN : app.update(EventSource::Button_Down); break; - case LEFT : app.update(EventSource::Button_Left); break; - case RIGHT: app.update(EventSource::Button_Right); break; - case CLICK: - case ENTER: app.update(EventSource::Button_Enter); break; - } - } -} - // Braccio.lvgl_lock(); - - // lv_event_code_t code = lv_event_get_code(e); - // lv_obj_t * obj = lv_event_get_target(e); - - // Braccio.lvgl_unlock(); - - // if (code == LV_EVENT_KEY){ - // pressed_key = Braccio.getKey(); - - // if (pressed_key == ENTER || pressed_key == CLICK){ - // state++; // Index the next joints in the states enum array - - // if (state > PINCH){ - // state = SHOULDER; // restart from the shoulder - // } - // } - // updateButtons(pressed_key); - // Braccio.positions(angles); - // delay(5); - // moveJoints(pressed_key); - // } -// } // Screens functions @@ -245,18 +212,13 @@ void directionScreen(void) lv_btnmatrix_set_one_checked(direction_btnm, true); lv_btnmatrix_set_selected_btn(direction_btnm, 1); - lv_obj_add_event_cb(direction_btnm, eventHandlerDirectional, LV_EVENT_ALL, NULL); - - label = lv_label_create(lv_scr_act()); + 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, jointsPair[state]); Braccio.lvgl_unlock(); - - //delay(50); - //Braccio.connectJoystickTo(direction_btnm); } void setup() @@ -266,6 +228,8 @@ void setup() Braccio.begin(directionScreen); delay(500); // Waits for the Braccio initialization + app.update(Button::None); + Braccio.speed(SLOW); Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); @@ -280,34 +244,122 @@ void loop() if ((now - prev_enter_event) > 200 && Braccio.isButtonPressed_ENTER()) { prev_enter_event = now; - app.update(EventSource::Button_Enter); + app.update(Button::Enter); } } - if (Braccio.isJoystickPressed_DOWN()) { - app.update(EventSource::Button_DownPressed); - Serial.println("Braccio.isJoystickPressed_DOWN()"); - } else { - app.update(EventSource::Button_DownReleased); + /* 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; - /* - pressed_key= Braccio.getKey(); - if (pressed_key == 0) { - if(pressed_key != last_pressed_key){ - Braccio.lvgl_lock(); - - lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); - lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); - lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); - lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); - - Braccio.lvgl_unlock(); - - delay(50); + /* 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(); } - last_pressed_key=pressed_key; - */ + 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; + + 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); + + delay(10); +} + +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(); } From 3725c9cc7c6a22b355236c00ef39c43695da2c6c Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 06:21:03 +0200 Subject: [PATCH 16/23] Hide buttons not used in a given state, execute enter action on press down. --- .../Controlling_Manually_Braccio/AppState.cpp | 32 +++++++++++++++++++ .../Controlling_Manually_Braccio/AppState.h | 4 +-- .../Controlling_Manually_Braccio.ino | 18 +++++------ 3 files changed, 43 insertions(+), 11 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/AppState.cpp b/examples/Controlling_Manually_Braccio/AppState.cpp index c74c797..f3de488 100644 --- a/examples/Controlling_Manually_Braccio/AppState.cpp +++ b/examples/Controlling_Manually_Braccio/AppState.cpp @@ -6,11 +6,23 @@ #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 @@ -67,6 +79,16 @@ 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(); } @@ -129,6 +151,16 @@ 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(); } diff --git a/examples/Controlling_Manually_Braccio/AppState.h b/examples/Controlling_Manually_Braccio/AppState.h index bde74ac..11dba57 100644 --- a/examples/Controlling_Manually_Braccio/AppState.h +++ b/examples/Controlling_Manually_Braccio/AppState.h @@ -66,7 +66,7 @@ class ElbowState : public State { public: ElbowState(); - virtual ~ElbowState() { } + virtual ~ElbowState(); protected: virtual State * handle_OnEnter() override; virtual State * handle_OnUp () override; @@ -91,7 +91,7 @@ class PinchState : public State { public: PinchState(); - virtual ~PinchState() { } + virtual ~PinchState(); protected: virtual State * handle_OnEnter() override; virtual State * handle_OnLeft () override; diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index ada5652..c014655 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -144,7 +144,7 @@ void updateButtons(uint32_t key) lv_btnmatrix_set_selected_btn(direction_btnm, NULL); Braccio.lvgl_unlock(); } - +/* if (state == ELBOW){ Braccio.lvgl_lock(); lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_HIDDEN); @@ -165,6 +165,7 @@ void updateButtons(uint32_t key) lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN); Braccio.lvgl_unlock(); } + */ /* Braccio.lvgl_lock(); lv_label_set_text(label, jointsPair[state]); @@ -238,15 +239,14 @@ void setup() void loop() { - if (Braccio.isButtonPressed_ENTER()) { - static auto prev_enter_event = millis(); - auto const now = millis(); - if ((now - prev_enter_event) > 200 && Braccio.isButtonPressed_ENTER()) - { - prev_enter_event = now; - app.update(Button::Enter); - } + /* 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; /* DOWN */ From 2cb30d98fa674f5f80882abdf838f2f1639ee2a0 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 06:23:22 +0200 Subject: [PATCH 17/23] Same functionality for SELECT as for ENTER. --- .../Controlling_Manually_Braccio.ino | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index c014655..78cd21b 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -248,6 +248,15 @@ void loop() } 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; From ffd4e8d83945e95bf7b2205dd02f46280f04b7ad Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 06:30:34 +0200 Subject: [PATCH 18/23] Only enable buttons after init sequence is complete. --- .../Controlling_Manually_Braccio.ino | 30 ++++++++++++------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 78cd21b..8271e9d 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -201,13 +201,13 @@ void directionScreen(void) 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_CHECKABLE); + 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_CHECKABLE); + 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_CHECKABLE); + 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_CHECKABLE); + 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); @@ -226,15 +226,23 @@ void setup() { Serial.begin(115200); - Braccio.begin(directionScreen); - delay(500); // Waits for the Braccio initialization + if (Braccio.begin(directionScreen)) + { + /* Configure Braccio. */ + Braccio.speed(SLOW); + /* Move to home position. */ + Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); + delay(500); + /* 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(); + } app.update(Button::None); - - Braccio.speed(SLOW); - - Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); - delay(500); } void loop() From e3a806981b5ef1a6829542f6271ae823813f4a99 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 06:48:23 +0200 Subject: [PATCH 19/23] Check buttons every 100 ms. --- .../Controlling_Manually_Braccio/AppState.cpp | 4 +-- .../Controlling_Manually_Braccio.ino | 26 ++++++++++++------- 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/AppState.cpp b/examples/Controlling_Manually_Braccio/AppState.cpp index f3de488..8eecef5 100644 --- a/examples/Controlling_Manually_Braccio/AppState.cpp +++ b/examples/Controlling_Manually_Braccio/AppState.cpp @@ -171,12 +171,12 @@ State * PinchState::handle_OnEnter() State * PinchState::handle_OnLeft() { - gripper.move().to(gripper.position() + 10.0f).in(10ms); + gripper.move().to(gripper.position() + 2.5f); return this; } State * PinchState::handle_OnRight() { - gripper.move().to(gripper.position() - 10.0f).in(10ms); + gripper.move().to(gripper.position() - 2.5f); return this; } diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 8271e9d..392373c 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -229,7 +229,7 @@ void setup() if (Braccio.begin(directionScreen)) { /* Configure Braccio. */ - Braccio.speed(SLOW); + Braccio.speed(MEDIUM); /* Move to home position. */ Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); delay(500); @@ -313,16 +313,22 @@ void loop() } prev_joystick_pressed_right = curr_joystick_pressed_right; - 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); - delay(10); + static auto prev = millis(); + auto const now = millis(); + if ((now - prev) > 100) + { + 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); + } } void handle_OnButtonDownPressed() From 87fdd598311ab50e0d6c12fe2de56241d2f8b085 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 06:51:26 +0200 Subject: [PATCH 20/23] Only check for button pressed/released events every 10 ms (debouncing). --- .../Controlling_Manually_Braccio.ino | 50 ++++++++++++------- 1 file changed, 33 insertions(+), 17 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 392373c..b6146f4 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -246,6 +246,39 @@ void setup() } void loop() +{ + /* Execute every 10 ms. */ + { + static auto prev = millis(); + auto const now = millis(); + if ((now - prev) > 10) + { + prev = now; + handle_ButtonPressedReleased(); + } + } + + /* Execute every 100 ms. */ + { + static auto prev = millis(); + auto const now = millis(); + if ((now - prev) > 100) + { + 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); + } + } +} + +void handle_ButtonPressedReleased() { /* ENTER */ @@ -312,23 +345,6 @@ void loop() handle_OnButtonRightReleased(); } prev_joystick_pressed_right = curr_joystick_pressed_right; - - - static auto prev = millis(); - auto const now = millis(); - if ((now - prev) > 100) - { - 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); - } } void handle_OnButtonDownPressed() From 33437a551ab6979e2484d291727eeb95ecf26d06 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 07:09:14 +0200 Subject: [PATCH 21/23] Cleanup. --- .../Controlling_Manually_Braccio.ino | 144 +----------------- 1 file changed, 5 insertions(+), 139 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index b6146f4..75d67b3 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -19,24 +19,6 @@ * TYPEDEF **************************************************************************************/ -enum states { - SHOULDER, - ELBOW, - WRIST, - PINCH -}; - -// Values of clicked input keys returned by Braccio.getKey() function (defined inside the library). -enum KEYS -{ - UP = 4, - DOWN = 5, - LEFT = 1, - RIGHT = 2, - CLICK = 3, // click of joystick - ENTER = 6, -}; - // IDs of the displayed directional UI buttons enum BUTTONS { BTN_UP = 1, @@ -54,129 +36,13 @@ static const char *direction_btnm_map[] = {" ", LV_SYMBOL_UP, " ", "\n", **************************************************************************************/ // Variables -float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 190.0}; -float angles[6]; // All motors current angles - -// Braccio ++ joints -auto gripper = Braccio.get(1); -auto wristRoll = Braccio.get(2); -auto wristPitch = Braccio.get(3); -auto elbow = Braccio.get(4); -auto shoulder = Braccio.get(5); -auto base = Braccio.get(6); - -char * jointsPair[] = {"Shoulder", "Elbow", "Wrist", "Pinch"}; - -int state = SHOULDER; +static float const HOME_POS[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; lv_obj_t * direction_btnm; // Direction button matrix lv_obj_t * label; // Label ManualControlApp app; -// Function -void moveJoints(uint32_t btnID) { - switch (state) { - case SHOULDER: - switch (btnID) { - case 4: shoulder.move().to(angles[4] - 10.0); break; - case 1: base.move().to(angles[5] - 10.0); break; - case 2: base.move().to(angles[5] + 10.0); break; - case 5: shoulder.move().to(angles[4] + 10.0); break; - default: break; - } - break; - case ELBOW: - switch (btnID) { - case 4: elbow.move().to(angles[3] - 10.0); break; - case 5: elbow.move().to(angles[3] + 10.0); break; - default: break; - } - break; - case WRIST: - switch (btnID) { - case 4: wristPitch.move().to(angles[2] - 10.0); break; - case 1: wristRoll.move().to(angles[1] - 10.0); break; - case 2: wristRoll.move().to(angles[1] + 10.0); break; - case 5: wristPitch.move().to(angles[2] + 10.0); break; - default: break; - } - break; - case PINCH: - switch (btnID) { - case 1: gripper.move().to(angles[0] + 10.0); break; - case 2: gripper.move().to(angles[0] - 10.0); break; - default: break; - } - default: - break; - } -} - -void updateButtons(uint32_t key) -{ - if (key == UP){ - Braccio.lvgl_lock(); - lv_btnmatrix_set_selected_btn(direction_btnm, BTN_UP); - lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED); - Braccio.lvgl_unlock(); - } - else if (key == DOWN){ - Braccio.lvgl_lock(); - lv_btnmatrix_set_selected_btn(direction_btnm, BTN_DOWN); - lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED); - Braccio.lvgl_unlock(); - } - else if (key == LEFT) { - Braccio.lvgl_lock(); - lv_btnmatrix_set_selected_btn(direction_btnm, BTN_LEFT); - lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED); - Braccio.lvgl_unlock(); - } - else if (key == RIGHT){ - Braccio.lvgl_lock(); - lv_btnmatrix_set_selected_btn(direction_btnm, BTN_RIGHT); - lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED); - Braccio.lvgl_unlock(); - } - else { - Braccio.lvgl_lock(); - lv_btnmatrix_set_selected_btn(direction_btnm, NULL); - Braccio.lvgl_unlock(); - } -/* - if (state == ELBOW){ - Braccio.lvgl_lock(); - 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(); - } - else if (state == PINCH){ - Braccio.lvgl_lock(); - 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(); - } - else{ - 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); - 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(); - } - */ - /* - Braccio.lvgl_lock(); - lv_label_set_text(label, jointsPair[state]); - Braccio.lvgl_unlock(); - */ -} - -// Event Handlers - - - // Screens functions void directionScreen(void) @@ -217,7 +83,7 @@ void directionScreen(void) 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, jointsPair[state]); + lv_label_set_text(label, ""); Braccio.lvgl_unlock(); } @@ -231,8 +97,10 @@ void setup() /* Configure Braccio. */ Braccio.speed(MEDIUM); /* Move to home position. */ - Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); + 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); @@ -241,8 +109,6 @@ void setup() lv_btnmatrix_clear_btn_ctrl(direction_btnm, 7, LV_BTNMATRIX_CTRL_DISABLED); Braccio.lvgl_unlock(); } - - app.update(Button::None); } void loop() From 4a8e9ce927e6680973f513a763645bcb201f44f6 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 10:08:04 +0200 Subject: [PATCH 22/23] Finalising application. --- .../Controlling_Manually_Braccio/AppState.cpp | 14 ++- .../Controlling_Manually_Braccio.ino | 115 ++++++++++-------- 2 files changed, 73 insertions(+), 56 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/AppState.cpp b/examples/Controlling_Manually_Braccio/AppState.cpp index 8eecef5..050a1ec 100644 --- a/examples/Controlling_Manually_Braccio/AppState.cpp +++ b/examples/Controlling_Manually_Braccio/AppState.cpp @@ -53,21 +53,25 @@ State * ShoulderState::handle_OnEnter() 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; } @@ -99,11 +103,13 @@ State * ElbowState::handle_OnEnter() 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; } @@ -125,21 +131,25 @@ State * WristState::handle_OnEnter() State * WristState::handle_OnUp() { + wristRoll.move().to(wristRoll.position() + 5.0f); return this; } State * WristState::handle_OnDown() { + wristRoll.move().to(wristRoll.position() - 5.0f); return this; } State * WristState::handle_OnLeft() { + wristPitch.move().to(wristPitch.position() + 5.0f); return this; } State * WristState::handle_OnRight() { + wristPitch.move().to(wristPitch.position() - 5.0f); return this; } @@ -171,12 +181,12 @@ State * PinchState::handle_OnEnter() State * PinchState::handle_OnLeft() { - gripper.move().to(gripper.position() + 2.5f); + gripper.move().to(gripper.position() + 5.0f); return this; } State * PinchState::handle_OnRight() { - gripper.move().to(gripper.position() - 2.5f); + gripper.move().to(gripper.position() - 5.0f); return this; } diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 75d67b3..23ed869 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -10,7 +10,6 @@ * DEFINES **************************************************************************************/ -// Colors #define COLOR_TEAL 0x00878F #define COLOR_LIGHT_TEAL 0x62AEB2 #define COLOR_YELLOW 0xE5AD24 @@ -27,66 +26,27 @@ enum BUTTONS { BTN_RIGHT = 5, }; -static const char *direction_btnm_map[] = {" ", LV_SYMBOL_UP, " ", "\n", - LV_SYMBOL_LEFT, " ", LV_SYMBOL_RIGHT, "\n", - " ", LV_SYMBOL_DOWN, " ", "\0"}; - /************************************************************************************** - * GLOBAL VARIABLES + * CONSTANTS **************************************************************************************/ -// Variables 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"}; -lv_obj_t * direction_btnm; // Direction button matrix -lv_obj_t * label; // Label - -ManualControlApp app; - -// Screens 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); +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ - 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_obj_t * direction_btnm; +lv_obj_t * label; - lv_btnmatrix_set_one_checked(direction_btnm, true); - lv_btnmatrix_set_selected_btn(direction_btnm, 1); +ManualControlApp app; - 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(); -} +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ void setup() { @@ -95,7 +55,7 @@ void setup() if (Braccio.begin(directionScreen)) { /* Configure Braccio. */ - Braccio.speed(MEDIUM); + Braccio.speed(speed_grade_t(150)/*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); @@ -144,6 +104,53 @@ void loop() } } +/************************************************************************************** + * 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 */ From fa97865343b038aaf947efa51b4e8379f7c0e7a4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 11 May 2022 10:30:57 +0200 Subject: [PATCH 23/23] Fine tuning ... --- examples/Controlling_Manually_Braccio/AppState.cpp | 8 ++++---- .../Controlling_Manually_Braccio.ino | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/Controlling_Manually_Braccio/AppState.cpp b/examples/Controlling_Manually_Braccio/AppState.cpp index 050a1ec..13cd03b 100644 --- a/examples/Controlling_Manually_Braccio/AppState.cpp +++ b/examples/Controlling_Manually_Braccio/AppState.cpp @@ -131,25 +131,25 @@ State * WristState::handle_OnEnter() State * WristState::handle_OnUp() { - wristRoll.move().to(wristRoll.position() + 5.0f); + wristPitch.move().to(wristPitch.position() + 5.0f); return this; } State * WristState::handle_OnDown() { - wristRoll.move().to(wristRoll.position() - 5.0f); + wristPitch.move().to(wristPitch.position() - 5.0f); return this; } State * WristState::handle_OnLeft() { - wristPitch.move().to(wristPitch.position() + 5.0f); + wristRoll.move().to(wristRoll.position() + 10.0f); return this; } State * WristState::handle_OnRight() { - wristPitch.move().to(wristPitch.position() - 5.0f); + wristRoll.move().to(wristRoll.position() - 10.0f); return this; } diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 23ed869..4d2f5ea 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -55,7 +55,7 @@ void setup() if (Braccio.begin(directionScreen)) { /* Configure Braccio. */ - Braccio.speed(speed_grade_t(150)/*MEDIUM*/); + 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); @@ -84,11 +84,11 @@ void loop() } } - /* Execute every 100 ms. */ + /* Execute every 50 ms. */ { static auto prev = millis(); auto const now = millis(); - if ((now - prev) > 100) + if ((now - prev) > 50) { prev = now;