Merge pull request #171 from ROBOTIS-GIT/develop

Develop

.gitignore

@@ -25,3 +25,5 @@
 *.exe
 *.out
 *.app
+
+.vscode

arduino/opencr_arduino/opencr/libraries/OP3/examples/opencr_op3/dxl.cpp

@@ -31,7 +31,6 @@
 //-- External Variables
 //
 
-
 //-- Internal Functions
 //
 static dxl_error_t dxlRxPacketVer2_0(dxl_t *p_packet, uint8_t data_in);
@@ -238,11 +237,15 @@ bool dxlOpenPort(dxl_t *p_packet, uint8_t ch, uint32_t baud)
 
 uint32_t dxlRxAvailable(dxl_t *p_packet)
 {
+  (void)(p_packet);
+
   return dxl_hw_available();
 }
 
 uint8_t dxlRxRead(dxl_t *p_packet)
 {
+  (void)(p_packet);
+
   return dxl_hw_read();
 }
 
@@ -581,7 +584,6 @@ dxl_error_t dxlTxPacket(dxl_t *p_packet)
 
   dxl_hw_write(p_packet->tx.data, p_packet->tx.packet_length);
 
-
 /*
   for(int i=0; i<p_packet->tx.packet_length; i++)
   {

arduino/opencr_arduino/opencr/libraries/OP3/examples/opencr_op3/dxl_hw.cpp

@@ -9,13 +9,8 @@
 
 #include "dxl_hw.h"
 
-
-
-
-
-
-
-
+/* For debug */
+uint32_t tx_led_count, rx_led_count;
 
 /*---------------------------------------------------------------------------
      TITLE   : dxl_hw_begin
@@ -25,6 +20,8 @@ uint32_t dxl_hw_begin(uint8_t baud)
 {
   uint32_t Baudrate = 0;
 
+  pinMode( DXL_LED_RX, OUTPUT);
+  pinMode( DXL_LED_TX, OUTPUT);
 
   pinMode( BDPIN_DXL_PWR_EN, OUTPUT );
   dxl_hw_power_disable();
@@ -122,6 +119,8 @@ void dxl_hw_power_disable(void)
 ---------------------------------------------------------------------------*/
 uint8_t dxl_hw_read(void)
 {
+  rx_led_count = 3;
+
   return DXL_PORT.read();
 }
 
@@ -151,6 +150,8 @@ void dxl_hw_write(uint8_t *p_data, uint32_t length)
   {
     DXL_PORT.write(p_data[i]);
   }
+  DXL_PORT.flush();
+  tx_led_count = 3;
 
   dxl_hw_tx_disable();
 }

arduino/opencr_arduino/opencr/libraries/OP3/examples/opencr_op3/dxl_hw.h

@@ -24,6 +24,8 @@
 #endif
 
 
+#define DXL_LED_RX            BDPIN_LED_USER_1
+#define DXL_LED_TX            BDPIN_LED_USER_2
 
 
 uint32_t dxl_hw_begin(uint8_t baud);

arduino/opencr_arduino/opencr/libraries/OP3/examples/opencr_op3/dxl_node_op3.cpp

@@ -33,7 +33,6 @@ void dxl_node_op3_factory_reset(void);
 void dxl_node_op3_btn_loop(void);
 
 
-
 //-- dxl sp driver function
 //
 dxl_error_t ping(dxl_t *p_dxl);
@@ -54,22 +53,17 @@ static void    dxl_node_write_byte(uint16_t addr, uint8_t data);
 static BOOL dxl_node_check_range(uint16_t addr, uint32_t addr_ptr, uint8_t length);
 void dxl_node_op3_change_baud(void);
 
+static void dxl_node_update_tx_rx_led();
 
 /*---------------------------------------------------------------------------
      TITLE   : dxl_node_op3_init
      WORK    :
 ---------------------------------------------------------------------------*/
 void dxl_node_op3_init(void)
 {
-  uint16_t i;
-
-
   p_dxl_mem = (dxl_mem_op3_t *)&mem.data;
 
 
-  dxl_error_t dxl_err;
-
-
   dxlInit(&dxl_sp, DXL_PACKET_VER_2_0);
 
 
@@ -118,17 +112,11 @@ void dxl_node_op3_init(void)
 ---------------------------------------------------------------------------*/
 void dxl_node_op3_loop(void)
 {
-  static uint32_t tTime[16];
   static uint8_t  gyro_cali_state = 0;
-  uint8_t ret;
   uint8_t i;
-  uint8_t ch;
-
-
-
 
   dxl_process_packet();
-
+  dxl_node_update_tx_rx_led();
 
 
   dxl_hw_op3_update();
@@ -453,9 +441,6 @@ void dxl_node_op3_change_baud(void)
 ---------------------------------------------------------------------------*/
 uint8_t dxl_node_read_byte(uint16_t addr)
 {
-  uint8_t data;
-
-
   if( RANGE_CHECK(addr, p_dxl_mem->Button) )
   {
     p_dxl_mem->Button  = dxl_hw_op3_button_read(PIN_BUTTON_S1)<<0;
@@ -535,7 +520,6 @@ BOOL dxl_node_check_range(uint16_t addr, uint32_t addr_ptr, uint8_t length)
 {
   BOOL ret = FALSE;
   uint32_t addr_offset;
-  uint16_t i;
 
   addr_offset = addr_ptr - (uint32_t)p_dxl_mem;
 
@@ -629,7 +613,6 @@ dxl_error_t read(dxl_t *p_dxl)
   dxl_error_t ret = DXL_RET_OK;
   uint16_t addr;
   uint16_t length;
-  uint16_t i;
   uint8_t data[DXL_MAX_BUFFER];
 
 
@@ -723,7 +706,6 @@ dxl_error_t sync_read(dxl_t *p_dxl)
   uint16_t addr;
   uint16_t length;
   uint8_t  *p_data;
-  uint16_t index;
   uint16_t i;
   uint16_t rx_id_cnt;
   uint8_t data[DXL_MAX_BUFFER];
@@ -869,7 +851,6 @@ dxl_error_t bulk_read(dxl_t *p_dxl)
   uint16_t addr;
   uint16_t length;
   uint8_t  *p_data;
-  uint16_t remain_length;
   uint16_t i;
   uint16_t rx_id_cnt;
   uint8_t data[DXL_MAX_BUFFER];
@@ -949,22 +930,18 @@ dxl_error_t bulk_write(dxl_t *p_dxl)
   uint16_t addr;
   uint16_t length;
   uint8_t  *p_data;
-  uint16_t remain_length;
   uint16_t index;
-  uint8_t  id;
 
   if (p_dxl->rx.id != DXL_GLOBAL_ID)
   {
     return DXL_RET_EMPTY;
   }
 
 
-
   index = 0;
   while(1)
   {
     p_data = &p_dxl->rx.p_param[index];
-    id     = p_data[0];
     addr   = (p_data[2]<<8) | p_data[1];
     length = (p_data[4]<<8) | p_data[3];
 
@@ -995,3 +972,42 @@ dxl_error_t bulk_write(dxl_t *p_dxl)
 
   return ret;
 }
+
+
+extern uint32_t tx_led_count, rx_led_count;
+
+static void dxl_node_update_tx_rx_led()
+{
+  static uint32_t tx_led_update_time = millis();
+  static uint32_t rx_led_update_time = millis();
+
+  if( (millis()-tx_led_update_time) > 50 )
+  {
+    tx_led_update_time = millis();
+
+    if( tx_led_count )
+    {
+      digitalWriteFast(DXL_LED_TX, !digitalReadFast(DXL_LED_TX));
+      tx_led_count--;
+    }
+    else
+    {
+      digitalWriteFast(DXL_LED_TX, HIGH);
+    }
+  }
+
+  if( (millis()-rx_led_update_time) > 50 )
+  {
+    rx_led_update_time = millis();
+
+    if( rx_led_count )
+    {
+      digitalWriteFast(DXL_LED_RX, !digitalReadFast(DXL_LED_RX));
+      rx_led_count--;
+    }
+    else
+    {
+      digitalWriteFast(DXL_LED_RX, HIGH);
+    }
+  }
+}

arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core.ino

@@ -500,7 +500,7 @@ void updateJointStates(void)
 
   for (uint8_t num = 0; num < (joint_cnt + gripper_cnt); num++)
   {
-    if (num > joint_cnt)
+    if (num >= joint_cnt)
       get_joint_position[num] = get_joint_position[num] * OPEN_MANIPULATOR_GRIPPER_OFFSET;
 
     joint_states_pos[WHEEL_NUM + num] = get_joint_position[num];

arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core_config.h

@@ -45,7 +45,7 @@
 
 #include <math.h>
 
-#define FIRMWARE_VER "2.0.0"
+#define FIRMWARE_VER "2.0.1"
 
 #define CONTROL_MOTOR_SPEED_FREQUENCY          30   //hz
 #define IMU_PUBLISH_FREQUENCY                  200  //hz
@@ -128,7 +128,7 @@ uint32_t current_offset;
 /*******************************************************************************
 * ROS Parameter
 *******************************************************************************/
-char get_prefix[10];
+char get_prefix[100];
 char* get_tf_prefix = get_prefix;
 
 char odom_header_frame_id[30];

arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core_config.h

@@ -26,9 +26,9 @@
 
 #include <math.h>
 
-#define HARDWARE_VER "0.1.0"
-#define SOFTWARE_VER "0.1.0"
-#define FIRMWARE_VER "0.1.0"
+#define HARDWARE_VER "1.0.0"
+#define SOFTWARE_VER "1.0.0"
+#define FIRMWARE_VER "1.0.1"
 
 #define SENSOR_STATE_PUBLISH_FREQUENCY         30    //hz
 #define VERSION_INFORMATION_PUBLISH_FREQUENCY  1    //hz 
@@ -195,9 +195,9 @@ class TurtleBot3 : public ros2::Node
     DEBUG_PRINT("\r\n [Publisher Create]   /imu            : "); DEBUG_PRINT((imu_pub_!=NULL?"Success":"Fail")); DEBUG_PRINT(this->err_code);
 
     // Joint(Dynamixel) state of Turtlebot3
-    joint_states_pub_  = this->createPublisher<sensor_msgs::JointState>("joint_states");
-    this->createWallFreq(JOINT_STATE_PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishJointState, NULL, joint_states_pub_);
-    DEBUG_PRINT("\r\n [Publisher Create]   /joint_states   : "); DEBUG_PRINT((joint_states_pub_!=NULL?"Success":"Fail")); DEBUG_PRINT(this->err_code);
+    // joint_states_pub_  = this->createPublisher<sensor_msgs::JointState>("joint_states");
+    // this->createWallFreq(JOINT_STATE_PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishJointState, NULL, joint_states_pub_);
+    // DEBUG_PRINT("\r\n [Publisher Create]   /joint_states   : "); DEBUG_PRINT((joint_states_pub_!=NULL?"Success":"Fail")); DEBUG_PRINT(this->err_code);
 
     // // (Not necessary) Battey state of Turtlebot3 
     // battery_state_pub_ = this->createPublisher<sensor_msgs::BatteryState>("battery_state");
@@ -246,7 +246,7 @@ class TurtleBot3 : public ros2::Node
   //ros2::Publisher<nav_msgs::Odometry>*            odom_pub_;
   ros2::Publisher<turtlebot3_msgs::SensorState>*  sensor_state_pub_;
   ros2::Publisher<sensor_msgs::Imu>*              imu_pub_;
-  ros2::Publisher<sensor_msgs::JointState>*       joint_states_pub_;
+  // ros2::Publisher<sensor_msgs::JointState>*       joint_states_pub_;
   ros2::Publisher<turtlebot3_msgs::VersionInfo>*  version_info_pub_;
 
   //ros2::Publisher<sensor_msgs::BatteryState>*     battery_state_pub_;

arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/examples/turtlebot3_waffle/turtlebot3_core/turtlebot3_core_config.h

@@ -26,9 +26,9 @@
 
 #include <math.h>
 
-#define HARDWARE_VER "0.1.0"
-#define SOFTWARE_VER "0.1.0"
-#define FIRMWARE_VER "0.1.0"
+#define HARDWARE_VER "1.0.0"
+#define SOFTWARE_VER "1.0.0"
+#define FIRMWARE_VER "1.0.1"
 
 #define SENSOR_STATE_PUBLISH_FREQUENCY         30    //hz
 #define VERSION_INFORMATION_PUBLISH_FREQUENCY  1    //hz 
@@ -195,9 +195,9 @@ class TurtleBot3 : public ros2::Node
     DEBUG_PRINT("\r\n [Publisher Create]   /imu            : "); DEBUG_PRINT((imu_pub_!=NULL?"Success":"Fail")); DEBUG_PRINT(this->err_code);
 
     // Joint(Dynamixel) state of Turtlebot3
-    joint_states_pub_  = this->createPublisher<sensor_msgs::JointState>("joint_states");
-    this->createWallFreq(JOINT_STATE_PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishJointState, NULL, joint_states_pub_);
-    DEBUG_PRINT("\r\n [Publisher Create]   /joint_states   : "); DEBUG_PRINT((joint_states_pub_!=NULL?"Success":"Fail")); DEBUG_PRINT(this->err_code);
+    // joint_states_pub_  = this->createPublisher<sensor_msgs::JointState>("joint_states");
+    // this->createWallFreq(JOINT_STATE_PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishJointState, NULL, joint_states_pub_);
+    // DEBUG_PRINT("\r\n [Publisher Create]   /joint_states   : "); DEBUG_PRINT((joint_states_pub_!=NULL?"Success":"Fail")); DEBUG_PRINT(this->err_code);
 
     // // (Not necessary) Battey state of Turtlebot3 
     // battery_state_pub_ = this->createPublisher<sensor_msgs::BatteryState>("battery_state");