diff --git a/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_workbench.h b/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_workbench.h index 7de05df18..fd18865c7 100644 --- a/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_workbench.h +++ b/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_workbench.h @@ -27,7 +27,7 @@ class DynamixelWorkbench : public DynamixelDriver DynamixelWorkbench(); ~DynamixelWorkbench(); - bool torque(uint8_t id, bool onoff, const char **log = NULL); + bool torque(uint8_t id, int32_t onoff, const char **log = NULL); bool torqueOn(uint8_t id, const char **log = NULL); bool torqueOff(uint8_t id, const char **log = NULL); @@ -38,7 +38,7 @@ class DynamixelWorkbench : public DynamixelDriver bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); bool itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log = NULL); - bool led(uint8_t id, bool onoff, const char **log = NULL); + bool led(uint8_t id, int32_t onoff, const char **log = NULL); bool ledOn(uint8_t id, const char **log = NULL); bool ledOff(uint8_t id, const char **log = NULL); diff --git a/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp b/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp index 9289b8aa7..7c500f058 100644 --- a/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp +++ b/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp @@ -569,6 +569,12 @@ bool DynamixelDriver::writeRegister(uint8_t id, uint16_t address, uint16_t lengt { ErrorFromSDK sdk_error = {0, false, false, 0}; +#if defined(__OPENCR__) || defined(__OPENCM904__) + delay(50); +#else + usleep(1000*50); +#endif + sdk_error.dxl_comm_result = packetHandler_->writeTxRx(portHandler_, id, address, @@ -610,6 +616,12 @@ bool DynamixelDriver::writeRegister(uint8_t id, const char *item_name, int32_t d uint16_t data_2_byte = (uint16_t)data; uint32_t data_4_byte = (uint32_t)data; +#if defined(__OPENCR__) || defined(__OPENCM904__) + delay(10); +#else + usleep(1000*10); +#endif + switch (control_item->data_length) { case BYTE: @@ -668,6 +680,12 @@ bool DynamixelDriver::writeOnlyRegister(uint8_t id, uint16_t address, uint16_t l { ErrorFromSDK sdk_error = {0, false, false, 0}; +#if defined(__OPENCR__) || defined(__OPENCM904__) + delay(50); +#else + usleep(1000*50); +#endif + sdk_error.dxl_comm_result = packetHandler_->writeTxOnly(portHandler_, id, address, @@ -699,9 +717,11 @@ bool DynamixelDriver::writeOnlyRegister(uint8_t id, const char *item_name, int32 control_item = tools_[factor].getControlItem(item_name, log); if (control_item == NULL) return false; - uint8_t data_1_byte = (uint8_t)data; - uint16_t data_2_byte = (uint16_t)data; - uint32_t data_4_byte = (uint32_t)data; +#if defined(__OPENCR__) || defined(__OPENCM904__) + delay(50); +#else + usleep(1000*50); +#endif switch (control_item->data_length) { @@ -709,28 +729,28 @@ bool DynamixelDriver::writeOnlyRegister(uint8_t id, const char *item_name, int32 sdk_error.dxl_comm_result = packetHandler_->write1ByteTxOnly(portHandler_, id, control_item->address, - data_1_byte); + (uint8_t)data); break; case WORD: sdk_error.dxl_comm_result = packetHandler_->write2ByteTxOnly(portHandler_, id, control_item->address, - data_2_byte); + (uint16_t)data); break; case DWORD: sdk_error.dxl_comm_result = packetHandler_->write4ByteTxOnly(portHandler_, id, control_item->address, - data_4_byte); + (uint32_t)data); break; default: sdk_error.dxl_comm_result = packetHandler_->write1ByteTxOnly(portHandler_, id, control_item->address, - data_1_byte); + (uint8_t)data); break; } @@ -812,9 +832,9 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d control_item = tools_[factor].getControlItem(item_name, log); if (control_item == NULL) return false; - uint8_t *data_1_byte = (uint8_t *)&data; - uint16_t *data_2_byte = (uint16_t*)&data; - uint32_t *data_4_byte = (uint32_t*)&data; + uint8_t data_1_byte = 0; + uint16_t data_2_byte = 0; + uint32_t data_4_byte = 0; switch (control_item->data_length) { @@ -822,7 +842,7 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d sdk_error.dxl_comm_result = packetHandler_->read1ByteTxRx(portHandler_, id, control_item->address, - data_1_byte, + &data_1_byte, &sdk_error.dxl_error); break; @@ -830,7 +850,7 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d sdk_error.dxl_comm_result = packetHandler_->read2ByteTxRx(portHandler_, id, control_item->address, - data_2_byte, + &data_2_byte, &sdk_error.dxl_error); break; @@ -838,7 +858,7 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d sdk_error.dxl_comm_result = packetHandler_->read4ByteTxRx(portHandler_, id, control_item->address, - data_4_byte, + &data_4_byte, &sdk_error.dxl_error); break; @@ -846,7 +866,7 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d sdk_error.dxl_comm_result = packetHandler_->read1ByteTxRx(portHandler_, id, control_item->address, - data_1_byte, + &data_1_byte, &sdk_error.dxl_error); break; } @@ -863,6 +883,25 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d } else { + switch (control_item->data_length) + { + case BYTE: + *data = data_1_byte; + break; + + case WORD: + *data = data_2_byte; + break; + + case DWORD: + *data = data_4_byte; + break; + + default: + *data = data_1_byte; + break; + } + if (log != NULL) *log = "[DynamixelDriver] Succeeded to read!"; return true; } diff --git a/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp b/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp index c8dfafea0..bf176f16f 100644 --- a/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp +++ b/arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp @@ -37,7 +37,7 @@ DynamixelWorkbench::DynamixelWorkbench(){} DynamixelWorkbench::~DynamixelWorkbench(){} -bool DynamixelWorkbench::torque(uint8_t id, bool onoff, const char **log) +bool DynamixelWorkbench::torque(uint8_t id, int32_t onoff, const char **log) { bool result = false; @@ -56,7 +56,7 @@ bool DynamixelWorkbench::torqueOn(uint8_t id, const char **log) { bool result = false; - result = torque(id, true, log); + result = torque(id, 1, log); return result; } @@ -65,7 +65,7 @@ bool DynamixelWorkbench::torqueOff(uint8_t id, const char **log) { bool result = false; - result = torque(id, false, log); + result = torque(id, 0, log); return result; } @@ -252,11 +252,11 @@ bool DynamixelWorkbench::itemRead(uint8_t id, const char *item_name, int32_t *da return readRegister(id, item_name, data, log); } -bool DynamixelWorkbench::led(uint8_t id, bool onoff, const char **log) +bool DynamixelWorkbench::led(uint8_t id, int32_t onoff, const char **log) { bool result = false; - result = writeRegister(id, "LED", (int32_t)onoff, log); + result = writeRegister(id, "LED", onoff, log); if (result == false) { if (log != NULL) *log = "[DynamixelWorkbench] Failed to change led status!"; @@ -271,7 +271,7 @@ bool DynamixelWorkbench::ledOn(uint8_t id, const char **log) { bool result = false; - result = led(id, true, log); + result = led(id, 1, log); return result; } @@ -280,7 +280,7 @@ bool DynamixelWorkbench::ledOff(uint8_t id, const char **log) { bool result = false; - result = led(id, false, log); + result = led(id, 0, log); return result; } diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.cpp b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.cpp index 8175d222a..fdc58e718 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.cpp +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.cpp @@ -133,7 +133,7 @@ bool OpenManipulatorDriver::init(uint8_t *joint_id, uint8_t joint_cnt, uint8_t * result = dxl_wb_.torqueOff(joint_id[num], &log); if (result == true) DEBUG_SERIAL.println("Succeeded to set torque off"); - result = dxl_wb_.setVelocityBasedProfile(joint_id[num], &log); + result = dxl_wb_.setTimeBasedProfile(joint_id[num], &log); if (result == true) DEBUG_SERIAL.println("Succeeded to set velocity based profile mode"); } @@ -183,12 +183,12 @@ bool OpenManipulatorDriver::init(uint8_t *joint_id, uint8_t joint_cnt, uint8_t * } } - double init_joint_position[4] = {0.0, -1.57, 1.37, 0.2258}; + double init_joint_position[4] = {0.0, -1.57, 1.20, 0.6}; double init_gripper_position[1] = {0.0}; - writeJointProfileControlParam(3.0f); - writeJointPosition(init_joint_position); + writeJointProfileControlParam(3.0f, 0.75f); writeGripperProfileControlParam(0.0f); + writeJointPosition(init_joint_position); writeGripperPosition(init_gripper_position); writeJointProfileControlParam(0.0f); @@ -457,7 +457,7 @@ bool OpenManipulatorDriver::getCurrent(double *get_data) return true; } -bool OpenManipulatorDriver::writeJointProfileControlParam(int32_t set_time) +bool OpenManipulatorDriver::writeJointProfileControlParam(double set_time, double acc) { const char *log; bool result = false; @@ -466,7 +466,7 @@ bool OpenManipulatorDriver::writeJointProfileControlParam(int32_t set_time) for (uint8_t num = 0; num < joint_.cnt * 2; num = num + 2) { - goal_data[num] = (set_time * 1000) / 4; + goal_data[num] = acc * 1000; goal_data[num+1] = set_time * 1000; } @@ -476,7 +476,7 @@ bool OpenManipulatorDriver::writeJointProfileControlParam(int32_t set_time) if (result == false) { DEBUG_SERIAL.println(log); - DEBUG_SERIAL.println("Failed to sync write position"); + DEBUG_SERIAL.println("Failed to sync write param for profile"); } } else @@ -509,7 +509,7 @@ bool OpenManipulatorDriver::writeJointPosition(double *set_data) return true; } -bool OpenManipulatorDriver::writeGripperProfileControlParam(int32_t set_time) +bool OpenManipulatorDriver::writeGripperProfileControlParam(double set_time) { const char *log; bool result = false; @@ -528,7 +528,7 @@ bool OpenManipulatorDriver::writeGripperProfileControlParam(int32_t set_time) if (result == false) { DEBUG_SERIAL.println(log); - DEBUG_SERIAL.println("Failed to sync write position"); + DEBUG_SERIAL.println("Failed to sync write param for profile"); } } else diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.h index ef6800f25..014f92ff3 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.h +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.h @@ -56,9 +56,9 @@ class OpenManipulatorDriver bool getVelocity(double *get_data); bool getCurrent(double *get_data); bool writeJointPosition(double *set_data); - bool writeJointProfileControlParam(int32_t set_time); + bool writeJointProfileControlParam(double set_time, double acc = 0.0f); bool writeGripperPosition(double *set_data); - bool writeGripperProfileControlParam(int32_t set_time); + bool writeGripperProfileControlParam(double set_time); private: DynamixelWorkbench dxl_wb_; diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core.ino b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core.ino index 3a3fb67ab..90da23175 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core.ino +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core.ino @@ -126,6 +126,12 @@ void loop() } #endif + if ((t-tTime[6]) >= (1000 / JOINT_CONTROL_FREQEUNCY)) + { + jointControl(); + tTime[6] = t; + } + // Send log message after ROS connection sendLogMsg(); @@ -173,25 +179,13 @@ void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg) /******************************************************************************* * Callback function for joint trajectory msg *******************************************************************************/ -void jointTrajectoryCallback(const trajectory_msgs::JointTrajectory& joint_trajectory_msg) +void jointTrajectoryPointCallback(const std_msgs::Float64MultiArray& joint_trajectory_point_msg) { - // trajectory_msgs::JointTrajectoryPoint jnt_tra_point_msg; - - // for (uint8_t id_num = 0; id_num < id_cnt; id_num++) - // { - // jnt_tra_point_msg.positions.push_back(goal[id_num].position); - // jnt_tra_point_msg.velocities.push_back(goal[id_num].velocity); - // jnt_tra_point_msg.accelerations.push_back(goal[id_num].acceleration); - // } - - // jnt_tra_msg_->points.push_back(jnt_tra_point_msg); - - // double goal_joint_position[20]; - - // for (int index = 0; index < joint_cnt; index++) - // goal_joint_position[index] = pos_msg.data[index]; - - // manipulator_driver.writeJointPosition(goal_joint_position); + if (is_moving == false) + { + joint_trajectory_point = joint_trajectory_point_msg; + is_moving = true; + } } /******************************************************************************* @@ -210,10 +204,10 @@ void jointMoveTimeCallback(const std_msgs::Float64& time_msg) void gripperPositionCallback(const std_msgs::Float64MultiArray& gripper_msg) { double goal_gripper_position[5] = {0.0, }; - const double open_manipulator_gripper_offset = -0.015f; + const double OPEN_MANIPULATOR_GRIPPER_OFFSET = -0.015f; for (int index = 0; index < gripper_cnt; index++) - goal_gripper_position[index] = gripper_msg.data[index] / open_manipulator_gripper_offset; + goal_gripper_position[index] = gripper_msg.data[index] / OPEN_MANIPULATOR_GRIPPER_OFFSET; manipulator_driver.writeGripperPosition(goal_gripper_position); } @@ -487,7 +481,7 @@ void updateJointStates(void) static float joint_states_vel[20] = {0.0, }; static float joint_states_eff[20] = {0.0, }; - const double open_manipulator_gripper_offset = -0.015f; + const double OPEN_MANIPULATOR_GRIPPER_OFFSET = -0.015f; double get_joint_position[joint_cnt + gripper_cnt]; double get_joint_velocity[joint_cnt + gripper_cnt]; @@ -507,7 +501,7 @@ void updateJointStates(void) for (uint8_t num = 0; num < (joint_cnt + gripper_cnt); num++) { if (num > joint_cnt) - get_joint_position[num] = get_joint_position[num] * open_manipulator_gripper_offset; + get_joint_position[num] = get_joint_position[num] * OPEN_MANIPULATOR_GRIPPER_OFFSET; joint_states_pos[WHEEL_NUM + num] = get_joint_position[num]; joint_states_vel[WHEEL_NUM + num] = get_joint_velocity[num]; @@ -631,6 +625,71 @@ bool calcOdometry(double diff_time) return true; } +/******************************************************************************* +* Manipulator's joint control +*******************************************************************************/ +void jointControl(void) +{ + const uint8_t POINT_SIZE = joint_cnt + 1; // Add time parameter + const double JOINT_CONTROL_PERIOD = 1.0f / (double)JOINT_CONTROL_FREQEUNCY; + static uint32_t points = 0; + + static uint8_t wait_for_write = 0; + static uint8_t loop_cnt = 0; + + if (is_moving == true) + { + uint32_t all_points_cnt = joint_trajectory_point.data_length; + uint8_t write_cnt = 0; + + if (loop_cnt < (wait_for_write)) + { + loop_cnt++; + return; + } + else + { + double goal_joint_position[joint_cnt]; + double move_time = 0.0f; + + if (points == 0) move_time = joint_trajectory_point.data[points + POINT_SIZE] - joint_trajectory_point.data[points]; + else if ((points + POINT_SIZE) >= all_points_cnt) move_time = joint_trajectory_point.data[points] / 2.0f; + else move_time = joint_trajectory_point.data[points] - joint_trajectory_point.data[points - POINT_SIZE]; + + for (uint32_t positions = points + 1; positions < (points + POINT_SIZE); positions++) + { + if ((points + POINT_SIZE) >= all_points_cnt) + { + goal_joint_position[write_cnt] = joint_trajectory_point.data[positions]; + } + else + { + double offset = 2.0f * (joint_trajectory_point.data[positions + POINT_SIZE] - joint_trajectory_point.data[positions]); + goal_joint_position[write_cnt] = joint_trajectory_point.data[positions] + offset; + } + write_cnt++; + } + + manipulator_driver.writeJointProfileControlParam(move_time * 2.0f); + manipulator_driver.writeJointPosition(goal_joint_position); + + wait_for_write = move_time / JOINT_CONTROL_PERIOD; + points = points + POINT_SIZE; + + if (points >= all_points_cnt) + { + points = 0; + wait_for_write = 0; + is_moving = false; + } + else + { + loop_cnt = 0; + } + } + } +} + /******************************************************************************* * Turtlebot3 test drive using push buttons *******************************************************************************/ diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h index bda88e17d..5c248bfb7 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h @@ -26,8 +26,6 @@ #include #include #include -#include -#include #include #include #include @@ -47,7 +45,7 @@ #include -#define FIRMWARE_VER "1.3.0" +#define FIRMWARE_VER "2.0.0" #define CONTROL_MOTOR_SPEED_FREQUENCY 30 //hz #define IMU_PUBLISH_FREQUENCY 200 //hz @@ -55,6 +53,7 @@ #define DRIVE_INFORMATION_PUBLISH_FREQUENCY 30 //hz #define VERSION_INFORMATION_PUBLISH_FREQUENCY 1 //hz #define DEBUG_LOG_FREQUENCY 10 //hz +#define JOINT_CONTROL_FREQEUNCY 100 //hz #define WHEEL_NUM 2 @@ -77,7 +76,7 @@ // Callback function prototypes void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg); -void jointTrajectoryCallback(const trajectory_msgs::JointTrajectory& joint_trajectory_msg); +void jointTrajectoryPointCallback(const std_msgs::Float64MultiArray& joint_trajectory_point_msg); void jointMoveTimeCallback(const std_msgs::Float64& time_msg); void gripperPositionCallback(const std_msgs::Float64MultiArray& pos_msg); void gripperMoveTimeCallback(const std_msgs::Float64& time_msg); @@ -112,6 +111,8 @@ void initJointStates(void); bool calcOdometry(double diff_time); +void jointControl(void); + void sendLogMsg(void); void waitForSerialLink(bool isConnected); @@ -143,7 +144,7 @@ char joint_state_header_frame_id[30]; *******************************************************************************/ ros::Subscriber cmd_vel_sub("cmd_vel", commandVelocityCallback); -ros::Subscriber joint_position_sub("joint_trajectory", jointTrajectoryCallback); +ros::Subscriber joint_position_sub("joint_trajectory_point", jointTrajectoryPointCallback); ros::Subscriber joint_move_time_sub("joint_move_time", jointMoveTimeCallback); @@ -260,4 +261,10 @@ double odom_vel[3]; bool setup_end = false; uint8_t battery_state = 0; +/******************************************************************************* +* Joint Control +*******************************************************************************/ +bool is_moving = false; +std_msgs::Float64MultiArray joint_trajectory_point; + #endif // TURTLEBOT3_WITH_OPEN_MANIPULATOR_CONFIG_H_