diff --git a/.github/workflows/sync-labels.yml b/.github/workflows/sync-labels.yml index 986bda6..94938f3 100644 --- a/.github/workflows/sync-labels.yml +++ b/.github/workflows/sync-labels.yml @@ -31,7 +31,7 @@ jobs: - name: Download JSON schema for labels configuration file id: download-schema - uses: carlosperate/download-file-action@v1 + uses: carlosperate/download-file-action@v2 with: file-url: https://raw.githubusercontent.com/arduino/tooling-project-assets/main/workflow-templates/assets/sync-labels/arduino-tooling-gh-label-configuration-schema.json location: ${{ runner.temp }}/label-configuration-schema @@ -65,7 +65,7 @@ jobs: steps: - name: Download - uses: carlosperate/download-file-action@v1 + uses: carlosperate/download-file-action@v2 with: file-url: https://raw.githubusercontent.com/arduino/tooling-project-assets/main/workflow-templates/assets/sync-labels/${{ matrix.filename }} @@ -114,7 +114,7 @@ jobs: path: ${{ env.CONFIGURATIONS_FOLDER }} - name: Remove unneeded artifact - uses: geekyeggo/delete-artifact@v1 + uses: geekyeggo/delete-artifact@v2 with: name: ${{ env.CONFIGURATIONS_ARTIFACT }} diff --git a/.gitignore b/.gitignore index b1babc9..9fa72d9 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ extras/build +.vscode diff --git a/examples/Braccio_Record_and_Replay/AppState.cpp b/examples/Braccio_Record_and_Replay/AppState.cpp index bc8c875..d25cac7 100644 --- a/examples/Braccio_Record_and_Replay/AppState.cpp +++ b/examples/Braccio_Record_and_Replay/AppState.cpp @@ -137,6 +137,41 @@ State * IdleState::handle_OnZeroPosition() return new ZeroState(); } +State * IdleState::handle_OnTimerTick() +{ + /* If data has been recorded and a serial command is + * sent to the device then all recorded positions are + * sent to the PC. + */ + if (!sample_cnt) + return this; + + if (!Serial) + return this; + + if (!Serial.available()) + return this; + + if (Serial.read() == 'r') + { + for (int i = 0; i < sample_cnt; i += 6) + { + char msg[64] = {0}; + snprintf(msg, sizeof(msg), "%d;%0.2f;%0.2f;%0.2f;%0.2f;%0.2f;%0.2f;", + i / 6, + sample_buf[i + 0], + sample_buf[i + 1], + sample_buf[i + 2], + sample_buf[i + 3], + sample_buf[i + 4], + sample_buf[i + 5]); + Serial.println(msg); + } + } + + return this; +} + /************************************************************************************** * RecordState **************************************************************************************/ diff --git a/examples/Braccio_Record_and_Replay/AppState.h b/examples/Braccio_Record_and_Replay/AppState.h index 09fd28b..0888f26 100644 --- a/examples/Braccio_Record_and_Replay/AppState.h +++ b/examples/Braccio_Record_and_Replay/AppState.h @@ -70,6 +70,7 @@ class IdleState : public State virtual State * handle_OnRecord () override; virtual State * handle_OnReplay () override; virtual State * handle_OnZeroPosition() override; + virtual State * handle_OnTimerTick () override; }; class RecordState : public State diff --git a/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino b/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino index 8e0439d..88e6b06 100644 --- a/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino +++ b/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino @@ -18,8 +18,11 @@ RecordAndReplayApp app; void setup() { - if (Braccio.begin(custom_main_menu)) { + if (Braccio.begin(custom_main_menu)) + { app.enableButtons(); + /* Allow greater angular velocity than the default one. */ + Braccio.setAngularVelocity(45.0f); } } diff --git a/examples/Braccio_Record_and_Replay/README.md b/examples/Braccio_Record_and_Replay/README.md new file mode 100644 index 0000000..9010372 --- /dev/null +++ b/examples/Braccio_Record_and_Replay/README.md @@ -0,0 +1,11 @@ +`Braccio_Record_and_Replay` +=========================== +For debug purposes all recorded angles can be received via serial command. The approach to do this is: +* Start `minicom`. +```bash +minicom -D /dev/ttyACM0 -C angle_log.csv +``` +* Press `Record` and record some angles. +* Retrieve the recorded angles by sending `r` (Press `r`). You'll see the output on your console which will automagically be stored in the CSV file. +* Close `minicom` with `Ctrl + A, Q`. + diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 4d2f5ea..679fd73 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -54,8 +54,6 @@ void setup() if (Braccio.begin(directionScreen)) { - /* Configure Braccio. */ - Braccio.speed(speed_grade_t(120)/*MEDIUM*/); /* Move to home position. */ Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); delay(500); diff --git a/examples/Platform_Tutorials/lessons/03-important-programming-concepts/Braccio_moving_range_angles/Braccio_moving_range_angles.ino b/examples/Platform_Tutorials/lessons/03-important-programming-concepts/Braccio_moving_range_angles/Braccio_moving_range_angles.ino new file mode 100644 index 0000000..7206a1b --- /dev/null +++ b/examples/Platform_Tutorials/lessons/03-important-programming-concepts/Braccio_moving_range_angles/Braccio_moving_range_angles.ino @@ -0,0 +1,117 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +/************************************************************************************** + * DEFINES + **************************************************************************************/ + +#define BUTTON_SELECT 3 +#define BUTTON_ENTER 6 +#define TIME_DELAY 2000 + +/************************************************************************************** + * CONSTANTS + **************************************************************************************/ + +static float const HOME_POS[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +static auto gripper = Braccio.get(1); +static auto wristRoll = Braccio.get(2); +static auto wristPitch = Braccio.get(3); +static auto elbow = Braccio.get(4); +static auto shoulder = Braccio.get(5); +static auto base = Braccio.get(6); + +float angles[6]; + +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ + +void setup() { + if (Braccio.begin()) + { + /* Warning: keep a safe distance from the robot and watch out for the robot's + movement. It could be speedy and hit someone. */ + /* 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(TIME_DELAY); + } +} + +void loop() { + int pressedKey = Braccio.getKey(); + + if (pressedKey == BUTTON_ENTER) + { + // Pinch movement + gripper.move().to(230.0f); delay(TIME_DELAY); + gripper.move().to(HOME_POS[0]); delay(TIME_DELAY); + + // // Wrist Roll movement + wristRoll.move().to(0.0f); delay(TIME_DELAY); + wristRoll.move().to(HOME_POS[1]); delay(TIME_DELAY); + wristRoll.move().to(315.0f); delay(TIME_DELAY); + wristRoll.move().to(HOME_POS[1]); delay(TIME_DELAY); + + // // Wrist Pitch movement + wristPitch.move().to(70.0f); delay(TIME_DELAY); + wristPitch.move().to(HOME_POS[2]); delay(TIME_DELAY); + wristPitch.move().to(260.0f); delay(TIME_DELAY); + wristPitch.move().to(HOME_POS[2]); delay(TIME_DELAY); + + // Elbow movement + for(float i=HOME_POS[3]; i >= 70.0; i-=5) + { elbow.move().to(i); delay(TIME_DELAY/2000); } + elbow.move().to(HOME_POS[3]); delay(TIME_DELAY); + for(float i=HOME_POS[3]; i <= 260.0; i+=5) + { elbow.move().to(i); delay(TIME_DELAY/2000); } + elbow.move().to(HOME_POS[3]); delay(TIME_DELAY); + + // Shoulder movement + shoulder.move().to(120.0f); delay(TIME_DELAY/2); + shoulder.move().to(90.0f); delay(TIME_DELAY/2); + shoulder.move().to(120.0f); delay(TIME_DELAY/2); + shoulder.move().to(HOME_POS[4]); delay(TIME_DELAY); + shoulder.move().to(200.0f); delay(TIME_DELAY/2); + shoulder.move().to(230.0f); delay(TIME_DELAY/2); + shoulder.move().to(200.0f); delay(TIME_DELAY/2); + shoulder.move().to(HOME_POS[4]); delay(TIME_DELAY); + + // Base movement + base.move().to(0.0f); delay(TIME_DELAY); + base.move().to(HOME_POS[5]); delay(TIME_DELAY); + base.move().to(315.0f); delay(TIME_DELAY); + base.move().to(HOME_POS[5]); delay(TIME_DELAY); + + while(pressedKey == BUTTON_ENTER) { pressedKey = Braccio.getKey(); } + } + + if (pressedKey == BUTTON_SELECT) + { + // Fetch the joints positions + Braccio.positions(angles); + + // Print the joint angles + Serial.println("************* Joints Angles *************"); + Serial.println("|\tMotor ID\t|\tAngle\t|"); + Serial.println("----------------------------------------"); + Serial.print("| 1 - Gripper\t\t|\t" + String(angles[0]) + "\t|\n" + + "| 2 - Wrist Rotation\t|\t" + String(angles[1]) + "\t|\n" + + "| 3 - Wrist Vertical\t|\t" + String(angles[2]) + "\t|\n" + + "| 4 - Elbow\t\t|\t" + String(angles[3]) + "\t|\n" + + "| 5 - Shoulder\t\t|\t" + String(angles[4]) + "\t|\n" + + "| 6 - Base\t\t|\t" + String(angles[5]) + "\t|\n" + + "*****************************************\n\n\n\n\n"); + + while(pressedKey == BUTTON_SELECT) { pressedKey = Braccio.getKey(); } + + } +} \ No newline at end of file diff --git a/examples/Platform_Tutorials/lessons/03-important-programming-concepts/Navigating_the_display_menu/Navigating_the_display_menu.ino b/examples/Platform_Tutorials/lessons/03-important-programming-concepts/Navigating_the_display_menu/Navigating_the_display_menu.ino new file mode 100644 index 0000000..a6b107b --- /dev/null +++ b/examples/Platform_Tutorials/lessons/03-important-programming-concepts/Navigating_the_display_menu/Navigating_the_display_menu.ino @@ -0,0 +1,81 @@ +#include + +#define MARGIN_LEFT 0 +#define MARGIN_TOP 0 + +// Arduino Colors +#define COLOR_TEAL 0x00878F +#define COLOR_LIGHT_TEAL 0x62AEB2 +#define COLOR_ORANGE 0xE47128 +#define COLOR_YELLOW 0xE5AD24 +#define COLOR_WHITE 0xFFFFFF + +static const char * btnm_map[] = {"Option 1", "Option 2", "\n", + "Option 3", "Option 4", "\n", + "Option 5", "Option 6", "\0" + }; + +void setup() { + if (!Braccio.begin(customMenu)) { + if (Serial) Serial.println("Braccio.begin() failed."); + for(;;) { } + } +} + +void loop() { + // Let here empty. +} + +void customMenu(){ + Braccio.lvgl_lock(); + + static lv_style_t style_bg; + lv_style_init(&style_bg); + lv_style_set_bg_color(&style_bg, lv_color_hex(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_obj_t * btnm = lv_btnmatrix_create(lv_scr_act()); + lv_btnmatrix_set_map(btnm, btnm_map); + lv_obj_align(btnm, LV_ALIGN_CENTER, MARGIN_LEFT, MARGIN_TOP); + + 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_btn_ctrl(btnm, 4, LV_BTNMATRIX_CTRL_CHECKABLE); + lv_btnmatrix_set_btn_ctrl(btnm, 5, LV_BTNMATRIX_CTRL_CHECKABLE); + + lv_btnmatrix_set_one_checked(btnm, true); + + lv_obj_add_event_cb(btnm, eventHandler, LV_EVENT_ALL, NULL); + + Braccio.lvgl_unlock(); + + Braccio.connectJoystickTo(btnm); +} + +static void eventHandler(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); + if (code == LV_EVENT_CLICKED) { + uint32_t id = lv_btnmatrix_get_selected_btn(obj); + const char * txt = lv_btnmatrix_get_btn_text(obj, id); + + LV_LOG_USER("%s was selected\n", txt); + Serial.println(String(txt) + " was selected."); + } + + Braccio.lvgl_unlock(); +} \ No newline at end of file diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/01_creating_a_button/01_creating_a_button.ino b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/01_creating_a_button/01_creating_a_button.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/01_creating_a_button/01_creating_a_button.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/01_creating_a_button/01_creating_a_button.ino diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/01_creating_a_button/sketch.json b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/01_creating_a_button/sketch.json similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/01_creating_a_button/sketch.json rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/01_creating_a_button/sketch.json diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/02_designing_the_button/02_designing_the_button.ino b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/02_designing_the_button/02_designing_the_button.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/02_designing_the_button/02_designing_the_button.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/02_designing_the_button/02_designing_the_button.ino diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/02_designing_the_button/btn_style.ino.NANO_RP2040_CONNECT.bin b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/02_designing_the_button/btn_style.ino.NANO_RP2040_CONNECT.bin similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/02_designing_the_button/btn_style.ino.NANO_RP2040_CONNECT.bin rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/02_designing_the_button/btn_style.ino.NANO_RP2040_CONNECT.bin diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/02_designing_the_button/sketch.json b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/02_designing_the_button/sketch.json similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/02_designing_the_button/sketch.json rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/02_designing_the_button/sketch.json diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/03_creating_a_menu/03_creating_a_menu.ino b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/03_creating_a_menu/03_creating_a_menu.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/03_creating_a_menu/03_creating_a_menu.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/03_creating_a_menu/03_creating_a_menu.ino diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/04_testing_it_out/04_testing_it_out.ino b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/04_testing_it_out/04_testing_it_out.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/04_testing_it_out/04_testing_it_out.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/04_testing_it_out/04_testing_it_out.ino diff --git a/examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/05_display_challenge/05_display_challenge.ino b/examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/05_display_challenge/05_display_challenge.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/01-programming-the-braccio-display/05_display_challenge/05_display_challenge.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/01-programming-the-braccio-display/05_display_challenge/05_display_challenge.ino diff --git a/examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/01_playing_with_the_Joystick/01_playing_with_the_Joystick.ino b/examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/01_playing_with_the_Joystick/01_playing_with_the_Joystick.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/01_playing_with_the_Joystick/01_playing_with_the_Joystick.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/01_playing_with_the_Joystick/01_playing_with_the_Joystick.ino diff --git a/examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/02_handling_events_in_the_menu/02_handling_events_in_the_menu.ino b/examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/02_handling_events_in_the_menu/02_handling_events_in_the_menu.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/02_handling_events_in_the_menu/02_handling_events_in_the_menu.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/02_handling_events_in_the_menu/02_handling_events_in_the_menu.ino diff --git a/examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/03_navigate_challenge_I/03_navigate_challenge_I.ino b/examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/03_navigate_challenge_I/03_navigate_challenge_I.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/03_navigate_challenge_I/03_navigate_challenge_I.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/03_navigate_challenge_I/03_navigate_challenge_I.ino diff --git a/examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/04_navigate_challenge_II/04_navigate_challenge_II.ino b/examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/04_navigate_challenge_II/04_navigate_challenge_II.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/02-navigatting-the-display-menu/04_navigate_challenge_II/04_navigate_challenge_II.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/02-navigatting-the-display-menu/04_navigate_challenge_II/04_navigate_challenge_II.ino diff --git a/examples/Platform_Tutorials/lessons/03-playing-with-the-motors/01_playing_with_the_motors/01_playing_with_the_motors.ino b/examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/01_playing_with_the_motors/01_playing_with_the_motors.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/03-playing-with-the-motors/01_playing_with_the_motors/01_playing_with_the_motors.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/01_playing_with_the_motors/01_playing_with_the_motors.ino diff --git a/examples/Platform_Tutorials/lessons/03-playing-with-the-motors/02_selecting_the_motor_with_the_enter_button/02_selecting_the_motor_with_the_enter_button.ino b/examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/02_selecting_the_motor_with_the_enter_button/02_selecting_the_motor_with_the_enter_button.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/03-playing-with-the-motors/02_selecting_the_motor_with_the_enter_button/02_selecting_the_motor_with_the_enter_button.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/02_selecting_the_motor_with_the_enter_button/02_selecting_the_motor_with_the_enter_button.ino diff --git a/examples/Platform_Tutorials/lessons/03-playing-with-the-motors/03_moving_the_motors_with_the_joystick/03_moving_the_motors_with_the_joystick.ino b/examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/03_moving_the_motors_with_the_joystick/03_moving_the_motors_with_the_joystick.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/03-playing-with-the-motors/03_moving_the_motors_with_the_joystick/03_moving_the_motors_with_the_joystick.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/03_moving_the_motors_with_the_joystick/03_moving_the_motors_with_the_joystick.ino diff --git a/examples/Platform_Tutorials/lessons/03-playing-with-the-motors/04_servo_motors_challenge/04_servo_motors_challenge.ino b/examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/04_servo_motors_challenge/04_servo_motors_challenge.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/03-playing-with-the-motors/04_servo_motors_challenge/04_servo_motors_challenge.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/03-playing-with-the-motors/04_servo_motors_challenge/04_servo_motors_challenge.ino diff --git a/examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/01_playing_with_a_joint_angle_gauge/01_playing_with_a_joint_angle_gauge.ino b/examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/01_playing_with_a_joint_angle_gauge/01_playing_with_a_joint_angle_gauge.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/01_playing_with_a_joint_angle_gauge/01_playing_with_a_joint_angle_gauge.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/01_playing_with_a_joint_angle_gauge/01_playing_with_a_joint_angle_gauge.ino diff --git a/examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/02_selecting_the_motor_in_the_LCD_menu/02_selecting_the_motor_in_the_LCD_menu.ino b/examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/02_selecting_the_motor_in_the_LCD_menu/02_selecting_the_motor_in_the_LCD_menu.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/02_selecting_the_motor_in_the_LCD_menu/02_selecting_the_motor_in_the_LCD_menu.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/02_selecting_the_motor_in_the_LCD_menu/02_selecting_the_motor_in_the_LCD_menu.ino diff --git a/examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/03_learnings_challenge_I/03_learnings_challenge_I.ino b/examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/03_learnings_challenge_I/03_learnings_challenge_I.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/03_learnings_challenge_I/03_learnings_challenge_I.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/03_learnings_challenge_I/03_learnings_challenge_I.ino diff --git a/examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/04_learnings_challenge_II/04_learnings_challenge_II.ino b/examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/04_learnings_challenge_II/04_learnings_challenge_II.ino similarity index 100% rename from examples/Platform_Tutorials/lessons/04-integration-of-previous-learnings/04_learnings_challenge_II/04_learnings_challenge_II.ino rename to examples/Platform_Tutorials/lessons/Basic_codes/04-integration-of-previous-learnings/04_learnings_challenge_II/04_learnings_challenge_II.ino diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino index bd0c162..dee140c 100644 --- a/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino +++ b/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino @@ -1,6 +1,19 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include -// Variables +/************************************************************************************** + * DEFINES + **************************************************************************************/ + +#define TIME_DELAY 1000 + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + // Braccio ++ joints auto gripper = Braccio.get(1); auto wristRoll = Braccio.get(2); @@ -9,31 +22,28 @@ auto elbow = Braccio.get(4); auto shoulder = Braccio.get(5); auto base = Braccio.get(6); -float initialGripper = 160.0; float initialBase = 90.0; -float initialAngle = 150.0; +float initialAngle = 157.5; + float angles[6]; +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ + void setup() { - Braccio.begin(); - delay(500); // Waits for the Braccio initialization - - // You can choose the speed beforehand with - Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW - - // Send motors initial angle - gripper.move().to(initialGripper); - delay(100); - wristRoll.move().to(initialAngle); - delay(100); - wristPitch.move().to(initialAngle); - delay(100); - elbow.move().to(initialAngle); - delay(100); - shoulder.move().to(initialAngle); - delay(100); - base.move().to(initialBase); - delay(100); + if (Braccio.begin()){ + /* Warning: keep a safe distance from the robot and watch out for the robot's + movement. It could be speedy and hit someone. */ + + /* Move to home position. */ + gripper.move().to(initialAngle); delay(TIME_DELAY); + wristRoll.move().to(initialAngle); delay(TIME_DELAY); + wristPitch.move().to(initialAngle); delay(TIME_DELAY); + elbow.move().to(initialAngle); delay(TIME_DELAY); + shoulder.move().to(initialAngle); delay(TIME_DELAY); + base.move().to(initialBase); delay(TIME_DELAY); + } Serial.begin(115200); while(!Serial){} @@ -56,5 +66,5 @@ void loop() { Serial.println("*****************************************\n\n\n\n\n"); Serial.println("\n\n\n\n"); - delay(1000); + delay(TIME_DELAY); } diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino index 2bda3b1..f969acb 100644 --- a/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino +++ b/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino @@ -1,7 +1,27 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include +/************************************************************************************** + * DEFINES + **************************************************************************************/ + #define BUTTON_ENTER 6 +#define TIME_DELAY 1000 + +/************************************************************************************** + * CONSTANTS + **************************************************************************************/ +float const HOME_POS[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; +float const WAVE_POS[6] = {180.0, 260.0, 157.5, 157.5, 157.5, 180.0}; + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + // Braccio ++ joints auto gripper = Braccio.get(1); auto wristRoll = Braccio.get(2); @@ -10,27 +30,27 @@ auto elbow = Braccio.get(4); auto shoulder = Braccio.get(5); auto base = Braccio.get(6); -/* Variables */ -// initialAngles[6] = {gripper, wristRoll, wristPitch, elbow, shoulder, base} -float homePos[6] = {160.0, 150.0, 220.0, 220.0, 100.0, 180.0}; -float wavePos[6] = {180.0, 250.0, 145.0, 150.0, 150.0, 90.0}; - bool movement = false; // Flag to initialize joints' movements -void setup() { - Braccio.begin(); - delay(500); // Waits for the Braccio initialization +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ - // You can choose the speed beforehand with - Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW +void setup() { + if (Braccio.begin()){ + /* Warning: + Keep a safe distance from the robot until you make sure the code is properly + working. Be mindful of the robot’s movement prior to that, as it could be + speedy and accidentally hit someone. + */ - // Set motors initial angle - // Should move all the motors at once - Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); - delay(500); + /* 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(TIME_DELAY); + } } -// Waving whit Wrist pitch +// Waving with Wrist pitch void loop() { int pressedKey = Braccio.getKey(); @@ -38,16 +58,13 @@ void loop() { movement = true; // Trigger joints' movements if (movement) { - Braccio.moveTo(wavePos[0], wavePos[1], wavePos[2], wavePos[3], wavePos[4], wavePos[5]); - delay(1000); - - for (int i = 1; i <= 10; i++) { - wristPitch.move().to(100.0f); - delay(300); - wristPitch.move().to(190.0f); - delay(600); - wristPitch.move().to(145.0f); - delay(300); + Braccio.moveTo(WAVE_POS[0], WAVE_POS[1], WAVE_POS[2], WAVE_POS[3], WAVE_POS[4], WAVE_POS[5]); + delay(TIME_DELAY); + + for (int i = 1; i <= 5; i++) { + wristPitch.move().to(120.0f); delay(TIME_DELAY); + wristPitch.move().to(200.0f); delay(2*TIME_DELAY); + wristPitch.move().to(WAVE_POS[3]); } movement = false; // Stop joints' movements diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino index 2999342..cee77a0 100644 --- a/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino +++ b/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino @@ -1,6 +1,26 @@ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include +/************************************************************************************** + * DEFINES + **************************************************************************************/ + #define BUTTON_ENTER 6 +#define TIME_DELAY 1000 + +/************************************************************************************** + * CONSTANTS + **************************************************************************************/ + +static float const HOME_POS[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0}; +static float const AGREE_POS[6] = {160.0, 160.0, 210.0, 240.0, 100.0, 180.0}; + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ // Braccio ++ joints auto gripper = Braccio.get(1); @@ -10,23 +30,21 @@ auto elbow = Braccio.get(4); auto shoulder = Braccio.get(5); auto base = Braccio.get(6); -/* Variables */ -// initialAngles[6] = {gripper, wristRoll, wristPitch, elbow, shoulder, base} -float homePos[6] = {160.0, 150.0, 220.0, 220.0, 100.0, 180.0}; - bool movement = false; // Flag to initialize joints' movements -void setup() { - Braccio.begin(); - delay(500); // Waits for the Braccio initialization +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ - // You can choose the speed beforehand with - Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW +void setup() { + if (Braccio.begin()){ + /* Warning: keep a safe distance from the robot and watch out for the robot's + movement. It could be speedy and hit someone. */ - // Set motors initial angle - // Should move all the motors at once - Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); - delay(500); + /* 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); + } } void loop() { @@ -36,18 +54,15 @@ void loop() { movement = true; // Trigger joints' movements if (movement) { - Braccio.moveTo(160.0, 150.0, 220.0, 220.0, 100.0, 270.0); - delay(1000); + Braccio.moveTo(AGREE_POS[0], AGREE_POS[1], AGREE_POS[2], AGREE_POS[3], AGREE_POS[4], AGREE_POS[5]); + delay(TIME_DELAY/10); for (int i = 1; i <= 10; i++) { - wristPitch.move().to(190.0f); - delay(200); - wristPitch.move().to(250.0f); - delay(400); - wristPitch.move().to(220.0f); - delay(200); + wristPitch.move().to(190.0f); delay(TIME_DELAY/2); + wristPitch.move().to(240.0f); delay(TIME_DELAY); + wristPitch.move().to(AGREE_POS[3]); } - movement = false; // Stop joints' moviments + movement = false; // Stop joints' movements } } diff --git a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino index 4d2f5ea..679fd73 100644 --- a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino +++ b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino @@ -54,8 +54,6 @@ void setup() if (Braccio.begin(directionScreen)) { - /* Configure Braccio. */ - Braccio.speed(speed_grade_t(120)/*MEDIUM*/); /* Move to home position. */ Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); delay(500); diff --git a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino index 4d2f5ea..679fd73 100644 --- a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino +++ b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino @@ -54,8 +54,6 @@ void setup() if (Braccio.begin(directionScreen)) { - /* Configure Braccio. */ - Braccio.speed(speed_grade_t(120)/*MEDIUM*/); /* Move to home position. */ Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); delay(500); diff --git a/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino b/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino index c5180cf..5456ea5 100644 --- a/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino +++ b/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino @@ -20,6 +20,8 @@ void setup() { if (Braccio.begin(custom_main_menu)) { app.enableButtons(); + /* Allow greater angular velocity than the default one. */ + Braccio.setAngularVelocity(45.0f); } } diff --git a/examples/Tools/Braccio_Basic/Braccio_Basic.ino b/examples/Tools/Braccio_Basic/Braccio_Basic.ino index ebc00c7..0ea708c 100644 --- a/examples/Tools/Braccio_Basic/Braccio_Basic.ino +++ b/examples/Tools/Braccio_Basic/Braccio_Basic.ino @@ -79,15 +79,16 @@ void setup() Braccio.moveTo(home_position[0], home_position[1], home_position[2], home_position[3], home_position[4], home_position[5]); delay(1000); + Braccio.setAngularVelocity(45.0f); /* 45 deg/sec */ } void loop() { if (move_joint) { - Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f).in(1s); - delay(1000); - Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f).in(1s); - delay(1000); + Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f); + delay(2000); + Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f); + delay(2000); } } diff --git a/examples/Tools/Braccio_LearnByDoing/Braccio_LearnByDoing.ino b/examples/Tools/Braccio_LearnByDoing/Braccio_LearnByDoing.ino deleted file mode 100644 index 3cc189d..0000000 --- a/examples/Tools/Braccio_LearnByDoing/Braccio_LearnByDoing.ino +++ /dev/null @@ -1,158 +0,0 @@ -/* - * @brief Learn the arm an movement and replay it. - */ - -/************************************************************************************** - * INCLUDE - **************************************************************************************/ - -#include -#include "FlashIAPBlockDevice.h" -#include "FATFileSystem.h" - -/************************************************************************************** - * VARIABLES - **************************************************************************************/ - -enum states { - LEARN, - REPEAT, - IDLE -}; - -int state = IDLE; - -float values[10000]; -float* idx = values; -float* final_idx = 0; - -/************************************************************************************** - * FUNCTIONS - **************************************************************************************/ - -static void event_handler(lv_event_t * e) { - lv_event_code_t code = lv_event_get_code(e); - lv_obj_t * obj = lv_event_get_target(e); - - if (code == LV_EVENT_KEY && lv_indev_get_key(lv_indev_get_act()) == LV_KEY_HOME) { - state = IDLE; - return; - } - - if (code == LV_EVENT_CLICKED) { - uint32_t id = lv_btnmatrix_get_selected_btn(obj); - const char * txt = lv_btnmatrix_get_btn_text(obj, id); - - if (state == LEARN) { - final_idx = idx; - } - - idx = values; - - FILE* f; - switch (id) { - case 0: - state = LEARN; - Braccio.disengage(); - Serial.println("LEARN"); - break; - case 1: - state = REPEAT; - Braccio.engage(); - Serial.println("REPEAT"); - break; - case 2: - state = IDLE; - f = fopen("/fs/movements", "wb"); - fwrite(values, 1, (final_idx - values) * sizeof(float), f); - fflush(f); - fclose(f); - Serial.print("SAVED "); - Serial.print((final_idx - values) * sizeof(float)); - Serial.println(" bytes"); - break; - case 3: - state = IDLE; - f = fopen("/fs/movements", "rb"); - final_idx = values + fread(values, 1, sizeof(values), f) / sizeof(float); - fclose(f); - Serial.print("LOADED "); - Serial.print((final_idx - values) * sizeof(float)); - Serial.println(" bytes"); - break; - default: - state = IDLE; - Serial.println("IDLE"); - break; - } - } -} - -static lv_obj_t *counter; - -static const char * btnm_map[] = { "Learn", "\n", "Replay", "\n", "Save in flash", "\n", "Load from flash", "\n", "Idle", "\n", "\0" }; - -void customMenu() { - lv_obj_t * btnm1 = lv_btnmatrix_create(lv_scr_act()); - lv_btnmatrix_set_map(btnm1, btnm_map); - lv_btnmatrix_set_btn_ctrl(btnm1, 0, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm1, 1, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm1, 2, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm1, 3, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_btnmatrix_set_btn_ctrl(btnm1, 4, LV_BTNMATRIX_CTRL_CHECKABLE); - lv_obj_align(btnm1, LV_ALIGN_CENTER, 0, 0); - counter = lv_label_create(btnm1); - lv_label_set_text_fmt(counter, "%d" , 0); - lv_obj_align(counter, LV_ALIGN_CENTER, 0, 50); - lv_obj_add_event_cb(btnm1, event_handler, LV_EVENT_ALL, NULL); - Braccio.connectJoystickTo(btnm1); -} - -static FlashIAPBlockDevice bd(XIP_BASE + 0x100000, 0x100000); -static mbed::FATFileSystem fs("fs"); - -/************************************************************************************** - * SETUP/LOOP - **************************************************************************************/ - -void setup() -{ - Serial.begin(115200); - for (auto const start = millis(); !Serial && ((millis() - start) < 5000); delay(10)) { } - - // Mount file system for load/store movements - int err = fs.mount(&bd); - if (err) { - err = fs.reformat(&bd); - } - - if (!Braccio.begin(customMenu)) { - if (Serial) Serial.println("Braccio.begin() failed."); - for(;;) { } - } - Serial.println("Replicate a movement"); -} - -void loop() -{ - if (state == LEARN) { - Braccio.positions(idx); - idx += 6; - } - if (state == REPEAT) { - Braccio.moveTo(idx[0], idx[1], idx[2], idx[3], idx[4], idx[5]); - idx += 6; - if (idx >= final_idx) { - Serial.println("Repeat done"); - state = IDLE; - } - } - if (idx - values >= sizeof(values)) { - Serial.println("IDLE"); - state = IDLE; - } - delay(100); - if (state != IDLE) { - lv_label_set_text_fmt(counter, "%d" , idx - values); - } -} diff --git a/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino b/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino index f833ffd..64ff66b 100644 --- a/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino +++ b/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino @@ -85,7 +85,7 @@ bool set_initial_servo_position(int const id, float const target_angle) for ( float current_angle = Braccio.get(id).position(); !isTargetAngleReached(EPSILON) && !isTimeout(start);) { - Braccio.get(id).move().to(target_angle).in(200ms); + Braccio.get(id).move().to(target_angle); delay(250); char msg[64] = {0}; diff --git a/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino b/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino index bba4fd5..cb97616 100644 --- a/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino +++ b/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino @@ -31,13 +31,13 @@ void test_motor(int const id) delay(500); Serial.print("Drive to start . "); - Braccio.get(id).move().to(0.0f).in(1s); + Braccio.get(id).move().to(0.0f); Serial.println("OK."); delay(1500); for (float target_angle = 0.0f; target_angle < SmartServoClass::MAX_ANGLE; target_angle += 1.0f) { - Braccio.get(id).move().to(target_angle).in(200ms); + Braccio.get(id).move().to(target_angle); delay(250); char msg[64] = {0}; diff --git a/library.properties b/library.properties index b33976a..52628ed 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Arduino_Braccio_plusplus -version=1.3.2 +version=1.3.3 author=Arduino maintainer=Arduino sentence=Board support library for the Arduino Braccio++ 6-DOF robot arm. diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index a0ddbfc..82c3776 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -136,7 +136,8 @@ bool BraccioClass::begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_ lvgl_defaultMenu(); _servos.begin(); - _servos.setTime(SLOW); + _servos.setMaxTorque(SmartServoClass::TORQUE_MAX); + _servos.setAngularVelocity(SmartServoClass::DEFAULT_ANGULAR_VELOCITY_deg_per_sec); _servos.setPositionMode(PositionMode::IMMEDIATE); _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc)); diff --git a/src/Braccio++.h b/src/Braccio++.h index 5ba6db9..565b9ab 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -38,16 +38,6 @@ #include using namespace std::chrono; -/************************************************************************************** - * TYPEDEF - **************************************************************************************/ - -enum speed_grade_t { - FAST = 10, - MEDIUM = 100, - SLOW = 1000, -}; - /************************************************************************************** * FORWARD DECLARATION **************************************************************************************/ @@ -80,8 +70,11 @@ class BraccioClass void positions(float * buffer); void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6); - inline void speed(speed_grade_t const speed_grade) { _servos.setTime(speed_grade); } - inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); } + inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(max_torque); } + inline void setMaxTorque(int const id, uint16_t const max_torque) { _servos.setMaxTorque(id, max_torque); } + + inline void setAngularVelocity(float const angular_velocity_deg_per_sec) { _servos.setAngularVelocity(angular_velocity_deg_per_sec); } + inline float getAngularVelocity() const { return _servos.getAngularVelocity(); } inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); } inline void engage (int const id = SmartServoClass::BROADCAST) { _servos.engage(id); } @@ -184,10 +177,10 @@ class Servo inline void disengage() { _servos.disengage(_id); } inline void engage() { _servos.engage(_id); } inline bool engaged() { return _servos.isEngaged(_id); } + inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(_id, max_torque); } - inline Servo & move() { return *this; } - inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; } - inline Servo & in (std::chrono::milliseconds const len) { _servos.setTime(_id, len.count()); return *this; } + inline Servo & move() { return *this; } + inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; } inline float position() { return _servos.getPosition(_id); } inline bool connected() { return Braccio.connected(_id); } diff --git a/src/lib/motors/RS485.cpp b/src/lib/motors/RS485.cpp index efa610f..484730a 100644 --- a/src/lib/motors/RS485.cpp +++ b/src/lib/motors/RS485.cpp @@ -51,15 +51,11 @@ void RS485Class::begin(unsigned long baudrate, uint16_t config, int predelay, in _predelay = predelay; _postdelay = postdelay; - if (_dePin > -1) { - pinMode(_dePin, OUTPUT); - digitalWrite(_dePin, LOW); - } + pinMode(_dePin, OUTPUT); + digitalWrite(_dePin, LOW); - if (_rePin > -1) { - pinMode(_rePin, OUTPUT); - digitalWrite(_rePin, HIGH); - } + pinMode(_rePin, OUTPUT); + digitalWrite(_rePin, HIGH); _transmisionBegun = false; @@ -70,15 +66,11 @@ void RS485Class::end() { _serial->end(); - if (_rePin > -1) { - digitalWrite(_rePin, LOW); - pinMode(_dePin, INPUT); - } + digitalWrite(_rePin, LOW); + pinMode(_dePin, INPUT); - if (_dePin > -1) { - digitalWrite(_dePin, LOW); - pinMode(_rePin, INPUT); - } + digitalWrite(_dePin, LOW); + pinMode(_rePin, INPUT); } int RS485Class::available() @@ -118,10 +110,8 @@ RS485Class::operator bool() void RS485Class::beginTransmission() { - if (_dePin > -1) { - digitalWrite(_dePin, HIGH); - if (_predelay) delayMicroseconds(_predelay); - } + digitalWrite(_dePin, HIGH); + if (_predelay) delayMicroseconds(_predelay); _transmisionBegun = true; } @@ -130,26 +120,20 @@ void RS485Class::endTransmission() { _serial->flush(); - if (_dePin > -1) { - if (_postdelay) delayMicroseconds(_postdelay); - digitalWrite(_dePin, LOW); - } + if (_postdelay) delayMicroseconds(_postdelay); + digitalWrite(_dePin, LOW); _transmisionBegun = false; } void RS485Class::receive() { - if (_rePin > -1) { - digitalWrite(_rePin, LOW); - } + digitalWrite(_rePin, LOW); } void RS485Class::noReceive() { - if (_rePin > -1) { - digitalWrite(_rePin, HIGH); - } + digitalWrite(_rePin, HIGH); } void RS485Class::sendBreak(unsigned int duration) @@ -171,10 +155,3 @@ void RS485Class::sendBreakMicroseconds(unsigned int duration) delayMicroseconds(duration); _serial->begin(_baudrate, _config); } - -void RS485Class::setPins(int txPin, int dePin, int rePin) -{ - _txPin = txPin; - _dePin = dePin; - _rePin = rePin; -} \ No newline at end of file diff --git a/src/lib/motors/RS485.h b/src/lib/motors/RS485.h index d7476c4..38b0a05 100644 --- a/src/lib/motors/RS485.h +++ b/src/lib/motors/RS485.h @@ -66,8 +66,6 @@ class RS485Class : public Stream { void sendBreak(unsigned int duration); void sendBreakMicroseconds(unsigned int duration); - void setPins(int txPin, int dePin, int rePin); - private: HardwareSerial* _serial; int _txPin; diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 397e960..1d5f031 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -31,6 +31,7 @@ SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} , _errors{0} +, _angular_velocity_deg_per_sec{DEFAULT_ANGULAR_VELOCITY_deg_per_sec} , _onError{} , _mtx{} { @@ -62,7 +63,7 @@ void SmartServoClass::sendPacket() _RS485.endTransmission(); // should now receive an echo of what we just sent while (_RS485.available() < len) { - delay(100); + delayMicroseconds(10); } // discard the echo int i = len; @@ -198,27 +199,54 @@ void SmartServoClass::begin() _targetSpeed[idToArrayIndex(i)] = 1000; } -void SmartServoClass::setPosition(uint8_t const id, float const angle) +void SmartServoClass::setPosition(uint8_t const id, float const angle_deg) { - if (!isValidAngle(angle)) + if (!isValidAngle(angle_deg)) return; - mbed::ScopedLock lock(_mtx); - if (isValidId(id)) + if (!isValidId(id)) + return; + + float const target_position_deg = angle_deg; + float const actual_position_deg = getPosition(id); + if (actual_position_deg < 0.0f) + return; + + float const abs_position_diff_deg = fabs(target_position_deg - actual_position_deg); + float const limited_runtime_sec = abs_position_diff_deg / _angular_velocity_deg_per_sec; + uint16_t const limited_runtime_ms = static_cast(limited_runtime_sec * 1000.f); + + if (_positionMode == PositionMode::IMMEDIATE) { - _targetPosition[idToArrayIndex(id)] = angleToPosition(angle); - if (_positionMode==PositionMode::IMMEDIATE) { - writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); - } - } + mbed::ScopedLock lock(_mtx); + writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), limited_runtime_ms); + writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(target_position_deg)); + } + else if (_positionMode == PositionMode::SYNC) + { + _targetSpeed[idToArrayIndex(id)] = limited_runtime_ms; + _targetPosition[idToArrayIndex(id)] = angleToPosition(target_position_deg); + } } float SmartServoClass::getPosition(uint8_t const id) { + if (!isValidId(id)) + return -1.0f; + mbed::ScopedLock lock(_mtx); - float ret = -1; - if (isValidId(id)) - return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H))); + int position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); + + // retry one more time + if (position < 0) { + delay(1); + position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); + } + + if (position < 0) + return -1.0f; + + return positionToAngle(position); } void SmartServoClass::center(uint8_t const id, uint16_t const position) @@ -261,29 +289,41 @@ void SmartServoClass::setTorque(uint8_t const id, bool const torque) void SmartServoClass::setTime(uint16_t const time) { - mbed::ScopedLock lock(_mtx); for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) _targetSpeed[idToArrayIndex(i)] = time; + + mbed::ScopedLock lock(_mtx); writeWordCmd(BROADCAST, REG(SmartServoRegister::RUN_TIME_H), time); } void SmartServoClass::setTime(uint8_t const id, uint16_t const time) { + if (!isValidId(id)) + return; + + if (id == BROADCAST) + return; + + _targetSpeed[idToArrayIndex(id)] = time; + mbed::ScopedLock lock(_mtx); - if ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)) _targetSpeed[idToArrayIndex(id)] = time; writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time); } -void SmartServoClass::setMaxTorque(uint16_t const torque) +uint16_t SmartServoClass::getTime(uint8_t const id) { mbed::ScopedLock lock(_mtx); - writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_TORQUE_H), torque); + return readWordCmd(id, REG(SmartServoRegister::RUN_TIME_H)); } -void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) +void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const max_torque) { mbed::ScopedLock lock(_mtx); - writeWordCmd(id+1, REG(SmartServoRegister::MAX_TORQUE_H), torque); + + if (max_torque > TORQUE_MAX) + writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), TORQUE_MAX); + else + writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), max_torque); } void SmartServoClass::setID(uint8_t const id) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 46d6927..d7186b6 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -46,19 +46,19 @@ class SmartServoClass inline void setPositionMode(PositionMode const mode) { _positionMode = mode; } - void setPosition(uint8_t const id, float const angle); - + void setPosition(uint8_t const id, float const angle_deg); float getPosition(uint8_t const id); + inline void setAngularVelocity(float const angular_velocity_deg_per_sec) { _angular_velocity_deg_per_sec = angular_velocity_deg_per_sec; } + inline float getAngularVelocity() const { return _angular_velocity_deg_per_sec; } + void synchronize(); void setTorque(bool const torque); - void setTorque(uint8_t const id, bool const torque); - void setMaxTorque(uint16_t const torque); - - void setMaxTorque(uint8_t const id, uint16_t const torque); + void setMaxTorque(uint8_t const id, uint16_t const max_torque); + inline void setMaxTorque(uint16_t const max_torque) { setMaxTorque(BROADCAST, max_torque); } void setID(uint8_t const id); @@ -75,8 +75,9 @@ class SmartServoClass void setMaxAngle(uint16_t const max_angle); void setMaxAngle(uint8_t const id, uint16_t const max_angle); - void setTime(uint16_t const time); - void setTime(uint8_t const id, uint16_t const time); + void setTime(uint16_t const time); + void setTime(uint8_t const id, uint16_t const time); + uint16_t getTime(uint8_t const id); void center(uint8_t const id, uint16_t const position); @@ -97,6 +98,10 @@ class SmartServoClass static int constexpr MAX_MOTOR_ID = 6; static int constexpr NUM_MOTORS = 6; static float constexpr MAX_ANGLE = 315.0f; + static float constexpr DEFAULT_ANGULAR_VELOCITY_deg_per_sec = 20.0f; + + static uint16_t constexpr TORQUE_MIN = 0; + static uint16_t constexpr TORQUE_MAX = 1000; static size_t idToArrayIndex(size_t const id) { return (id - 1); } @@ -126,6 +131,7 @@ class SmartServoClass RS485Class& _RS485; int _errors; + float _angular_velocity_deg_per_sec; mbed::Callback _onError; rtos::Mutex _mtx;