From c04d6569f0512ddfc4105fa5e8725048d655e236 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Wed, 19 Jun 2019 14:01:16 +0200 Subject: [PATCH 01/37] added polar coordinates protocol function --- code/headers/uarm_gcode_generator.hpp | 23 +++++++++++++++++++++-- code/headers/uarm_swift_pro.hpp | 12 +++++++----- code/src/uarm_swift_pro.cpp | 11 +++++++---- 3 files changed, 35 insertions(+), 11 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index ea1996d..ba6c947 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -10,8 +10,7 @@ namespace r2d2::robot_arm { * @param vector3i * @param uint8_t speed * */ - void coordinate_to_gcode(const vector3i_c &coordinate, - const uint16_t &speed) { + void coordinate_to_gcode(const vector3i_c &coordinate, const uint16_t &speed) { char x_string[11]; // max number of int digits (10) + '\0' = 11 char y_string[11]; char z_string[11]; @@ -30,5 +29,25 @@ namespace r2d2::robot_arm { this->append(speed_string); this->append("\n"); } + + void polar_coordinate_to_gcode(const vector3i_c &coordinate, const uint16_t &speed){ + char x_string[11]; // max number of int digits (10) + '\0' = 11 + char y_string[11]; + char z_string[11]; + char speed_string[11]; + this->int_to_string(coordinate.x, x_string); + this->int_to_string(coordinate.y, y_string); + this->int_to_string(coordinate.z, z_string); + this->int_to_string(speed, speed_string); + this->append("#n G2201 S"); + this->append(x_string); + this->append(" R"); + this->append(y_string); + this->append(" H"); + this->append(z_string); + this->append(" F"); + this->append(speed_string); + this->append("\n"); + } }; } // namespace r2d2::robot_arm \ No newline at end of file diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index d52b358..06eb445 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -12,14 +12,14 @@ */ namespace r2d2::robot_arm { class uarm_swift_pro_c : public robot_arm_interface_c { - protected: - const uint16_t default_speed = 1000; - private: using usart_c = r2d2::usart::usart_connection_c; uarm_gcode_generator_c<50> gcode_generator; usart_c &usart_bus; + protected: + const uint16_t default_speed = 1000; + public: /** * Robot arm constructor, needs an usart bus for communication @@ -64,14 +64,16 @@ namespace r2d2::robot_arm { * @param coordinate 3D integer coordinate for the head * @param speed movement speed in mm/min */ - void move_head_to_coordinate(const vector3i_c &coordinate, - const uint16_t &speed) override; + void move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) override; /** * This function moves the uArm swift pro head to a certain 3d location. * * @param coordinate */ void move_head_to_coordinate(const vector3i_c &coordinate) override; + + void move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed); + /** * This function rotates the head of the 4dof diy robot arm. * diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index defd614..2fb88a5 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -5,18 +5,21 @@ namespace r2d2::robot_arm { : usart_bus(usart_bus) { } - void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, - const uint16_t &speed) { + void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) { gcode_generator.coordinate_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); } - void - uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate) { + void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate) { gcode_generator.coordinate_to_gcode(coordinate, default_speed); this->send_command(gcode_generator.get_buffer()); } + void uarm_swift_pro_c::move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed){ + gcode_generator.coordinate_to_gcode(coordinate, speed); + this->send_command(gcode_generator.get_buffer()); + } + void uarm_swift_pro_c::rotate_head(const int16_t &rotation) { // TODO impl rotate head uarm swift pro } From b5a1c257dd249b669fea56f4662b2c18a4c9fd54 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Mon, 24 Jun 2019 15:22:14 +0200 Subject: [PATCH 02/37] add debug method --- code/headers/uarm_gcode_generator.hpp | 30 +++++++++++++++++++-------- code/headers/uarm_swift_pro.hpp | 13 +++++++++++- code/main.cpp | 8 +++++-- code/src/uarm_swift_pro.cpp | 23 +++++++++++++++----- 4 files changed, 57 insertions(+), 17 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index ba6c947..a9bd397 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -31,23 +31,35 @@ namespace r2d2::robot_arm { } void polar_coordinate_to_gcode(const vector3i_c &coordinate, const uint16_t &speed){ - char x_string[11]; // max number of int digits (10) + '\0' = 11 - char y_string[11]; - char z_string[11]; + char s_string[11]; // max number of int digits (10) + '\0' = 11 + char r_string[11]; + char h_string[11]; char speed_string[11]; - this->int_to_string(coordinate.x, x_string); - this->int_to_string(coordinate.y, y_string); - this->int_to_string(coordinate.z, z_string); + this->int_to_string(coordinate.x, s_string); + this->int_to_string(coordinate.y, r_string); + this->int_to_string(coordinate.z, h_string); this->int_to_string(speed, speed_string); this->append("#n G2201 S"); - this->append(x_string); + this->append(s_string); this->append(" R"); - this->append(y_string); + this->append(r_string); this->append(" H"); - this->append(z_string); + this->append(h_string); this->append(" F"); this->append(speed_string); this->append("\n"); } + + void rotate_id_motor_degree_to_gcode(const uint8_t &id, const uint16_t °ree){ + char id_string[2]; + char degree_string[4]; + this->int_to_string(id, id_string); + this->int_to_string(degree, degree_string); + this->append("G2202 N"); + this->append(id_string); + this->append(" V"); + this->append(degree_string); + this->append("\n"); + } }; } // namespace r2d2::robot_arm \ No newline at end of file diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 06eb445..8b206cd 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -72,8 +72,19 @@ namespace r2d2::robot_arm { */ void move_head_to_coordinate(const vector3i_c &coordinate) override; + /** + * This function moves the uArm swift pro head to a certain polar coordination. + * + * @param coordinate polar coordinates, S is stretch(mm), R is rotation(degree),H is height(mm) + * @param speed movement speed in mm/min + */ + void move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed); - + + void rotate_id_motor_to_degree(const uint8_t &id, const uint16_t °ree); + + void debug(); + /** * This function rotates the head of the 4dof diy robot arm. * diff --git a/code/main.cpp b/code/main.cpp index 5404e95..04885e2 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -9,10 +9,14 @@ int main() { auto usart = r2d2::usart::hardware_usart_c(115200); r2d2::robot_arm::uarm_swift_pro_c uarm(usart); + + // driver code uarm.init(); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 100, 100)); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(25, 175, 100),500); + //uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 100, 100)); + //uarm.rotate_id_motor_to_degree(0,60); + //hwlib::wait_ms(20000); + //uarm.move_head_to_polar_coordinate(r2d2::robot_arm::vector3i_c(200, 200, 200),1000); // You can add an extra parameter after the vector for speed if you'd like // i.e. uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i(100, 100, // 100), 500); diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 2fb88a5..fb94539 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -8,16 +8,25 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) { gcode_generator.coordinate_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); + this->debug(); } void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate) { gcode_generator.coordinate_to_gcode(coordinate, default_speed); this->send_command(gcode_generator.get_buffer()); + this->debug(); } void uarm_swift_pro_c::move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed){ - gcode_generator.coordinate_to_gcode(coordinate, speed); + gcode_generator.polar_coordinate_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::rotate_id_motor_to_degree(const uint8_t &id, const uint16_t °ree){ + gcode_generator.rotate_id_motor_degree_to_gcode(id, degree); + this->send_command(gcode_generator.get_buffer()); + this->debug(); } void uarm_swift_pro_c::rotate_head(const int16_t &rotation) { @@ -25,12 +34,16 @@ namespace r2d2::robot_arm { } void uarm_swift_pro_c::init() { - this->send_command("#n M2122 V1\n"); // report when stop + this->send_command("#1 M2122 V1\n"); // report when stop + this->debug(); } - bool uarm_swift_pro_c::check_connection() { - // TODO impl check_connection() - return true; + void uarm_swift_pro_c::debug(){ + char debug = '0'; + while ( debug != '\n'){ + debug = this->usart_bus.getc(); + hwlib::cout << debug; + } } bool uarm_swift_pro_c::send_command(const char *command) { From ff8115bd24af62ecf101ffa847577e09b6b42bc9 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Mon, 24 Jun 2019 15:42:17 +0200 Subject: [PATCH 03/37] added by method init a parameter for turning on/off the motors --- code/headers/uarm_gcode_generator.hpp | 8 ++++++++ code/headers/uarm_swift_pro.hpp | 2 ++ code/main.cpp | 2 +- code/src/uarm_swift_pro.cpp | 5 +++-- 4 files changed, 14 insertions(+), 3 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index a9bd397..bb46a93 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -61,5 +61,13 @@ namespace r2d2::robot_arm { this->append(degree_string); this->append("\n"); } + + void init(const uint8_t &on_off){ + char on_off_string[2]; + this->int_to_string(on_off, on_off_string); + this->append("#1 M2122 V"); + this->append(on_off_string); + this->append("\n"); + } }; } // namespace r2d2::robot_arm \ No newline at end of file diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 8b206cd..fdcb0c9 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -85,6 +85,8 @@ namespace r2d2::robot_arm { void debug(); + void init(const uint8_t &on_off); + /** * This function rotates the head of the 4dof diy robot arm. * diff --git a/code/main.cpp b/code/main.cpp index 04885e2..b32b996 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -12,7 +12,7 @@ int main() { // driver code - uarm.init(); + uarm.init(1); //uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 100, 100)); //uarm.rotate_id_motor_to_degree(0,60); //hwlib::wait_ms(20000); diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index fb94539..633770f 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -33,8 +33,9 @@ namespace r2d2::robot_arm { // TODO impl rotate head uarm swift pro } - void uarm_swift_pro_c::init() { - this->send_command("#1 M2122 V1\n"); // report when stop + void uarm_swift_pro_c::init(const uint8_t &on_off) { + gcode_generator.init(on_off); + this->send_command(gcode_generator.get_buffer()); // report when stop this->debug(); } From 7574c41dcca6f0cd73876ec2f5afbc0510dd9793 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Mon, 24 Jun 2019 16:22:58 +0200 Subject: [PATCH 04/37] cleaned up: methods that were not implemented and added commands --- code/headers/dof4_diy.hpp | 6 ------ code/headers/robot_arm_interface.hpp | 7 ------- code/headers/uarm_swift_pro.hpp | 16 +++------------- code/src/dof4_diy.cpp | 3 --- code/src/uarm_swift_pro.cpp | 19 +++++++++---------- 5 files changed, 12 insertions(+), 39 deletions(-) diff --git a/code/headers/dof4_diy.hpp b/code/headers/dof4_diy.hpp index f41b5bf..d2e9ec7 100644 --- a/code/headers/dof4_diy.hpp +++ b/code/headers/dof4_diy.hpp @@ -27,11 +27,5 @@ namespace r2d2::robot_arm { */ void move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) override; - /** - * This function rotates the head of the 4dof diy robot arm. - * - * @param rotation - */ - void rotate_head(const int16_t &rotation) override; }; } // namespace r2d2::robot_arm diff --git a/code/headers/robot_arm_interface.hpp b/code/headers/robot_arm_interface.hpp index 4b4e54a..6189ed8 100644 --- a/code/headers/robot_arm_interface.hpp +++ b/code/headers/robot_arm_interface.hpp @@ -26,12 +26,5 @@ namespace r2d2::robot_arm { * @param coordinate */ virtual void move_head_to_coordinate(const vector3i_c &coordinate) = 0; - - /** - * This function rotates the head of the robot arm. - * - * @param rotation - */ - virtual void rotate_head(const int16_t &rotation) = 0; }; }; // namespace r2d2::robot_arm \ No newline at end of file diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index fdcb0c9..ebde178 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -33,6 +33,7 @@ namespace r2d2::robot_arm { * */ void init(); + /** * @brief Check connection * Function to check whether the connection with the arm is right or not @@ -41,6 +42,7 @@ namespace r2d2::robot_arm { * @return false means connection is wrong */ bool check_connection(); + /** * @brief Sending command to arm * Sends false as default @@ -50,12 +52,7 @@ namespace r2d2::robot_arm { * @return true when command correctly sent */ bool send_command(const char *command); - /** - * Move specific joint of the arm - * Moves the robot arm joint [joint_id] in a given angle - * - */ - void move_joint(const int &joint_id, const int &angle); + /** * Move arm's head to 3D coordinate * Moves head of robot arm head/end effector towards the 3D coordinate @@ -86,12 +83,5 @@ namespace r2d2::robot_arm { void debug(); void init(const uint8_t &on_off); - - /** - * This function rotates the head of the 4dof diy robot arm. - * - * @param rotation - */ - void rotate_head(const int16_t &rotation) override; }; } // namespace r2d2::robot_arm diff --git a/code/src/dof4_diy.cpp b/code/src/dof4_diy.cpp index 1ddad33..ddbc48d 100644 --- a/code/src/dof4_diy.cpp +++ b/code/src/dof4_diy.cpp @@ -8,7 +8,4 @@ namespace r2d2::robot_arm { const uint16_t &speed) { // TODO impl 4dof diy move head + speed } - void dof4_diy_c::rotate_head(const int16_t &rotation) { - // TODO impl rotate head 4dof diy - } } // namespace r2d2::robot_arm \ No newline at end of file diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 633770f..360c7b8 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -4,8 +4,10 @@ namespace r2d2::robot_arm { uarm_swift_pro_c::uarm_swift_pro_c(usart_c &usart_bus) : usart_bus(usart_bus) { } - - void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) { + + //------------------MOVING COMMANDS------------------// + void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, + const uint16_t &speed) { gcode_generator.coordinate_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); this->debug(); @@ -17,7 +19,8 @@ namespace r2d2::robot_arm { this->debug(); } - void uarm_swift_pro_c::move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed){ + void uarm_swift_pro_c::move_head_to_polar_coordinate(const vector3i_c &coordinate, + const uint16_t &speed){ gcode_generator.polar_coordinate_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); this->debug(); @@ -29,9 +32,7 @@ namespace r2d2::robot_arm { this->debug(); } - void uarm_swift_pro_c::rotate_head(const int16_t &rotation) { - // TODO impl rotate head uarm swift pro - } + //------------------SETTING COMMANDS------------------// void uarm_swift_pro_c::init(const uint8_t &on_off) { gcode_generator.init(on_off); @@ -39,6 +40,8 @@ namespace r2d2::robot_arm { this->debug(); } + //------------------ESSENTIAL METHODS------------------// + void uarm_swift_pro_c::debug(){ char debug = '0'; while ( debug != '\n'){ @@ -52,8 +55,4 @@ namespace r2d2::robot_arm { return true; } - void uarm_swift_pro_c::move_joint(const int &joint_id, const int &angle) { - // TODO impl uarm_swift_c::move_joint - } - } // namespace r2d2::robot_arm From 14aa36793fa460504e7603bb7a1d74029028ca85 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Wed, 26 Jun 2019 23:18:26 +0200 Subject: [PATCH 05/37] edit test environment for mock_bus --- test/Makefile | 4 ++-- test/main.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/test/Makefile b/test/Makefile index 1b6f2af..3c3253f 100644 --- a/test/Makefile +++ b/test/Makefile @@ -9,10 +9,10 @@ ############################################################################# # source files in this project (main.cpp is automatically assumed) -SOURCES := +SOURCES := $(wildcard ../code/src/*.cpp) # header files in this project -HEADERS := ../code/headers/uarm_gcode_generator.hpp ../code/headers/vector3.hpp +HEADERS := $(wildcard ../code/headers/*.hpp) # other places to look for files for this project SEARCH := ../code/headers ../code/src diff --git a/test/main.cpp b/test/main.cpp index 0e80f2f..7b42473 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -1,6 +1,7 @@ #include #define CATCH_CONFIG_MAIN #include +#include TEST_CASE("Appending gone wrong!", "Testing generator") { r2d2::robot_arm::uarm_gcode_generator_c<2> generator; From a0e8980bba749671ee651690d2f8989d0b2560c4 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Wed, 26 Jun 2019 23:20:15 +0200 Subject: [PATCH 06/37] implemented canbus --- Makefile.link | 5 +++++ code/headers/robot_arm_interface.hpp | 26 +++++++++++++++++++++++++- code/headers/uarm_swift_pro.hpp | 2 +- code/main.cpp | 8 +++++++- code/src/uarm_swift_pro.cpp | 5 +++-- 5 files changed, 41 insertions(+), 5 deletions(-) diff --git a/Makefile.link b/Makefile.link index 2ac71c0..9a9478c 100644 --- a/Makefile.link +++ b/Makefile.link @@ -18,6 +18,11 @@ include $(internal_comm)/Makefile.inc DATA ?= ../../../libraries/datastructures/code/headers/queue.hpp +#add USART_library +USART ?= ../../../libraries/USART_library +include $(USART)/Makefile.inc + + # add hwlib HWLIB ?= ../../../libraries/hwlib include $(HWLIB)/Makefile.inc diff --git a/code/headers/robot_arm_interface.hpp b/code/headers/robot_arm_interface.hpp index 6189ed8..75f3589 100644 --- a/code/headers/robot_arm_interface.hpp +++ b/code/headers/robot_arm_interface.hpp @@ -2,14 +2,38 @@ #include "vector3.hpp" #include +#include /** * Class robot_arm_interface provides the interface for different robot arms. */ namespace r2d2::robot_arm { - class robot_arm_interface_c { + class robot_arm_interface_c: public base_module_c { public: + + robot_arm_interface_c(base_comm_c &comm) : base_module_c(comm){ + + comm.listen_for_frames({r2d2::frame_type::ROBOT_ARM}); + } + + void process()override{ + comm.request(r2d2::frame_type::ROBOT_ARM); + while (comm.has_data()){ + auto frame = comm.get_data(); + // Process the frame + + // Don't handle requests + if (frame.request) { + continue; + } + + const auto data = frame.as_frame_type(); + + move_head_to_coordinate(vector3i_c(data.x, data.y, data.z), data.speed); + + } + } /** * This function moves the robot arm head to a certain 3d location * at a given speed. diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index ebde178..818d8e4 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -24,7 +24,7 @@ namespace r2d2::robot_arm { /** * Robot arm constructor, needs an usart bus for communication */ - uarm_swift_pro_c(usart_c &usart_bus); + uarm_swift_pro_c(usart_c &usart_bus, base_comm_c &comm); /** * @brief Initialization function diff --git a/code/main.cpp b/code/main.cpp index b32b996..b0d16a0 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -1,14 +1,20 @@ #include #include #include +#include #include +#include int main() { WDT->WDT_MR = WDT_MR_WDDIS; hwlib::wait_ms(1000); auto usart = r2d2::usart::hardware_usart_c(115200); - r2d2::robot_arm::uarm_swift_pro_c uarm(usart); + + r2d2::comm_c comm; + + r2d2::robot_arm::uarm_swift_pro_c uarm(usart, comm); + // driver code diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 360c7b8..43fcbc8 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -1,8 +1,9 @@ #include namespace r2d2::robot_arm { - uarm_swift_pro_c::uarm_swift_pro_c(usart_c &usart_bus) - : usart_bus(usart_bus) { + uarm_swift_pro_c::uarm_swift_pro_c(usart_c &usart_bus,base_comm_c &comm): + robot_arm_interface_c(comm), + usart_bus(usart_bus) { } //------------------MOVING COMMANDS------------------// From e1b7e2dc78f76fceb739deee8937ec72808172a4 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Wed, 26 Jun 2019 23:32:36 +0200 Subject: [PATCH 07/37] add debug order and add relative displacement to gcode (coordinate/polar) --- code/headers/uarm_gcode_generator.hpp | 57 +++++++++++++++++++++++---- 1 file changed, 49 insertions(+), 8 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index bb46a93..3db9e92 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -4,6 +4,15 @@ namespace r2d2::robot_arm { template class uarm_gcode_generator_c : public gcode_generator_c { public: + + void init(const uint8_t &on_off){ + char on_off_string[2]; + this->int_to_string(on_off, on_off_string); + this->append("#1 M2122 V"); + this->append(on_off_string); + this->append("\n"); + } + /** * Converts a vector3i_c to a gcode command for uArm * @@ -19,7 +28,7 @@ namespace r2d2::robot_arm { this->int_to_string(coordinate.y, y_string); this->int_to_string(coordinate.z, z_string); this->int_to_string(speed, speed_string); - this->append("#n G0 X"); + this->append("#2 G0 X"); this->append(x_string); this->append(" Y"); this->append(y_string); @@ -39,7 +48,7 @@ namespace r2d2::robot_arm { this->int_to_string(coordinate.y, r_string); this->int_to_string(coordinate.z, h_string); this->int_to_string(speed, speed_string); - this->append("#n G2201 S"); + this->append("#3 G2201 S"); this->append(s_string); this->append(" R"); this->append(r_string); @@ -55,18 +64,50 @@ namespace r2d2::robot_arm { char degree_string[4]; this->int_to_string(id, id_string); this->int_to_string(degree, degree_string); - this->append("G2202 N"); + this->append("#4 G2202 N"); this->append(id_string); this->append(" V"); this->append(degree_string); this->append("\n"); } - void init(const uint8_t &on_off){ - char on_off_string[2]; - this->int_to_string(on_off, on_off_string); - this->append("#1 M2122 V"); - this->append(on_off_string); + void relative_displacement_to_gcode(const vector3i_c &coordinate, const uint16_t &speed) { + char x_string[11]; // max number of int digits (10) + '\0' = 11 + char y_string[11]; + char z_string[11]; + char speed_string[11]; + this->int_to_string(coordinate.x, x_string); + this->int_to_string(coordinate.y, y_string); + this->int_to_string(coordinate.z, z_string); + this->int_to_string(speed, speed_string); + this->append("#5 G2204 X"); + this->append(x_string); + this->append(" Y"); + this->append(y_string); + this->append(" Z"); + this->append(z_string); + this->append(" F"); + this->append(speed_string); + this->append("\n"); + } + + void relative_displacement_polar_to_gcode(const vector3i_c &coordinate, const uint16_t &speed){ + char s_string[11]; // max number of int digits (10) + '\0' = 11 + char r_string[11]; + char h_string[11]; + char speed_string[11]; + this->int_to_string(coordinate.x, s_string); + this->int_to_string(coordinate.y, r_string); + this->int_to_string(coordinate.z, h_string); + this->int_to_string(speed, speed_string); + this->append("#6 G2205 S"); + this->append(s_string); + this->append(" R"); + this->append(r_string); + this->append(" H"); + this->append(h_string); + this->append(" F"); + this->append(speed_string); this->append("\n"); } }; From 1daea7e76eb845b9149634ae47c4fb25a5c242cf Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Wed, 26 Jun 2019 23:46:34 +0200 Subject: [PATCH 08/37] added the two methods (relative displacement(xyz/polar)) to uarm class --- code/headers/uarm_swift_pro.hpp | 20 +++++++++++--------- code/src/uarm_swift_pro.cpp | 14 ++++++++++++++ 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 818d8e4..33f988d 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -26,14 +26,6 @@ namespace r2d2::robot_arm { */ uarm_swift_pro_c(usart_c &usart_bus, base_comm_c &comm); - /** - * @brief Initialization function - * This function is for actions that need to be executed at - * initialization of the object - * - */ - void init(); - /** * @brief Check connection * Function to check whether the connection with the arm is right or not @@ -80,8 +72,18 @@ namespace r2d2::robot_arm { void rotate_id_motor_to_degree(const uint8_t &id, const uint16_t °ree); - void debug(); + void relative_displacement_head(const vector3i_c &coordinate, const uint16_t &speed); + + void relative_displacement_polar_head(const vector3i_c &coordinate, const uint16_t &speed); + /** + * @brief Initialization function + * This function is for actions that need to be executed at + * initialization of the object + * + */ void init(const uint8_t &on_off); + + void debug(); }; } // namespace r2d2::robot_arm diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 43fcbc8..d8d3937 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -33,6 +33,20 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::relative_displacement_head(const vector3i_c &coordinate, const uint16_t &speed){ + gcode_generator.relative_displacement_to_gcode(coordinate, speed); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::relative_displacement_polar_head(const vector3i_c &coordinate, const uint16_t &speed){ + gcode_generator.relative_displacement_polar_to_gcode(coordinate, speed); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + + //------------------SETTING COMMANDS------------------// void uarm_swift_pro_c::init(const uint8_t &on_off) { From 31fca85a6455fb210faf59e9f7a523bffe7b81f8 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 00:28:52 +0200 Subject: [PATCH 09/37] add attach all joint motors to gcode method --- code/headers/uarm_gcode_generator.hpp | 30 +++++++++++++++------------ 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 3db9e92..f53b043 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -5,14 +5,6 @@ namespace r2d2::robot_arm { class uarm_gcode_generator_c : public gcode_generator_c { public: - void init(const uint8_t &on_off){ - char on_off_string[2]; - this->int_to_string(on_off, on_off_string); - this->append("#1 M2122 V"); - this->append(on_off_string); - this->append("\n"); - } - /** * Converts a vector3i_c to a gcode command for uArm * @@ -28,7 +20,7 @@ namespace r2d2::robot_arm { this->int_to_string(coordinate.y, y_string); this->int_to_string(coordinate.z, z_string); this->int_to_string(speed, speed_string); - this->append("#2 G0 X"); + this->append("#1 G0 X"); this->append(x_string); this->append(" Y"); this->append(y_string); @@ -48,7 +40,7 @@ namespace r2d2::robot_arm { this->int_to_string(coordinate.y, r_string); this->int_to_string(coordinate.z, h_string); this->int_to_string(speed, speed_string); - this->append("#3 G2201 S"); + this->append("#2 G2201 S"); this->append(s_string); this->append(" R"); this->append(r_string); @@ -64,7 +56,7 @@ namespace r2d2::robot_arm { char degree_string[4]; this->int_to_string(id, id_string); this->int_to_string(degree, degree_string); - this->append("#4 G2202 N"); + this->append("#3 G2202 N"); this->append(id_string); this->append(" V"); this->append(degree_string); @@ -80,7 +72,7 @@ namespace r2d2::robot_arm { this->int_to_string(coordinate.y, y_string); this->int_to_string(coordinate.z, z_string); this->int_to_string(speed, speed_string); - this->append("#5 G2204 X"); + this->append("#4 G2204 X"); this->append(x_string); this->append(" Y"); this->append(y_string); @@ -100,7 +92,7 @@ namespace r2d2::robot_arm { this->int_to_string(coordinate.y, r_string); this->int_to_string(coordinate.z, h_string); this->int_to_string(speed, speed_string); - this->append("#6 G2205 S"); + this->append("#5 G2205 S"); this->append(s_string); this->append(" R"); this->append(r_string); @@ -110,5 +102,17 @@ namespace r2d2::robot_arm { this->append(speed_string); this->append("\n"); } + + void attach_all_joint_motors_to_gcode(){ + this->append("#7 M17\n"); + } + + void init(const uint8_t &on_off){ + char on_off_string[2]; + this->int_to_string(on_off, on_off_string); + this->append("#1 M2122 V"); + this->append(on_off_string); + this->append("\n"); + } }; } // namespace r2d2::robot_arm \ No newline at end of file From 9d09d422e934f694bc6b1233f3cd885c1564836a Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 00:38:28 +0200 Subject: [PATCH 10/37] add method return_cartesian_coordinates_by_speed to uarm_gcode_generator --- code/headers/uarm_gcode_generator.hpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index f53b043..971e2a9 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -104,7 +104,20 @@ namespace r2d2::robot_arm { } void attach_all_joint_motors_to_gcode(){ - this->append("#7 M17\n"); + this->append("#6 M17\n"); + + } + + void detach_all_joint_motors_to_gcode(){ + this->append("#7 M2019\n"); + } + + void return_cartesian_coordinates_by_speed(const uint16_t &speed){ + char speed_string[11]; + this->int_to_string(speed, speed_string); + this->append("#8 M2120 V"); + this->append(speed_string); + this->append("\n"); } void init(const uint8_t &on_off){ From edea0b928dc3b740bbd5f1ee533e11bb93cfc2cd Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 00:45:47 +0200 Subject: [PATCH 11/37] add attach_motor_by_id_to_gcode to uarm_gcode_generator class --- code/headers/uarm_gcode_generator.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 971e2a9..cc97fc0 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -105,7 +105,6 @@ namespace r2d2::robot_arm { void attach_all_joint_motors_to_gcode(){ this->append("#6 M17\n"); - } void detach_all_joint_motors_to_gcode(){ @@ -120,6 +119,14 @@ namespace r2d2::robot_arm { this->append("\n"); } + void attach_motor_by_id_to_gcode(const uint8_t &id){ + char id_string[2]; + this->int_to_string(id, id_string); + this->append("#9 M2201 N"); + this->append(id_string); + this->append("\n"); + } + void init(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); From 66c6ca91a67538a8c05ba9a0400277ae13e75ddb Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 00:48:17 +0200 Subject: [PATCH 12/37] add detach_motor_by_id_to_gcode to uarm_gcode_generator class --- code/headers/uarm_gcode_generator.hpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index cc97fc0..557b373 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -111,7 +111,7 @@ namespace r2d2::robot_arm { this->append("#7 M2019\n"); } - void return_cartesian_coordinates_by_speed(const uint16_t &speed){ + void return_cartesian_coordinates_by_speed_to_gcode(const uint16_t &speed){ char speed_string[11]; this->int_to_string(speed, speed_string); this->append("#8 M2120 V"); @@ -127,6 +127,14 @@ namespace r2d2::robot_arm { this->append("\n"); } + void detach_motor_by_id_to_gcode(const uint8_t &id){ + char id_string[2]; + this->int_to_string(id, id_string); + this->append("#10 M2202 N"); + this->append(id_string); + this->append("\n"); + } + void init(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); From 089c086d520fe7e5ebf43e0f13d2ee7d0f79182e Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 00:50:15 +0200 Subject: [PATCH 13/37] add check_attached_motor_by_id_to_gcode to uarm_gcode_generator class --- code/headers/uarm_gcode_generator.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 557b373..1224aa8 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -135,6 +135,14 @@ namespace r2d2::robot_arm { this->append("\n"); } + void check_attached_motor_by_id_to_gcode(const uint8_t &id){ + char id_string[2]; + this->int_to_string(id, id_string); + this->append("#11 M2203 N"); + this->append(id_string); + this->append("\n"); + } + void init(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); From 1f7b3dbf98879868f494e1b680b6de46858e4c2d Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 00:56:30 +0200 Subject: [PATCH 14/37] add set_buzzer_to_gcode to uarm_gcode_generator class --- code/headers/uarm_gcode_generator.hpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 1224aa8..6eddaea 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -143,6 +143,18 @@ namespace r2d2::robot_arm { this->append("\n"); } + void set_buzzer_to_gcode(const uint16_t &frequency, const uint16_t &time){ + char frequency_string[11]; + char time_string[11]; + this->int_to_string(frequency, frequency_string); + this->int_to_string(time, time_string); + this->append("#12 M2210 F"); + this->append(frequency); + this->append(" T"); + this->append(time); + this->append("\n"); + } + void init(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); From fb52ce260063aa55a45ce7549ebe9607174b40f7 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 01:09:50 +0200 Subject: [PATCH 15/37] add convert_coordinates_to_angle_of_joints_to_gcode to uarm_gcode_generator class --- code/headers/uarm_gcode_generator.hpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 6eddaea..72c3ba1 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -155,6 +155,22 @@ namespace r2d2::robot_arm { this->append("\n"); } + void convert_coordinates_to_angle_of_joints_to_gcode(const vector3i_c &coordinate){ + char x_string[11]; // max number of int digits (10) + '\0' = 11 + char y_string[11]; + char z_string[11]; + this->int_to_string(coordinate.x, x_string); + this->int_to_string(coordinate.y, y_string); + this->int_to_string(coordinate.z, z_string); + this->append("#13 M2220 X"); + this->append(x_string); + this->append(" Y"); + this->append(y_string); + this->append(" Z"); + this->append(z_string); + this->append("\n"); + } + void init(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); @@ -162,5 +178,7 @@ namespace r2d2::robot_arm { this->append(on_off_string); this->append("\n"); } + + }; } // namespace r2d2::robot_arm \ No newline at end of file From 1e502d19ac955b8d8a2a0f7cbf51ac4cc84ac83b Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 01:16:17 +0200 Subject: [PATCH 16/37] add convert_angle_of_joints_to_coordinates_to_gcode to uarm_gcode_generator class --- code/headers/uarm_gcode_generator.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 72c3ba1..8b27d06 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -171,6 +171,22 @@ namespace r2d2::robot_arm { this->append("\n"); } + void convert_angle_of_joints_to_coordinates_to_gcode(const vector3i_c &angle){ + char b_string[11]; // max number of int digits (10) + '\0' = 11 + char l_string[11]; + char r_string[11]; + this->int_to_string(angle.x, b_string); + this->int_to_string(angle.y, l_string); + this->int_to_string(angle.z, r_string); + this->append("#14 M2221 B"); + this->append(b_string); + this->append(" L"); + this->append(l_string); + this->append(" R"); + this->append(r_string); + this->append("\n"); + } + void init(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); From 11ab9561b7b90cc51c15092a3fb9b0000c4ec000 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 01:28:08 +0200 Subject: [PATCH 17/37] add set_current_position_head_to_reference_position_to_gcode to uarm_gcode_generator class --- code/headers/uarm_gcode_generator.hpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 8b27d06..aa5bc8b 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -187,6 +187,30 @@ namespace r2d2::robot_arm { this->append("\n"); } + void check_posibility_of_coordinates_cartesian_polar_to_gecode(const vector3i_c &coordinate, const uint8_t &id){ + char x_string[11]; // max number of int digits (10) + '\0' = 11 + char y_string[11]; + char z_string[11]; + char id_string[2]; + this->int_to_string(coordinate.x, x_string); + this->int_to_string(coordinate.y, y_string); + this->int_to_string(coordinate.z, z_string); + this->int_to_string(id, id_string); + this->append("#15 M2222 X"); + this->append(x_string); + this->append(" Y"); + this->append(y_string); + this->append(" Z"); + this->append(z_string); + this->append(" P"); + this->append(id_string); + this->append("\n"); + } + + void set_current_position_head_to_reference_position_to_gcode(){ + this->append("#16 M2401\n"); + } + void init(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); From b77d27ee86b6acb14b32c8c95589950744f3068d Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 11:43:11 +0200 Subject: [PATCH 18/37] add attach_all_joint_motors to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 10 +++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 33f988d..b543c59 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -76,6 +76,8 @@ namespace r2d2::robot_arm { void relative_displacement_polar_head(const vector3i_c &coordinate, const uint16_t &speed); + void attach_all_joint_motors(); + /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index d8d3937..00c1c01 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -45,7 +45,7 @@ namespace r2d2::robot_arm { this->debug(); } - + //------------------SETTING COMMANDS------------------// @@ -55,6 +55,14 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::attach_all_joint_motors() { + gcode_generator.attach_all_joint_motors_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + + //------------------ESSENTIAL METHODS------------------// void uarm_swift_pro_c::debug(){ From 9a9d50f09905980390f0f127ffa96bf02ac87fad Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 27 Jun 2019 11:47:32 +0200 Subject: [PATCH 19/37] add detach_all_joint_motors to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index b543c59..8c7e906 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -77,6 +77,8 @@ namespace r2d2::robot_arm { void relative_displacement_polar_head(const vector3i_c &coordinate, const uint16_t &speed); void attach_all_joint_motors(); + + void detach_all_joint_motors(); /** * @brief Initialization function diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 00c1c01..cf289f3 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -61,6 +61,12 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::detach_all_joint_motors() { + gcode_generator.detach_all_joint_motors_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + //------------------ESSENTIAL METHODS------------------// From 0d7b6ed7695fefb44d798ee4831936947dd477cb Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 15:44:59 +0200 Subject: [PATCH 20/37] add attach_motor_by_id to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 4 ++++ code/src/uarm_swift_pro.cpp | 12 ++++++++++++ 2 files changed, 16 insertions(+) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 8c7e906..636ff60 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -80,6 +80,10 @@ namespace r2d2::robot_arm { void detach_all_joint_motors(); + void return_cartesian_coordinates_by_speed(const uint16_t &speed); + + void attach_motor_by_id(const uint8_t &id); + /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index cf289f3..f5ee64d 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -67,6 +67,18 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::return_cartesian_coordinates_by_speed(const uint16_t &speed){ + gcode_generator.return_cartesian_coordinates_by_speed_to_gcode(speed); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::attach_motor_by_id(const uint8_t &id){ + gcode_generator.attach_motor_by_id_to_gcode(id); + this->send_command(gcode_generator.get_buffer); + this->debug(); + } + //------------------ESSENTIAL METHODS------------------// From f1748a5cb9acc62bc432c6b946d765dfbc5d2d02 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 15:50:05 +0200 Subject: [PATCH 21/37] add detach_motor_by_id to uarm_swift_pro class and debug get_buffer by missing () --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 636ff60..1ee89a2 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -84,6 +84,8 @@ namespace r2d2::robot_arm { void attach_motor_by_id(const uint8_t &id); + void detach_motor_by_id(const uint8_t &id); + /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index f5ee64d..b441eee 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -75,11 +75,15 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::attach_motor_by_id(const uint8_t &id){ gcode_generator.attach_motor_by_id_to_gcode(id); - this->send_command(gcode_generator.get_buffer); + this->send_command(gcode_generator.get_buffer()); this->debug(); } - + void uarm_swift_pro_c::detach_motor_by_id(const uint8_t &id){ + gcode_generator.detach_motor_by_id_to_gcode(id); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } //------------------ESSENTIAL METHODS------------------// From 2d6bbe55b26f42456690b7cd98adfa5ce8fe267f Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 15:59:45 +0200 Subject: [PATCH 22/37] add check_attached_motor_by_id to set_buzzer to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 1ee89a2..6a36d83 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -85,6 +85,8 @@ namespace r2d2::robot_arm { void attach_motor_by_id(const uint8_t &id); void detach_motor_by_id(const uint8_t &id); + + void check_attached_motor_by_id(const uint8_t &id); /** * @brief Initialization function diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index b441eee..e6eceec 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -85,6 +85,18 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::check_attached_motor_by_id(const uint8_t &id){ + gcode_generator.check_attached_motor_by_id_to_gcode(id); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::set_buzzer(const uint16_t &frequence, const uint16_t &time){ + gcode_generator.set_buzzer_to_gcode(frequence,time); + this->send_command(gcode_generator.get_buffer()); + this-debug(); + } + //------------------ESSENTIAL METHODS------------------// void uarm_swift_pro_c::debug(){ From b5bd0e8938463a29d1a580a0d1f27926d49c32f2 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 16:11:08 +0200 Subject: [PATCH 23/37] debug set buzzer_to_gcode and added it to uarm_swift_pro --- code/headers/uarm_gcode_generator.hpp | 4 ++-- code/headers/uarm_swift_pro.hpp | 3 ++- code/src/uarm_swift_pro.cpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index aa5bc8b..2db8212 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -149,9 +149,9 @@ namespace r2d2::robot_arm { this->int_to_string(frequency, frequency_string); this->int_to_string(time, time_string); this->append("#12 M2210 F"); - this->append(frequency); + this->append(frequency_string); this->append(" T"); - this->append(time); + this->append(time_string); this->append("\n"); } diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 6a36d83..6dfd93f 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -87,7 +87,8 @@ namespace r2d2::robot_arm { void detach_motor_by_id(const uint8_t &id); void check_attached_motor_by_id(const uint8_t &id); - + + void set_buzzer(const uint16_t &frequence, const uint16_t &time); /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index e6eceec..710f307 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -94,7 +94,7 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::set_buzzer(const uint16_t &frequence, const uint16_t &time){ gcode_generator.set_buzzer_to_gcode(frequence,time); this->send_command(gcode_generator.get_buffer()); - this-debug(); + this->debug(); } //------------------ESSENTIAL METHODS------------------// From 1dbc218a52a4fbc121761032546ae78e78e72778 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 16:55:09 +0200 Subject: [PATCH 24/37] add convert_coordinates_to_angle_of_joints to set_buzzer to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 6dfd93f..908b321 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -89,6 +89,8 @@ namespace r2d2::robot_arm { void check_attached_motor_by_id(const uint8_t &id); void set_buzzer(const uint16_t &frequence, const uint16_t &time); + + void convert_coordinates_to_angle_of_joints(const vector3i_c &coordinate); /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 710f307..f68866b 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -97,6 +97,12 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::convert_coordinates_to_angle_of_joints(const vector3i_c &coordinate){ + gcode_generator.convert_coordinates_to_angle_of_joints_to_gcode(coordinate); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + //------------------ESSENTIAL METHODS------------------// void uarm_swift_pro_c::debug(){ From 96e32b665b14a5e07a05216cd916e346720107ab Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 17:00:03 +0200 Subject: [PATCH 25/37] add convert_angle_of_joints_to_coordinates to set_buzzer to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 908b321..52f397a 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -91,6 +91,8 @@ namespace r2d2::robot_arm { void set_buzzer(const uint16_t &frequence, const uint16_t &time); void convert_coordinates_to_angle_of_joints(const vector3i_c &coordinate); + + void convert_angle_of_joints_to_coordinates(const vector3i_c &angle); /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index f68866b..d8c09a2 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -103,6 +103,12 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::convert_angle_of_joints_to_coordinates(const vector3i_c &angle){ + gcode_generator.convert_angle_of_joints_to_coordinates_to_gcode(angle); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + //------------------ESSENTIAL METHODS------------------// void uarm_swift_pro_c::debug(){ From c5464560fb90f3aded03aedc3ff7805b08ec1be8 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 17:03:54 +0200 Subject: [PATCH 26/37] add check_posibility_of_coordinates_cartesian_polar to set_buzzer to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 52f397a..81f4fc1 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -93,6 +93,8 @@ namespace r2d2::robot_arm { void convert_coordinates_to_angle_of_joints(const vector3i_c &coordinate); void convert_angle_of_joints_to_coordinates(const vector3i_c &angle); + + void check_posibility_of_coordinates_cartesian_polar(const vector3i_c &coordinate, const uint8_t &id); /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index d8c09a2..9b388e0 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -109,6 +109,12 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::check_posibility_of_coordinates_cartesian_polar(const vector3i_c &coordinate, const uint8_t &id){ + gcode_generator.check_posibility_of_coordinates_cartesian_polar_to_gecode(coordinate, id); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + //------------------ESSENTIAL METHODS------------------// void uarm_swift_pro_c::debug(){ From 3967ab9222bb08022a2b23e20d16e974965748ba Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 17:07:17 +0200 Subject: [PATCH 27/37] add set_current_position_head_to_reference_position to set_buzzer to uarm_swift_pro class --- code/headers/uarm_swift_pro.hpp | 2 ++ code/src/uarm_swift_pro.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 81f4fc1..8ea0f80 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -95,6 +95,8 @@ namespace r2d2::robot_arm { void convert_angle_of_joints_to_coordinates(const vector3i_c &angle); void check_posibility_of_coordinates_cartesian_polar(const vector3i_c &coordinate, const uint8_t &id); + + void set_current_position_head_to_reference_position(); /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 9b388e0..373d507 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -115,6 +115,12 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::set_current_position_head_to_reference_position(){ + gcode_generator.set_current_position_head_to_reference_position_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + //------------------ESSENTIAL METHODS------------------// void uarm_swift_pro_c::debug(){ From 3d46c26d9d2547ca40b374fcff81df35907f6b97 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Fri, 28 Jun 2019 17:49:07 +0200 Subject: [PATCH 28/37] add querying commands to uarm_gcode_generator.hpp --- code/headers/uarm_gcode_generator.hpp | 49 ++++++++++++++++++++++++++- code/src/uarm_swift_pro.cpp | 2 +- 2 files changed, 49 insertions(+), 2 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 2db8212..b2e6044 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -211,7 +211,7 @@ namespace r2d2::robot_arm { this->append("#16 M2401\n"); } - void init(const uint8_t &on_off){ + void init_to_gcode(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); this->append("#1 M2122 V"); @@ -219,6 +219,53 @@ namespace r2d2::robot_arm { this->append("\n"); } + void get_current_angle_of_joints_to_gecode(){ + this->append("#17 P2200\n"); + } + + void get_device_name_to_gecode(){ + this->append("#18 P2201\n"); + } + + void get_hardware_version_to_gecode(){ + this->append("#19 P2202\n"); + } + + void get_software_version_to_gcode(){ + this->append("#20 P2203\n"); + } + + void get_API_version_to_gcode(){ + this->append("#21 P2204\n"); + } + + void get_UID_to_gcode(){ + this->append("#22 P2205\n"); + } + + void get_angle_of_joint_id(const uint8_t &id){ + char id_string[2]; + this->int_to_string(id, id_string); + this->append("#23 P2206 N"); + this->append(id); + this->append("\n"); + } + + void get_current_coordinates(){ + this->append("#24 P2220\n"); + } + + void get_current_polar_coordinates(){ + this->append("#25 P2221\n"); + } + + void get_status_power_connection(){ + this->append("#26 P2234\n"); + } + + void check_current_status(){ + this->append("#27 P2400\n"); + } }; } // namespace r2d2::robot_arm \ No newline at end of file diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 373d507..57710ba 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -50,7 +50,7 @@ namespace r2d2::robot_arm { //------------------SETTING COMMANDS------------------// void uarm_swift_pro_c::init(const uint8_t &on_off) { - gcode_generator.init(on_off); + gcode_generator.init_to_gcode(on_off); this->send_command(gcode_generator.get_buffer()); // report when stop this->debug(); } From d7a937096aad8f6a65ce458ceacb609bc9cd923d Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Sat, 29 Jun 2019 15:00:24 +0200 Subject: [PATCH 29/37] add querying commands to uarm_swift_pro class --- code/headers/uarm_gcode_generator.hpp | 12 ++--- code/headers/uarm_swift_pro.hpp | 24 ++++++++- code/src/uarm_swift_pro.cpp | 70 +++++++++++++++++++++++++++ 3 files changed, 99 insertions(+), 7 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index b2e6044..db5ac2e 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -243,27 +243,27 @@ namespace r2d2::robot_arm { this->append("#22 P2205\n"); } - void get_angle_of_joint_id(const uint8_t &id){ + void get_angle_of_joint_id_to_gcode(const uint8_t &id){ char id_string[2]; this->int_to_string(id, id_string); this->append("#23 P2206 N"); - this->append(id); + this->append(id_string); this->append("\n"); } - void get_current_coordinates(){ + void get_current_coordinate_to_gcode(){ this->append("#24 P2220\n"); } - void get_current_polar_coordinates(){ + void get_current_polar_coordinates_to_gcode(){ this->append("#25 P2221\n"); } - void get_status_power_connection(){ + void get_status_power_connection_to_gcode(){ this->append("#26 P2234\n"); } - void check_current_status(){ + void check_current_status_to_gcode(){ this->append("#27 P2400\n"); } diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 8ea0f80..21fb4c2 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -96,7 +96,29 @@ namespace r2d2::robot_arm { void check_posibility_of_coordinates_cartesian_polar(const vector3i_c &coordinate, const uint8_t &id); - void set_current_position_head_to_reference_position(); + void set_current_position_head_to_reference_position(); + + void get_current_angle_of_joints(); + + void get_device_name(); + + void get_hardware_version(); + + void get_software_version(); + + void get_API_version(); + + void get_UID(); + + void get_angle_of_joint_id(const uint8_t &id); + + void get_current_coordinates(); + + void get_current_polar_coordinates(); + + void get_status_power_connection(); + + void check_current_status(); /** * @brief Initialization function * This function is for actions that need to be executed at diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 57710ba..74c524c 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -121,6 +121,76 @@ namespace r2d2::robot_arm { this->debug(); } + + //-------------------QUERYING METHODS------------------// + + void uarm_swift_pro_c::get_current_angle_of_joints(){ + gcode_generator.get_current_angle_of_joints_to_gecode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_device_name(){ + gcode_generator.get_device_name_to_gecode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_hardware_version(){ + gcode_generator.get_hardware_version_to_gecode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_software_version(){ + gcode_generator.get_software_version_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_API_version(){ + gcode_generator.get_API_version_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_UID(){ + gcode_generator.get_UID_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_angle_of_joint_id(const uint8_t &id){ + gcode_generator.get_angle_of_joint_id_to_gcode(id); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_current_coordinates(){ + gcode_generator.get_current_coordinate_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_current_polar_coordinates(){ + gcode_generator.get_current_polar_coordinates_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::get_status_power_connection(){ + gcode_generator.get_status_power_connection_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + void uarm_swift_pro_c::check_current_status(){ + gcode_generator.check_current_status_to_gcode(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); + } + + //------------------ESSENTIAL METHODS------------------// void uarm_swift_pro_c::debug(){ From 32e9f55ce6028b8f4e1ca17bec16f6357138239f Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Sun, 30 Jun 2019 09:53:13 +0200 Subject: [PATCH 30/37] add test with mock_bus --- code/headers/gcode_generator.hpp | 2 +- code/headers/robot_arm_interface.hpp | 10 ++++----- code/headers/uarm_gcode_generator.hpp | 3 ++- code/main.cpp | 4 ++-- code/src/uarm_swift_pro.cpp | 10 +++++++-- test/main.cpp | 32 ++++++++++++++++++++++++++- 6 files changed, 49 insertions(+), 12 deletions(-) diff --git a/code/headers/gcode_generator.hpp b/code/headers/gcode_generator.hpp index d630909..f400510 100644 --- a/code/headers/gcode_generator.hpp +++ b/code/headers/gcode_generator.hpp @@ -160,4 +160,4 @@ namespace r2d2::robot_arm { virtual void coordinate_to_gcode(const vector3i_c &coordinate, const uint16_t &speed = 500) = 0; }; -} // namespace r2d2::robot_arm \ No newline at end of file +} // namespace r2d2::robot_arm diff --git a/code/headers/robot_arm_interface.hpp b/code/headers/robot_arm_interface.hpp index 75f3589..5693069 100644 --- a/code/headers/robot_arm_interface.hpp +++ b/code/headers/robot_arm_interface.hpp @@ -19,18 +19,18 @@ namespace r2d2::robot_arm { void process()override{ comm.request(r2d2::frame_type::ROBOT_ARM); + hwlib::cout << comm.has_data() << hwlib::endl; while (comm.has_data()){ + hwlib::cout << "test" << hwlib::endl; auto frame = comm.get_data(); // Process the frame - // Don't handle requests - if (frame.request) { - continue; - } - const auto data = frame.as_frame_type(); + hwlib::cout << "mock bus data x"<< data.x << hwlib::endl; + move_head_to_coordinate(vector3i_c(data.x, data.y, data.z), data.speed); + hwlib::cout << "test" << hwlib::endl; } } diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index db5ac2e..d05a979 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -1,3 +1,4 @@ +#pragma once #include namespace r2d2::robot_arm { @@ -214,7 +215,7 @@ namespace r2d2::robot_arm { void init_to_gcode(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); - this->append("#1 M2122 V"); + this->append("#17 M2122 V"); this->append(on_off_string); this->append("\n"); } diff --git a/code/main.cpp b/code/main.cpp index b0d16a0..c5a015b 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -18,8 +18,8 @@ int main() { // driver code - uarm.init(1); - //uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 100, 100)); + //uarm.init(1); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 100, 100),90); //uarm.rotate_id_motor_to_degree(0,60); //hwlib::wait_ms(20000); //uarm.move_head_to_polar_coordinate(r2d2::robot_arm::vector3i_c(200, 200, 200),1000); diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 74c524c..553b1b2 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -9,13 +9,17 @@ namespace r2d2::robot_arm { //------------------MOVING COMMANDS------------------// void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) { + + hwlib::cout << "coordinaat x " <send_command(gcode_generator.get_buffer()); - this->debug(); + hwlib::cout << gcode_generator.get_buffer() << hwlib::endl; + //this->send_command(gcode_generator.get_buffer()); + //this->debug(); } void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate) { gcode_generator.coordinate_to_gcode(coordinate, default_speed); + hwlib::cout << gcode_generator.get_buffer() << hwlib::endl; this->send_command(gcode_generator.get_buffer()); this->debug(); } @@ -23,6 +27,8 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed){ gcode_generator.polar_coordinate_to_gcode(coordinate, speed); + hwlib::cout << "kip" << hwlib::endl; + hwlib::cout << gcode_generator.get_buffer() << hwlib::endl; this->send_command(gcode_generator.get_buffer()); this->debug(); } diff --git a/test/main.cpp b/test/main.cpp index 7b42473..c0a6dc8 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -1,6 +1,8 @@ #include +#include #define CATCH_CONFIG_MAIN #include +#include #include TEST_CASE("Appending gone wrong!", "Testing generator") { @@ -36,7 +38,7 @@ TEST_CASE("Appending front", "Testing generator") { TEST_CASE("Converting vector to gcode command", "Testing generator") { r2d2::robot_arm::uarm_gcode_generator_c<100> generator; generator.coordinate_to_gcode(r2d2::robot_arm::vector3i_c(1, 2, 3), 200); - REQUIRE(std::strcmp(generator.get_buffer(), "#n G0 X1 Y2 Z3 F200\n") == + REQUIRE(std::strcmp(generator.get_buffer(), "#1 G0 X1 Y2 Z3 F200\n") == 0); // std::strcmp will not be needed later for debug only } @@ -171,3 +173,31 @@ TEST_CASE("Operator/= with test_data_type", "Testing vector3i") { c /= 2; REQUIRE(c == r2d2::robot_arm::vector3i_c(5, 50, 100)); } + +TEST_CASE("process function", "[canbus]"){ + r2d2::mock_comm_c comm; + + auto usart = r2d2::usart::test_usart_c(); + + comm.listen_for_frames({r2d2::frame_type::ALL}); + + REQUIRE(comm.accepts_frame(r2d2::frame_type::ROBOT_ARM)); + + r2d2::robot_arm::uarm_swift_pro_c uarm(usart, comm); + + + r2d2::frame_robot_arm_s frame; + + frame.x = 8; + frame.y = 200; + frame.z = 200; + frame.speed = 300; + + auto data2 = comm.create_frame(frame); + + comm.accept_frame(data2); + + uarm.process(); + + +} \ No newline at end of file From 79182126346339ed0431468fd7c4fc157be12d0f Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Sun, 30 Jun 2019 11:58:57 +0200 Subject: [PATCH 31/37] add documentation in uarm_swift_pro.hpp --- code/headers/uarm_swift_pro.hpp | 157 ++++++++++++++++++++++++++++++-- 1 file changed, 150 insertions(+), 7 deletions(-) diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 21fb4c2..aaf2171 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -22,7 +22,12 @@ namespace r2d2::robot_arm { public: /** - * Robot arm constructor, needs an usart bus for communication + * @brief constructor + * Robot arm constructor, needs an usart bus for communication and base_comm_c for + * can bus implementation + * + * @param usart_bus usart bus for communication with the uarm + * @param comm internal communication for can bus */ uarm_swift_pro_c(usart_c &usart_bus, base_comm_c &comm); @@ -46,22 +51,25 @@ namespace r2d2::robot_arm { bool send_command(const char *command); /** - * Move arm's head to 3D coordinate + * @brief Move arm's head to cartesian coordinate * Moves head of robot arm head/end effector towards the 3D coordinate * [x,y,z] The speed is default at 500 mm/min * - * @param coordinate 3D integer coordinate for the head + * @param coordinate cartesian integer coordinate for the head (XYZ) * @param speed movement speed in mm/min */ void move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) override; + /** - * This function moves the uArm swift pro head to a certain 3d location. + * @brief move head to coordinate + * This function moves the uArm swift pro head to a certain cartesian location. * - * @param coordinate + * @param coordinate cartesian integer coordinate for the head (XYZ) */ void move_head_to_coordinate(const vector3i_c &coordinate) override; /** + * @brief Move head to polar coordinate * This function moves the uArm swift pro head to a certain polar coordination. * * @param coordinate polar coordinates, S is stretch(mm), R is rotation(degree),H is height(mm) @@ -69,56 +77,187 @@ namespace r2d2::robot_arm { */ void move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed); - + + /** + * @brief Rotate id motor to degree + * This function rotates the chosen motor with his id to a certain degree. + * + * @param id id of joints (0 ~ 3) + * @param degree degree of angle (0 ~ 180) + */ void rotate_id_motor_to_degree(const uint8_t &id, const uint16_t °ree); + /** + * @brief relative displacement head + * This function displace relative the head from his starting point with to a cartesian location. + * + * @param coordinate cartesian integer coordinate for the head (XYZ) + * @param speed movement speed in mm/min + */ void relative_displacement_head(const vector3i_c &coordinate, const uint16_t &speed); + /** + * @brief relative displacement polar head + * This function displace relative the head from his starting point with to a polar location. + * + * @param coordinate polar coordinates, S is stretch(mm), R is rotation(degree),H is height(mm) + * @param speed movement speed in mm/min + */ void relative_displacement_polar_head(const vector3i_c &coordinate, const uint16_t &speed); + /** + * @brief attach all joint motors + * This setting function attach all the joint motors. + */ void attach_all_joint_motors(); + /** + * @brief detach all joint motors + * This setting function attach all the joint motors. + */ void detach_all_joint_motors(); + /** + * @brief return cartesian coordinates by speed + * This function wil set a time cycle of feedback, return Cartesian coordinates by debug funciton + * + * @param speed is time(seconds) + */ void return_cartesian_coordinates_by_speed(const uint16_t &speed); + /** + * @brief attach motor by id + * This function wil attach motor by id + * + * @param id id of joints (0 ~ 3) + */ void attach_motor_by_id(const uint8_t &id); + /** + * @brief detach motor by id + * This function wil detach motor by id + * + * @param id id of joints (0 ~ 3) + */ void detach_motor_by_id(const uint8_t &id); - + + /** + * @brief check attached motor by id + * This function wil check if the motor is attached by id + * + * @param id id of joints (0 ~ 3) + */ void check_attached_motor_by_id(const uint8_t &id); + /** + * @brief set buzzer + * This function turns on a buzzer for a chosen time with a chosen frequency + * + * @param frequence frequence of buzzer (hertz) + * @param time the amount of time the buzzer turns on (ms) + */ void set_buzzer(const uint16_t &frequence, const uint16_t &time); + /** + * @brief convert coordinates to angle of joints + * This function convert coordinates to angle of joints, + * return angle of joints by debug method + * + * @param coordinate cartesian integer coordinate(XYZ) + */ void convert_coordinates_to_angle_of_joints(const vector3i_c &coordinate); + /** + * @brief convert angle of joints to coordinate + * This function Convert angle of joints to cartesian coordinates, + * return cartesian coordinates by debug method + * + * @param angle angle of the joints: vector3("B joint 0","L joint 1","R joints 2") from 0~180 degree + */ void convert_angle_of_joints_to_coordinates(const vector3i_c &angle); + /** + * @brief check posibility of coordinates cartesian polar + * This function check if the head can reach, id 1 polar, id 0 Cartesian coordinates, + * return ok V1 when reachable, ok V0 when unreachable + * + * @param coordinate cartesian integer coordinate(XYZ) + * @param id 0 = cartesian , 1 = polar + */ void check_posibility_of_coordinates_cartesian_polar(const vector3i_c &coordinate, const uint8_t &id); + /** + * @brief set current position head to reference position + * This function set the current position into the reference position + */ void set_current_position_head_to_reference_position(); + /** + * @brief get current angle of joints + * This function wil get the current angle of joints + */ void get_current_angle_of_joints(); + /** + * @brief get device name + * This function wil get the device name + */ void get_device_name(); + /** + * @brief get hardware version + * This function wil get the hardware version + */ void get_hardware_version(); + /** + * @brief get software version + * This function wil get the software version + */ void get_software_version(); + /** + * @brief get API version + * This function wil get the API version + */ void get_API_version(); + /** + * @brief get UID + * This function wil get the UID + */ void get_UID(); + /** + * @brief get angle of joint id + * This function wil get the angle of number 0 joint (0~2) + */ void get_angle_of_joint_id(const uint8_t &id); + /** + * @brief get current coordinates + * This function wil get current coordinates + */ void get_current_coordinates(); + /** + * @brief get current polar coordinates + * This function wil get current polar coordinates + */ void get_current_polar_coordinates(); + /** + * @brief get status power connection + * This function wil get the status of power connection + */ void get_status_power_connection(); + /** + * @brief get current status + * This function wil check current status + */ void check_current_status(); + /** * @brief Initialization function * This function is for actions that need to be executed at @@ -127,6 +266,10 @@ namespace r2d2::robot_arm { */ void init(const uint8_t &on_off); + /** + * @brief debug + * This function will write the feedback of the uarm to hwlib::cout + */ void debug(); }; } // namespace r2d2::robot_arm From db09e636ec4dc15cf744147ca0a3bea3641421a5 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Sun, 30 Jun 2019 11:59:47 +0200 Subject: [PATCH 32/37] deleted debug cout code for testing the mockbus --- code/headers/robot_arm_interface.hpp | 8 -------- code/src/uarm_swift_pro.cpp | 10 ++-------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/code/headers/robot_arm_interface.hpp b/code/headers/robot_arm_interface.hpp index 5693069..9fa7a88 100644 --- a/code/headers/robot_arm_interface.hpp +++ b/code/headers/robot_arm_interface.hpp @@ -19,18 +19,10 @@ namespace r2d2::robot_arm { void process()override{ comm.request(r2d2::frame_type::ROBOT_ARM); - hwlib::cout << comm.has_data() << hwlib::endl; while (comm.has_data()){ - hwlib::cout << "test" << hwlib::endl; auto frame = comm.get_data(); - // Process the frame - const auto data = frame.as_frame_type(); - - hwlib::cout << "mock bus data x"<< data.x << hwlib::endl; - move_head_to_coordinate(vector3i_c(data.x, data.y, data.z), data.speed); - hwlib::cout << "test" << hwlib::endl; } } diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 553b1b2..74c524c 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -9,17 +9,13 @@ namespace r2d2::robot_arm { //------------------MOVING COMMANDS------------------// void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) { - - hwlib::cout << "coordinaat x " <send_command(gcode_generator.get_buffer()); - //this->debug(); + this->send_command(gcode_generator.get_buffer()); + this->debug(); } void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate) { gcode_generator.coordinate_to_gcode(coordinate, default_speed); - hwlib::cout << gcode_generator.get_buffer() << hwlib::endl; this->send_command(gcode_generator.get_buffer()); this->debug(); } @@ -27,8 +23,6 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::move_head_to_polar_coordinate(const vector3i_c &coordinate, const uint16_t &speed){ gcode_generator.polar_coordinate_to_gcode(coordinate, speed); - hwlib::cout << "kip" << hwlib::endl; - hwlib::cout << gcode_generator.get_buffer() << hwlib::endl; this->send_command(gcode_generator.get_buffer()); this->debug(); } From 12daf0a5726b41b588e22ff1e12bb3a15ead0599 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Sun, 30 Jun 2019 13:02:59 +0200 Subject: [PATCH 33/37] add documentation for uarm_gcode_generator.hpp --- code/headers/uarm_gcode_generator.hpp | 174 +++++++++++++++++++++++--- code/headers/uarm_swift_pro.hpp | 9 +- 2 files changed, 165 insertions(+), 18 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index d05a979..a32dba8 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -7,10 +7,12 @@ namespace r2d2::robot_arm { public: /** - * Converts a vector3i_c to a gcode command for uArm + * @brief coordinate to gcode + * Makes a vector3i_c cartesian coordinate gcode command for uArm where the head + * will go * - * @param vector3i - * @param uint8_t speed + * @param coordinate cartesian integer coordinate for the head (XYZ) + * @param speed movement speed in mm/min * */ void coordinate_to_gcode(const vector3i_c &coordinate, const uint16_t &speed) { char x_string[11]; // max number of int digits (10) + '\0' = 11 @@ -32,6 +34,14 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief polar coordinate to gcode + * Makes a vector3i_c polar coordinate gcode command for uArm where the head + * will go. + * + * @param coordinate polar coordinates, S is stretch(mm), R is rotation(degree),H is height(mm) + * @param speed movement speed in mm/min + */ void polar_coordinate_to_gcode(const vector3i_c &coordinate, const uint16_t &speed){ char s_string[11]; // max number of int digits (10) + '\0' = 11 char r_string[11]; @@ -52,6 +62,13 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief rotate id motor degree to gcode + * Makes for a specific motor(by id) the degree the motor has to go gcode command for the uarm + * + * @param id id of joints (0 ~ 3) + * @param degree degree of angle (0 ~ 180) + */ void rotate_id_motor_degree_to_gcode(const uint8_t &id, const uint16_t °ree){ char id_string[2]; char degree_string[4]; @@ -64,6 +81,13 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief relative displacement to gcode + * Makes the relative displavement of the head gcode command for the uarm + * + * @param coordinate cartesian integer coordinate for the head (XYZ) + * @param speed movement speed in mm/min + */ void relative_displacement_to_gcode(const vector3i_c &coordinate, const uint16_t &speed) { char x_string[11]; // max number of int digits (10) + '\0' = 11 char y_string[11]; @@ -84,6 +108,13 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief relative displacement polar to gcode + * Makes the relative polar displacement of the head gcode command for the uarm + * + * @param coordinate polar coordinates, S is stretch(mm), R is rotation(degree),H is height(mm) + * @param speed movement speed in mm/min + */ void relative_displacement_polar_to_gcode(const vector3i_c &coordinate, const uint16_t &speed){ char s_string[11]; // max number of int digits (10) + '\0' = 11 char r_string[11]; @@ -104,14 +135,27 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief attach all joint motors to gcode + * Makes the attach all joint motors gcode command for the uarm + */ void attach_all_joint_motors_to_gcode(){ this->append("#6 M17\n"); } + /** + * @brief detach all joint motors to gcode + * Makes the detach all joint motors gcode command for the uarm + */ void detach_all_joint_motors_to_gcode(){ this->append("#7 M2019\n"); } - + /** + * @brief return cartesian coordinates by speed to gcode + * Makes the cartesian coordinates by speed gcode command for the uarm + * + * @param speed is time(seconds) + */ void return_cartesian_coordinates_by_speed_to_gcode(const uint16_t &speed){ char speed_string[11]; this->int_to_string(speed, speed_string); @@ -120,6 +164,12 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief attach motor by id to gcode + * Makes the attach motor by id gcode command for the uarm + * + * @param id id of joints (0 ~ 3) + */ void attach_motor_by_id_to_gcode(const uint8_t &id){ char id_string[2]; this->int_to_string(id, id_string); @@ -128,6 +178,12 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief detach motor by id to gcode + * Makes the detach motor by id gcode command for the uarm + * + * @param id id of joints (0 ~ 3) + */ void detach_motor_by_id_to_gcode(const uint8_t &id){ char id_string[2]; this->int_to_string(id, id_string); @@ -136,6 +192,12 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief check attached motor by id to gcode + * Makes the check motor by id gcode command for the uarm + * + * @param id id of joints (0 ~ 3) + */ void check_attached_motor_by_id_to_gcode(const uint8_t &id){ char id_string[2]; this->int_to_string(id, id_string); @@ -144,6 +206,13 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief set buzzer to gcode + * Makes the set buzzer gcode command for the uarm + * + * @param frequence frequence of buzzer (hertz) + * @param time the amount of time the buzzer turns on (ms) + */ void set_buzzer_to_gcode(const uint16_t &frequency, const uint16_t &time){ char frequency_string[11]; char time_string[11]; @@ -156,6 +225,12 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief convert coordinates to angle of joints to gcode + * Makes the convert coordinates to angle of joints gcode command for the uarm + * + * @param coordinate cartesian integer coordinate(XYZ) + */ void convert_coordinates_to_angle_of_joints_to_gcode(const vector3i_c &coordinate){ char x_string[11]; // max number of int digits (10) + '\0' = 11 char y_string[11]; @@ -172,6 +247,12 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief convert angle of joints to coordinates to gcode + * Makes the convert angle of joints to coordinates gcode command for the uarm + * + * @param angle angle of the joints: vector3("B joint 0","L joint 1","R joints 2") from 0~180 degree + */ void convert_angle_of_joints_to_coordinates_to_gcode(const vector3i_c &angle){ char b_string[11]; // max number of int digits (10) + '\0' = 11 char l_string[11]; @@ -188,6 +269,13 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief check posibility of coordinates cartesian polar to gcode + * Makes the check posibility of coordinates cartesian polar gcode command for the uarm + * + * @param coordinate cartesian integer coordinate(XYZ) + * @param id 0 = cartesian , 1 = polar + */ void check_posibility_of_coordinates_cartesian_polar_to_gecode(const vector3i_c &coordinate, const uint8_t &id){ char x_string[11]; // max number of int digits (10) + '\0' = 11 char y_string[11]; @@ -208,10 +296,20 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief set current position head to reference position to gcode + * Makes the set current position head to reference position gcode command for the uarm + */ void set_current_position_head_to_reference_position_to_gcode(){ this->append("#16 M2401\n"); } + /** + * @brief Initialization to gcode + * Makes the initialization gcode command for the uarm + * + * @parram on_off to set the uarm on or off + */ void init_to_gcode(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); @@ -220,52 +318,98 @@ namespace r2d2::robot_arm { this->append("\n"); } + /** + * @brief get current angle of joints to gcode + * Makes the get current angle of the joints gcode command for the uarm + */ void get_current_angle_of_joints_to_gecode(){ - this->append("#17 P2200\n"); + this->append("#18 P2200\n"); } + /** + * @brief get device name to gcode + * Makes the get device name gcode command for the uarm + */ void get_device_name_to_gecode(){ - this->append("#18 P2201\n"); + this->append("#19 P2201\n"); } + /** + * @brief get hardware version to gcode + * Makes the get device name gcode command for the uarm + */ void get_hardware_version_to_gecode(){ - this->append("#19 P2202\n"); + this->append("#20 P2202\n"); } + /** + * @brief get software version to gcode + * Makes the get software version gcode command for the uarm + */ void get_software_version_to_gcode(){ - this->append("#20 P2203\n"); + this->append("#21 P2203\n"); } + /** + * @brief get API version to gcode + * Makes the get API version gcode command for the uarm + */ void get_API_version_to_gcode(){ - this->append("#21 P2204\n"); + this->append("#22 P2204\n"); } + /** + * @brief get UID to gcode + * Makes the get UID gcode command for the uarm for the uarm + */ void get_UID_to_gcode(){ - this->append("#22 P2205\n"); + this->append("#23 P2205\n"); } + /** + * @brief get angle of joint id to gcode + * Makes the get angle of joint id gcode command for the uarm + * + * @param id id of joints (0 ~ 2) + */ void get_angle_of_joint_id_to_gcode(const uint8_t &id){ char id_string[2]; this->int_to_string(id, id_string); - this->append("#23 P2206 N"); + this->append("#24 P2206 N"); this->append(id_string); this->append("\n"); } + /** + * @brief get current coordinates to gcode + * Makes the get current coordinates gcode command for the uarm + */ void get_current_coordinate_to_gcode(){ - this->append("#24 P2220\n"); + this->append("#25 P2220\n"); } + /** + * @brief get current polar coordinates to gcode + * Makes the get current polar coordinates gcode command for the uarm + */ void get_current_polar_coordinates_to_gcode(){ - this->append("#25 P2221\n"); + this->append("#26 P2221\n"); } + /** + * @brief get status power connection to gcode + * Makes the get tatus power connections gcode command for the uarm + */ void get_status_power_connection_to_gcode(){ - this->append("#26 P2234\n"); + this->append("#27 P2234\n"); } + /** + * @brief check current status to gcode + * Makes the check current status gcode command for the uarm + */ void check_current_status_to_gcode(){ - this->append("#27 P2400\n"); + this->append("#28 P2400\n"); } }; diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index aaf2171..023a3ff 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -89,7 +89,7 @@ namespace r2d2::robot_arm { /** * @brief relative displacement head - * This function displace relative the head from his starting point with to a cartesian location. + * This function displace relative the head from his starting point with a cartesian location. * * @param coordinate cartesian integer coordinate for the head (XYZ) * @param speed movement speed in mm/min @@ -112,7 +112,7 @@ namespace r2d2::robot_arm { void attach_all_joint_motors(); /** - * @brief detach all joint motors + * @brief de/tach all joint motors * This setting function attach all the joint motors. */ void detach_all_joint_motors(); @@ -231,6 +231,8 @@ namespace r2d2::robot_arm { /** * @brief get angle of joint id * This function wil get the angle of number 0 joint (0~2) + * + * @param id id of joints (0 ~ 2) */ void get_angle_of_joint_id(const uint8_t &id); @@ -259,10 +261,11 @@ namespace r2d2::robot_arm { void check_current_status(); /** - * @brief Initialization function + * @brief Initialization * This function is for actions that need to be executed at * initialization of the object * + * @parram on_off to set the uarm on or off */ void init(const uint8_t &on_off); From ce8decca5705c1908c1a15d7e541e78ad6232a06 Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Sun, 30 Jun 2019 15:58:17 +0200 Subject: [PATCH 34/37] resolves bug after resolving merge conflicts --- code/headers/robot_arm_interface.hpp | 2 +- code/src/dof4_diy.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/code/headers/robot_arm_interface.hpp b/code/headers/robot_arm_interface.hpp index 6313cdc..4d172c6 100644 --- a/code/headers/robot_arm_interface.hpp +++ b/code/headers/robot_arm_interface.hpp @@ -35,7 +35,7 @@ namespace r2d2::robot_arm { * @param speed The speed the head moves at */ virtual void move_head_to_coordinate(const vector3i_c &coordinate, - uint16_t speed) = 0; + const uint16_t &speed) = 0; /** * @brief diff --git a/code/src/dof4_diy.cpp b/code/src/dof4_diy.cpp index d6c95e9..ddbc48d 100644 --- a/code/src/dof4_diy.cpp +++ b/code/src/dof4_diy.cpp @@ -5,7 +5,7 @@ namespace r2d2::robot_arm { // TODO impl 4dof diy move head } void dof4_diy_c::move_head_to_coordinate(const vector3i_c &coordinate, - uint16_t speed) { + const uint16_t &speed) { // TODO impl 4dof diy move head + speed } } // namespace r2d2::robot_arm \ No newline at end of file From f8c4fdd0469467a902592fe964cfc3c86c27763f Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Tue, 2 Jul 2019 00:48:47 +0200 Subject: [PATCH 35/37] resolve the feedback of PR also I discovered some bugs and discovered + little demo --- code/headers/gcode_generator.hpp | 8 ++++-- code/headers/uarm_gcode_generator.hpp | 8 +++--- code/headers/uarm_swift_pro.hpp | 2 +- code/main.cpp | 40 ++++++++++++++++++++++++++- code/src/uarm_swift_pro.cpp | 30 ++++++++++++++++++++ 5 files changed, 79 insertions(+), 9 deletions(-) diff --git a/code/headers/gcode_generator.hpp b/code/headers/gcode_generator.hpp index 3177534..ce1eed9 100644 --- a/code/headers/gcode_generator.hpp +++ b/code/headers/gcode_generator.hpp @@ -3,6 +3,7 @@ #include // size_t, (u)intx_t #include #include +#include namespace r2d2::robot_arm { /** @@ -122,6 +123,10 @@ namespace r2d2::robot_arm { return buffer; } + void reset_buffer() { + memset(buffer, 0, sizeof(buffer)); + } + /** * @brief * Appends a string (source) at the end of the buffer (array) @@ -130,9 +135,6 @@ namespace r2d2::robot_arm { * @param source The string that needs to be added to the end of the buffer */ void append(const char *source) { - if (!string_fits(source)) { - return; - } int i = 0; size_t start = get_string_length(buffer); while (source[i]) { diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index a302633..7bc957d 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -274,7 +274,7 @@ namespace r2d2::robot_arm { /** * @brief check posibility of coordinates cartesian polar to gcode - * Makes the check posibility of coordinates cartesian polar gcode command for the uarm + * Makes the gcode command who checks if it's possible to reach the chosen coordinates (cartesian or polar) for the uarm * * @param coordinate cartesian integer coordinate(XYZ) * @param id 0 = cartesian , 1 = polar @@ -301,7 +301,7 @@ namespace r2d2::robot_arm { /** * @brief set current position head to reference position to gcode - * Makes the set current position head to reference position gcode command for the uarm + * Makes the gcode command who set the current position of the head to reference position for the uarm */ void set_current_position_head_to_reference_position_to_gcode(){ this->append("#16 M2401\n"); @@ -316,8 +316,8 @@ namespace r2d2::robot_arm { void init_to_gcode(const uint8_t &on_off){ char on_off_string[2]; this->int_to_string(on_off, on_off_string); - this->append("#17 M2122 V"); - this->append(on_off_string); + this->append("#17 M2122 V1"); + //this->append(on_off_string); this->append("\n"); } diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index db83f0f..431463b 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -108,7 +108,7 @@ namespace r2d2::robot_arm { /** * @brief attach all joint motors - * This setting function attach all the joint motors. + * This function attach all the joint motors.cd */ void attach_all_joint_motors(); diff --git a/code/main.cpp b/code/main.cpp index c5a015b..879a556 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -18,8 +18,46 @@ int main() { // driver code + uarm.attach_all_joint_motors(); + uarm.init(1); + hwlib::wait_ms(500); + //hwlib::cout << "command 1" << hwlib::endl; + //uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, 100, 100), 200); + //uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 100, 100), 200); + // hwlib::wait_ms(2000); //uarm.init(1); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 100, 100),90); + //hwlib::cout << "command 2" << hwlib::endl; + for(int i = 1; i < 4; i++){ + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(150, 90, 50), 1000); + hwlib::wait_ms(2000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(130, 90, 50)); + hwlib::wait_ms(2000); + uarm.move_head_to_polar_coordinate(r2d2::robot_arm::vector3i_c(100, 90, 50), 1000); + hwlib::wait_ms(2000); + uarm.rotate_id_motor_to_degree(0,90); + uarm.rotate_id_motor_to_degree(1,40); + uarm.rotate_id_motor_to_degree(2,90); + hwlib::wait_ms(2000); + uarm.relative_displacement_head(r2d2::robot_arm::vector3i_c(10, 10, 10), 1000); + hwlib::wait_ms(2000); + uarm.relative_displacement_polar_head(r2d2::robot_arm::vector3i_c(10, 10, 10), 1000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(132, 30, 105), 1000); + hwlib::wait_ms(10000); + uarm.detach_all_joint_motors(); + hwlib::wait_ms(10000); + uarm.attach_all_joint_motors(); + hwlib::wait_ms(5000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(150, 90, 50), 1000); + hwlib::wait_ms(10000); + uarm.detach_motor_by_id(2); + hwlib::wait_ms(10000); + uarm.attach_motor_by_id(2); + hwlib::wait_ms(5000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 90, 50), 1000); + hwlib::wait_ms(2000); + uarm.set_buzzer(1000,1000); + hwlib::wait_ms(10000); + } //uarm.rotate_id_motor_to_degree(0,60); //hwlib::wait_ms(20000); //uarm.move_head_to_polar_coordinate(r2d2::robot_arm::vector3i_c(200, 200, 200),1000); diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index 70b9326..f9ee808 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -12,12 +12,14 @@ namespace r2d2::robot_arm { const uint16_t &speed) { gcode_generator.coordinate_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate) { gcode_generator.coordinate_to_gcode(coordinate, default_speed); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } @@ -25,6 +27,7 @@ namespace r2d2::robot_arm { const uint16_t &speed){ gcode_generator.polar_coordinate_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } @@ -32,18 +35,21 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::rotate_id_motor_to_degree(const uint8_t &id, const uint16_t °ree){ gcode_generator.rotate_id_motor_degree_to_gcode(id, degree); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::relative_displacement_head(const vector3i_c &coordinate, const uint16_t &speed){ gcode_generator.relative_displacement_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::relative_displacement_polar_head(const vector3i_c &coordinate, const uint16_t &speed){ gcode_generator.relative_displacement_polar_to_gcode(coordinate, speed); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } @@ -54,72 +60,84 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::init(const uint8_t &on_off) { gcode_generator.init_to_gcode(on_off); this->send_command(gcode_generator.get_buffer()); // report when stop + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::attach_all_joint_motors() { gcode_generator.attach_all_joint_motors_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::detach_all_joint_motors() { gcode_generator.detach_all_joint_motors_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::return_cartesian_coordinates_by_speed(const uint16_t &speed){ gcode_generator.return_cartesian_coordinates_by_speed_to_gcode(speed); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::attach_motor_by_id(const uint8_t &id){ gcode_generator.attach_motor_by_id_to_gcode(id); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::detach_motor_by_id(const uint8_t &id){ gcode_generator.detach_motor_by_id_to_gcode(id); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::check_attached_motor_by_id(const uint8_t &id){ gcode_generator.check_attached_motor_by_id_to_gcode(id); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::set_buzzer(const uint16_t &frequence, const uint16_t &time){ gcode_generator.set_buzzer_to_gcode(frequence,time); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::convert_coordinates_to_angle_of_joints(const vector3i_c &coordinate){ gcode_generator.convert_coordinates_to_angle_of_joints_to_gcode(coordinate); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::convert_angle_of_joints_to_coordinates(const vector3i_c &angle){ gcode_generator.convert_angle_of_joints_to_coordinates_to_gcode(angle); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::check_posibility_of_coordinates_cartesian_polar(const vector3i_c &coordinate, const uint8_t &id){ gcode_generator.check_posibility_of_coordinates_cartesian_polar_to_gecode(coordinate, id); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::set_current_position_head_to_reference_position(){ gcode_generator.set_current_position_head_to_reference_position_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } @@ -129,12 +147,14 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::get_current_angle_of_joints(){ gcode_generator.get_current_angle_of_joints_to_gecode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::get_device_name(){ gcode_generator.get_device_name_to_gecode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } @@ -147,48 +167,56 @@ namespace r2d2::robot_arm { void uarm_swift_pro_c::get_software_version(){ gcode_generator.get_software_version_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::get_API_version(){ gcode_generator.get_API_version_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::get_UID(){ gcode_generator.get_UID_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::get_angle_of_joint_id(const uint8_t &id){ gcode_generator.get_angle_of_joint_id_to_gcode(id); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::get_current_coordinates(){ gcode_generator.get_current_coordinate_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::get_current_polar_coordinates(){ gcode_generator.get_current_polar_coordinates_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::get_status_power_connection(){ gcode_generator.get_status_power_connection_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } void uarm_swift_pro_c::check_current_status(){ gcode_generator.check_current_status_to_gcode(); this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); this->debug(); } @@ -201,10 +229,12 @@ namespace r2d2::robot_arm { debug = this->usart_bus.getc(); hwlib::cout << debug; } + } bool uarm_swift_pro_c::send_command(const char *command) { this->usart_bus << command; + hwlib::cout << "send_command: " < Date: Wed, 3 Jul 2019 10:04:55 +0200 Subject: [PATCH 36/37] add the demo for 3 July and add some end effectors --- code/headers/uarm_gcode_generator.hpp | 8 ++ code/headers/uarm_swift_pro.hpp | 3 + code/main.cpp | 128 +++++++++++++++++++------- code/src/uarm_swift_pro.cpp | 14 +++ 4 files changed, 122 insertions(+), 31 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 7bc957d..072704c 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -307,6 +307,14 @@ namespace r2d2::robot_arm { this->append("#16 M2401\n"); } + void switch_uart2_uart0(){ + this->append("#0 M2500\n"); + } + + void gripper_close_open(){ + this->append("#30 M2232 V1\n"); + } + /** * @brief Initialization to gcode * Makes the initialization gcode command for the uarm diff --git a/code/headers/uarm_swift_pro.hpp b/code/headers/uarm_swift_pro.hpp index 431463b..3648abd 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -261,6 +261,9 @@ namespace r2d2::robot_arm { */ void check_current_status(); + void switch_uart2_uart0(); + + void gripper_close_open(); /** * @brief Initialization * This function is for actions that need to be executed at diff --git a/code/main.cpp b/code/main.cpp index 879a556..2f72b65 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -14,6 +14,7 @@ int main() { r2d2::robot_arm::uarm_swift_pro_c uarm(usart, comm); + uarm.switch_uart2_uart0(); @@ -27,37 +28,102 @@ int main() { // hwlib::wait_ms(2000); //uarm.init(1); //hwlib::cout << "command 2" << hwlib::endl; - for(int i = 1; i < 4; i++){ - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(150, 90, 50), 1000); - hwlib::wait_ms(2000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(130, 90, 50)); - hwlib::wait_ms(2000); - uarm.move_head_to_polar_coordinate(r2d2::robot_arm::vector3i_c(100, 90, 50), 1000); - hwlib::wait_ms(2000); - uarm.rotate_id_motor_to_degree(0,90); - uarm.rotate_id_motor_to_degree(1,40); - uarm.rotate_id_motor_to_degree(2,90); - hwlib::wait_ms(2000); - uarm.relative_displacement_head(r2d2::robot_arm::vector3i_c(10, 10, 10), 1000); - hwlib::wait_ms(2000); - uarm.relative_displacement_polar_head(r2d2::robot_arm::vector3i_c(10, 10, 10), 1000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(132, 30, 105), 1000); - hwlib::wait_ms(10000); - uarm.detach_all_joint_motors(); - hwlib::wait_ms(10000); - uarm.attach_all_joint_motors(); - hwlib::wait_ms(5000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(150, 90, 50), 1000); - hwlib::wait_ms(10000); - uarm.detach_motor_by_id(2); - hwlib::wait_ms(10000); - uarm.attach_motor_by_id(2); - hwlib::wait_ms(5000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 90, 50), 1000); - hwlib::wait_ms(2000); - uarm.set_buzzer(1000,1000); - hwlib::wait_ms(10000); - } + + + + //----------------------------------DEMO----------------------------------------// + // DOTS + // int x = 150; + // int y = 50; + // for(int i = 0; i < 10; i++){ + // x = 150; + // for (int i = 0; i < 10; i++){ + // x += 10; + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 3000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 50), 3000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 3000); + // } + // y -= 10; + // //for(int i = 200; i > 0; i -= 10){ + // //} + + // } + uarm.gripper_close_open(); + hwlib::wait_ms(1000); + //----------------------------------DEMO----------------------------------------// + + //smile:) + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, 50, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, 50, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(170, 30, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(170, -20, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, -40, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, -40, 75), 3000); + + + // Eye see you + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 30, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 30, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 10, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 10, 75), 3000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 0, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 0, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -20, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -20, 75), 3000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 20, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 20, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 20, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 20, 75), 3000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -10, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -10, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -10, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -10, 75), 3000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 30, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 30, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 10, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 10, 75), 3000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 0, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 0, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -20, 50), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -20, 75), 3000); + + //----------------------------------TEST---------------------------------------// + + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(130, 90, 50)); + // hwlib::wait_ms(2000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(170, 90, 50)); + // hwlib::wait_ms(2000); + // uarm.move_head_to_polar_coordinate(r2d2::robot_arm::vector3i_c(100, 90, 50), 1000); + // hwlib::wait_ms(2000); + // uarm.rotate_id_motor_to_degree(0,90); + // uarm.rotate_id_motor_to_degree(1,40); + // uarm.rotate_id_motor_to_degree(2,90); + // hwlib::wait_ms(2000); + // uarm.relative_displacement_head(r2d2::robot_arm::vector3i_c(10, 10, 10), 1000); + // hwlib::wait_ms(2000); + // uarm.relative_displacement_polar_head(r2d2::robot_arm::vector3i_c(10, 10, 10), 1000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(132, 30, 105), 1000); + // hwlib::wait_ms(10000); + // uarm.detach_all_joint_motors(); + // hwlib::wait_ms(10000); + // uarm.attach_all_joint_motors(); + // hwlib::wait_ms(5000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(150, 90, 50), 1000); + // hwlib::wait_ms(10000); + // uarm.detach_motor_by_id(2); + // hwlib::wait_ms(10000); + // uarm.attach_motor_by_id(2); + // hwlib::wait_ms(5000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(100, 90, 50), 1000); + // hwlib::wait_ms(2000); + // uarm.set_buzzer(1000,1000); + // hwlib::wait_ms(10000); + //uarm.rotate_id_motor_to_degree(0,60); //hwlib::wait_ms(20000); //uarm.move_head_to_polar_coordinate(r2d2::robot_arm::vector3i_c(200, 200, 200),1000); diff --git a/code/src/uarm_swift_pro.cpp b/code/src/uarm_swift_pro.cpp index f9ee808..9e8bbfe 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -141,6 +141,20 @@ namespace r2d2::robot_arm { this->debug(); } + void uarm_swift_pro_c::switch_uart2_uart0(){ + gcode_generator.switch_uart2_uart0(); + this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); + this->debug(); + + } + + void uarm_swift_pro_c::gripper_close_open(){ + gcode_generator.gripper_close_open(); + this->send_command(gcode_generator.get_buffer()); + gcode_generator.reset_buffer(); + this->debug(); + } //-------------------QUERYING METHODS------------------// From b43efe505a2c38037ee46c017523ee37dec48bce Mon Sep 17 00:00:00 2001 From: Dylan van Eck Date: Thu, 4 Jul 2019 15:02:28 +0200 Subject: [PATCH 37/37] add some demo code --- code/headers/uarm_gcode_generator.hpp | 2 +- code/main.cpp | 111 ++++++++++++++++---------- 2 files changed, 69 insertions(+), 44 deletions(-) diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 072704c..bb876ee 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -312,7 +312,7 @@ namespace r2d2::robot_arm { } void gripper_close_open(){ - this->append("#30 M2232 V1\n"); + this->append("#30 M2232 V0\n"); } /** diff --git a/code/main.cpp b/code/main.cpp index 2f72b65..8abd3a4 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -33,65 +33,90 @@ int main() { //----------------------------------DEMO----------------------------------------// // DOTS - // int x = 150; - // int y = 50; + while(true){ + int x = 150; + int y = 50; // for(int i = 0; i < 10; i++){ // x = 150; // for (int i = 0; i < 10; i++){ // x += 10; - // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 3000); - // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 50), 3000); - // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 3000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 6000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 50), 6000); + // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 6000); // } // y -= 10; // //for(int i = 200; i > 0; i -= 10){ // //} // } - uarm.gripper_close_open(); - hwlib::wait_ms(1000); + + x = 150; + y = 50; + for(int i = 0; i < 10; i++){ + x = 150; + for (int i = 0; i < 10; i++){ + x += 10; + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 50), 6000); + x -= 10; + y += 5; + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(x, y, 75), 6000); + y -= 5; + x += 10; + + } + y -= 10; + //for(int i = 200; i > 0; i -= 10){ + //} + + } + //----------------------------------DEMO----------------------------------------// + + // uarm.gripper_close_open(); + // hwlib::wait_ms(1000); //smile:) - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, 50, 75), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, 50, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(170, 30, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(170, -20, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, -40, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, -40, 75), 3000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, 50, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, 50, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(170, 30, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(170, -20, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, -40, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(190, -40, 75), 6000); // Eye see you - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 30, 75), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 30, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 10, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 10, 75), 3000); - - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 0, 75), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 0, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -20, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -20, 75), 3000); - - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 20, 75), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 20, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 20, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 20, 75), 3000); - - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -10, 75), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -10, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -10, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -10, 75), 3000); - - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 30, 75), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 30, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 10, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 10, 75), 3000); - - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 0, 75), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 0, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -20, 50), 3000); - uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -20, 75), 3000); - + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 30, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 30, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 10, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 10, 75), 6000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 0, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 0, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -20, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -20, 75), 6000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 20, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, 20, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 20, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 20, 75), 6000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -10, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(200, -10, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -10, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -10, 75), 6000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 30, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 30, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 10, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 10, 75), 6000); + + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 0, 75), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, 0, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -20, 50), 6000); + uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(240, -20, 75), 6000); + } //----------------------------------TEST---------------------------------------// // uarm.move_head_to_coordinate(r2d2::robot_arm::vector3i_c(130, 90, 50));