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/dof4_diy.hpp b/code/headers/dof4_diy.hpp index bb91b02..307e094 100644 --- a/code/headers/dof4_diy.hpp +++ b/code/headers/dof4_diy.hpp @@ -27,13 +27,6 @@ namespace r2d2::robot_arm { * @param speed The speed the head moves at */ void move_head_to_coordinate(const vector3i_c &coordinate, - uint16_t speed) override; - /** - * @brief - * This function rotates the head of the dof4 diy robot arm. - * - * @param rotation The rotational position the head needs to move to, in degrees. - */ - void rotate_head(int16_t rotation) override; + const uint16_t &speed) override; }; } // namespace r2d2::robot_arm diff --git a/code/headers/gcode_generator.hpp b/code/headers/gcode_generator.hpp index ddd30c9..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]) { @@ -176,4 +178,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 22a9616..4d172c6 100644 --- a/code/headers/robot_arm_interface.hpp +++ b/code/headers/robot_arm_interface.hpp @@ -2,14 +2,30 @@ #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(); + const auto data = frame.as_frame_type(); + move_head_to_coordinate(vector3i_c(data.x, data.y, data.z), data.speed); + + } + } /** * @brief * This function moves the robot arm head to a certain 3d location @@ -19,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 @@ -29,12 +45,5 @@ namespace r2d2::robot_arm { */ virtual void move_head_to_coordinate(const vector3i_c &coordinate) = 0; - /** - * @brief - * This function rotates the head of the a robot arm. - * - * @param rotation The rotational position the head needs to move to in degrees - */ - virtual void rotate_head(int16_t rotation) = 0; }; }; // namespace r2d2::robot_arm \ No newline at end of file diff --git a/code/headers/uarm_gcode_generator.hpp b/code/headers/uarm_gcode_generator.hpp index 3cde367..bb876ee 100644 --- a/code/headers/uarm_gcode_generator.hpp +++ b/code/headers/uarm_gcode_generator.hpp @@ -1,4 +1,5 @@ -#include +#pragma once + #include @@ -6,17 +7,91 @@ namespace r2d2::robot_arm { template class uarm_gcode_generator_c : public gcode_generator_c { public: + /** - * @brief - * 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 coordinate The coordinate that needs to be converted - * @param speed The speed that needs to be used in the gcode + * @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 + 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("#1 G0 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"); + } + + /** + * @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 coordinate_to_gcode(const vector3i_c &coordinate, - const uint16_t &speed) { + 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]; + 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("#2 G2201 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"); + } + + /** + * @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]; + this->int_to_string(id, id_string); + this->int_to_string(degree, degree_string); + this->append("#3 G2202 N"); + this->append(id_string); + this->append(" V"); + this->append(degree_string); + 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]; char z_string[11]; @@ -25,7 +100,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("#4 G2204 X"); this->append(x_string); this->append(" Y"); this->append(y_string); @@ -35,5 +110,318 @@ namespace r2d2::robot_arm { this->append(speed_string); 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]; + 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("#5 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"); + } + + /** + * @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); + this->append("#8 M2120 V"); + this->append(speed_string); + 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); + this->append("#9 M2201 N"); + this->append(id_string); + 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); + this->append("#10 M2202 N"); + this->append(id_string); + 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); + this->append("#11 M2203 N"); + this->append(id_string); + 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]; + this->int_to_string(frequency, frequency_string); + this->int_to_string(time, time_string); + this->append("#12 M2210 F"); + this->append(frequency_string); + this->append(" T"); + this->append(time_string); + 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]; + 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"); + } + + /** + * @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]; + 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"); + } + + /** + * @brief check posibility of coordinates cartesian polar to gcode + * 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 + */ + 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"); + } + + /** + * @brief set current position head to reference position to gcode + * 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"); + } + + void switch_uart2_uart0(){ + this->append("#0 M2500\n"); + } + + void gripper_close_open(){ + this->append("#30 M2232 V0\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); + this->append("#17 M2122 V1"); + //this->append(on_off_string); + 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("#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("#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("#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("#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("#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("#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("#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("#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("#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("#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("#28 P2400\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 a177f28..3648abd 100644 --- a/code/headers/uarm_swift_pro.hpp +++ b/code/headers/uarm_swift_pro.hpp @@ -12,27 +12,25 @@ */ 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 + * @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); + 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 @@ -41,6 +39,7 @@ namespace r2d2::robot_arm { * @return false means connection is wrong */ bool check_connection(); + /** * @brief Sending command to arm * Sends false as default @@ -50,39 +49,234 @@ namespace r2d2::robot_arm { * @return true when command correctly sent */ bool send_command(const char *command); + /** - * @brief - * Move specific joint of the arm - * Moves the robot arm joint [joint_id] to a given angle in degrees limited from 0 to 180 - * - * @param joint_id the id of the joint that needs to be moved - * @param angle the angle at which the joint needs to be - */ - void move_joint(const int &joint_id, const int &angle); - /** - * @brief - * 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, - uint16_t speed) override; + void move_head_to_coordinate(const vector3i_c &coordinate, const uint16_t &speed) override; + /** - * @brief - * 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 the coordinate the head needs to move to + * @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) + * @param speed movement speed in mm/min + */ + + 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 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 function attach all the joint motors.cd + */ + void attach_all_joint_motors(); + + /** + * @brief de/tach 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 - * This function rotates the head of the uarm swift pro robot arm. + * @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) + * + * @param id id of joints (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(); + + void switch_uart2_uart0(); + + void gripper_close_open(); + /** + * @brief Initialization + * This function is for actions that need to be executed at + * initialization of the object * - * @param rotation The rotational position the head needs to move to, in degrees, limited from 0 to 180 + * @parram on_off to set the uarm on or off + */ + void init(const uint8_t &on_off); + + /** + * @brief debug + * This function will write the feedback of the uarm to hwlib::cout */ - void rotate_head(int16_t rotation) override; + void debug(); }; } // namespace r2d2::robot_arm diff --git a/code/main.cpp b/code/main.cpp index 5404e95..8abd3a4 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -1,18 +1,157 @@ #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); + + uarm.switch_uart2_uart0(); + + // 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.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); + //hwlib::cout << "command 2" << hwlib::endl; + + + + //----------------------------------DEMO----------------------------------------// + // DOTS + 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), 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){ + // //} + + // } + + 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), 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), 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)); + // 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); // 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/dof4_diy.cpp b/code/src/dof4_diy.cpp index 3b17eef..ddbc48d 100644 --- a/code/src/dof4_diy.cpp +++ b/code/src/dof4_diy.cpp @@ -5,10 +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 } - void dof4_diy_c::rotate_head(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 7396f86..9e8bbfe 100644 --- a/code/src/uarm_swift_pro.cpp +++ b/code/src/uarm_swift_pro.cpp @@ -1,42 +1,255 @@ #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) { } - void uarm_swift_pro_c::move_head_to_coordinate(const vector3i_c &coordinate, - 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()); + gcode_generator.reset_buffer(); + this->debug(); } - 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()); + gcode_generator.reset_buffer(); + this->debug(); } - void uarm_swift_pro_c::rotate_head(int16_t rotation) { - // TODO impl rotate head uarm swift pro + 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()); + gcode_generator.reset_buffer(); + this->debug(); + } - void uarm_swift_pro_c::init() { - this->send_command("#n M2122 V1\n"); // report when stop + 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(); } - bool uarm_swift_pro_c::check_connection() { - // TODO impl check_connection() - return true; + 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(); + } + + + + //------------------SETTING COMMANDS------------------// + + 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(); + } + + 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------------------// + + 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(); + } + + 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()); + 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(); + } + + + //------------------ESSENTIAL METHODS------------------// + + 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) { this->usart_bus << command; + hwlib::cout << "send_command: " < +#include #define CATCH_CONFIG_MAIN #include +#include +#include TEST_CASE("Appending gone wrong!", "Testing generator") { r2d2::robot_arm::uarm_gcode_generator_c<2> generator; @@ -35,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 } @@ -170,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