Develop

arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_friends/turtlebot3_conveyor/turtlebot3_conveyor.ino

@@ -0,0 +1,49 @@
+#include "turtlebot3_conveyor.h"
+#include "turtlebot3_conveyor_motor_driver.h"
+
+#define LOOP_TIME_SEC 0.010f
+
+RC100 rc100;
+DynamixelStatus conveyor;
+Turtlebot3MotorDriver motor_driver;
+
+uint8_t conveyor_joint[4] = {JOINT_L_R, JOINT_R_R, JOINT_L_F, JOINT_R_F};
+uint8_t conveyor_wheel[4] = {WHEEL_L_R, WHEEL_R_R, WHEEL_L_F, WHEEL_R_F};
+
+void setup()
+{
+  Serial.begin(57600);
+  // while(!Serial);
+
+  rc100.begin(1);
+  motor_driver.init();
+}
+
+void loop()
+{
+  static uint32_t previous_time = 0;
+  uint32_t present_time = millis();
+
+  getRC100Data();
+
+  if((present_time - previous_time) >= (LOOP_TIME_SEC * 1000))
+  {
+    motor_driver.controlJoints(conveyor.setJointAngle());
+    motor_driver.controlWheels(conveyor.setWheelVel());
+
+    previous_time = millis();
+  } 
+}
+
+void getRC100Data()
+{
+  if (rc100.available())
+  {    
+    int rcData = rc100.readData();
+
+    conveyor.getDirection(rcData);
+    delay(1);
+
+    conveyor.setParams();
+  }
+}

arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_friends/turtlebot3_conveyor/turtlebot3_conveyor_motor_driver.cpp

@@ -0,0 +1,173 @@
+/*******************************************************************************
+* Copyright 2016 ROBOTIS CO., LTD.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*     http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*******************************************************************************/
+
+/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim, HanCheol Cho */
+
+#include "turtlebot3_conveyor_motor_driver.h"
+
+Turtlebot3MotorDriver::Turtlebot3MotorDriver()
+: baudrate_(BAUDRATE),
+  protocol_version_(PROTOCOL_VERSION)
+{
+}
+
+Turtlebot3MotorDriver::~Turtlebot3MotorDriver()
+{
+  closeDynamixel();
+}
+
+bool Turtlebot3MotorDriver::init(void)
+{
+  portHandler_   = dynamixel::PortHandler::getPortHandler(DEVICENAME);
+  packetHandler_ = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
+
+  // Open port
+  if (portHandler_->openPort())
+  {
+    ERROR_PRINT("Port is opened");
+  }
+  else
+  {
+    ERROR_PRINT("Port couldn't be opened");
+
+    return false;
+  }
+
+  // Set port baudrate
+  if (portHandler_->setBaudRate(baudrate_))
+  {
+    ERROR_PRINT("Baudrate is set");
+  }
+  else
+  {
+    ERROR_PRINT("Baudrate couldn't be set");
+
+    return false;
+  }
+
+  // Enable Dynamixel Torque
+  setTorque(WHEEL_L_R, true);
+  setTorque(WHEEL_R_R, true);
+  setTorque(WHEEL_L_F, true);
+  setTorque(WHEEL_R_F, true);
+
+  setTorque(JOINT_L_R, true);
+  setTorque(JOINT_R_R, true);
+  setTorque(JOINT_L_F, true);
+  setTorque(JOINT_R_F, true);
+
+  groupSyncWriteVelocity_ = new dynamixel::GroupSyncWrite(portHandler_, packetHandler_, ADDR_X_GOAL_VELOCITY, LEN_X_GOAL_VELOCITY);
+  groupSyncWritePosition_ = new dynamixel::GroupSyncWrite(portHandler_, packetHandler_, ADDR_X_GOAL_POSITION, LEN_X_GOAL_POSITION);
+
+  return true;
+}
+
+bool Turtlebot3MotorDriver::setTorque(uint8_t id, bool onoff)
+{
+  uint8_t dxl_error = 0;
+  int dxl_comm_result = COMM_TX_FAIL;
+
+  dxl_comm_result = packetHandler_->write1ByteTxRx(portHandler_, id, ADDR_X_TORQUE_ENABLE, onoff, &dxl_error);
+  if(dxl_comm_result != COMM_SUCCESS)
+  {
+    packetHandler_->getTxRxResult(dxl_comm_result);
+  }
+  else if(dxl_error != 0)
+  {
+    packetHandler_->getRxPacketError(dxl_error);
+  }
+}
+
+void Turtlebot3MotorDriver::closeDynamixel(void)
+{
+  // Disable Dynamixel Torque
+  setTorque(WHEEL_L_R, false);
+  setTorque(WHEEL_R_R, false);
+  setTorque(WHEEL_L_F, false);
+  setTorque(WHEEL_R_F, false);
+
+  setTorque(JOINT_L_R, false);
+  setTorque(JOINT_R_R, false);
+  setTorque(JOINT_L_F, false);
+  setTorque(JOINT_R_F, false);
+
+  // Close port
+  portHandler_->closePort();
+}
+
+bool Turtlebot3MotorDriver::controlJoints(int32_t *value)
+{
+  bool dxl_addparam_result_;
+  int8_t dxl_comm_result_;
+
+  dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_L_R, (uint8_t*)&value[0]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_R_R, (uint8_t*)&value[1]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_L_F, (uint8_t*)&value[2]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_R_F, (uint8_t*)&value[3]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_comm_result_ = groupSyncWritePosition_->txPacket();
+  if (dxl_comm_result_ != COMM_SUCCESS)
+  {
+    packetHandler_->getTxRxResult(dxl_comm_result_);
+    return false;
+  }
+
+  groupSyncWritePosition_->clearParam();
+  return true;
+}
+
+bool Turtlebot3MotorDriver::controlWheels(int32_t *value)
+{
+  bool dxl_addparam_result_;
+  int8_t dxl_comm_result_;
+
+  dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_L_R, (uint8_t*)&value[0]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_R_R, (uint8_t*)&value[1]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_L_F, (uint8_t*)&value[2]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_R_F, (uint8_t*)&value[3]);
+  if (dxl_addparam_result_ != true)
+    return false;
+
+  dxl_comm_result_ = groupSyncWriteVelocity_->txPacket();
+  if (dxl_comm_result_ != COMM_SUCCESS)
+  {
+    packetHandler_->getTxRxResult(dxl_comm_result_);
+    return false;
+  }
+
+  groupSyncWriteVelocity_->clearParam();
+  return true;
+}

arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_friends/turtlebot3_conveyor/turtlebot3_conveyor_motor_driver.h

@@ -0,0 +1,88 @@
+/*******************************************************************************
+* Copyright 2016 ROBOTIS CO., LTD.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*     http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*******************************************************************************/
+
+/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim, HanCheol Cho */
+
+#ifndef TURTLEBOT3_CONVEYOR_MOTOR_DRIVER_H_
+#define TURTLEBOT3_CONVEYOR_MOTOR_DRIVER_H_
+
+#include <DynamixelSDK.h>
+
+// Control table address (Dynamixel X-series)
+#define ADDR_X_TORQUE_ENABLE            64
+#define ADDR_X_GOAL_VELOCITY            104
+#define ADDR_X_PROFILE_ACCELERATION     108
+#define ADDR_X_PROFILE_VELOCITY         112
+#define ADDR_X_GOAL_POSITION            116
+#define ADDR_X_REALTIME_TICK            120
+#define ADDR_X_PRESENT_VELOCITY         128
+#define ADDR_X_PRESENT_POSITION         132
+
+// Limit values (XM430-W210-T)
+#define LIMIT_X_MAX_VELOCITY            240
+
+// Data Byte Length
+#define LEN_X_TORQUE_ENABLE             1
+#define LEN_X_GOAL_VELOCITY             4
+#define LEN_X_GOAL_POSITION             4
+#define LEN_X_REALTIME_TICK             2
+#define LEN_X_PRESENT_VELOCITY          4
+#define LEN_X_PRESENT_POSITION          4
+
+#define PROTOCOL_VERSION                2.0     // Dynamixel protocol version 2.0
+
+#define WHEEL_L_R 1
+#define WHEEL_R_R 2
+#define WHEEL_L_F 3
+#define WHEEL_R_F 4
+
+#define JOINT_L_R 5
+#define JOINT_R_R 6
+#define JOINT_L_F 7
+#define JOINT_R_F 8
+
+#define BAUDRATE                        1000000 // baud rate of Dynamixel
+#define DEVICENAME                      ""      // no need setting on OpenCR
+
+#define TORQUE_ENABLE                   1       // Value for enabling the torque
+#define TORQUE_DISABLE                  0       // Value for disabling the torque
+
+class Turtlebot3MotorDriver
+{
+ public:
+  Turtlebot3MotorDriver();
+  ~Turtlebot3MotorDriver();
+  bool init(void);
+  void closeDynamixel(void);
+  bool setTorque(uint8_t id, bool onoff);
+  bool setProfileAcceleration(uint8_t id, uint32_t value);
+  bool setProfileVelocity(uint8_t id, uint32_t value);
+  bool controlJoints(int32_t *value);
+  bool controlWheels(int32_t *value);
+
+
+ private:
+  uint32_t baudrate_;
+  float  protocol_version_;
+
+  dynamixel::PortHandler *portHandler_;
+  dynamixel::PacketHandler *packetHandler_;
+
+  dynamixel::GroupSyncWrite *groupSyncWriteVelocity_;
+  dynamixel::GroupSyncWrite *groupSyncWritePosition_;
+};
+
+#endif // TURTLEBOT3_CONVEYOR_MOTOR_DRIVER_H_

arduino/opencr_arduino/opencr/libraries/turtlebot3/src/turtlebot3/turtlebot3_motor_driver.cpp

@@ -161,20 +161,27 @@ bool Turtlebot3MotorDriver::writeVelocity(int64_t left_value, int64_t right_valu
   bool dxl_addparam_result;
   int8_t dxl_comm_result;
 
-  int64_t value[2] = {left_value, right_value};
-  uint8_t data_byte[4] = {0, };
+  uint8_t left_data_byte[4] = {0, };
+  uint8_t right_data_byte[4] = {0, };
 
-  for (uint8_t index = 0; index < 2; index++)
-  {
-    data_byte[0] = DXL_LOBYTE(DXL_LOWORD(value[index]));
-    data_byte[1] = DXL_HIBYTE(DXL_LOWORD(value[index]));
-    data_byte[2] = DXL_LOBYTE(DXL_HIWORD(value[index]));
-    data_byte[3] = DXL_HIBYTE(DXL_HIWORD(value[index]));
-
-    dxl_addparam_result = groupSyncWriteVelocity_->addParam(index+1, (uint8_t*)&data_byte);
-    if (dxl_addparam_result != true)
-      return false;
-  }
+
+  left_data_byte[0] = DXL_LOBYTE(DXL_LOWORD(left_value));
+  left_data_byte[1] = DXL_HIBYTE(DXL_LOWORD(left_value));
+  left_data_byte[2] = DXL_LOBYTE(DXL_HIWORD(left_value));
+  left_data_byte[3] = DXL_HIBYTE(DXL_HIWORD(left_value));
+
+  dxl_addparam_result = groupSyncWriteVelocity_->addParam(left_wheel_id_, (uint8_t*)&left_data_byte);
+  if (dxl_addparam_result != true)
+    return false;
+
+  right_data_byte[0] = DXL_LOBYTE(DXL_LOWORD(right_value));
+  right_data_byte[1] = DXL_HIBYTE(DXL_LOWORD(right_value));
+  right_data_byte[2] = DXL_LOBYTE(DXL_HIWORD(right_value));
+  right_data_byte[3] = DXL_HIBYTE(DXL_HIWORD(right_value));
+
+  dxl_addparam_result = groupSyncWriteVelocity_->addParam(right_wheel_id_, (uint8_t*)&right_data_byte);
+  if (dxl_addparam_result != true)
+    return false;
 
   dxl_comm_result = groupSyncWriteVelocity_->txPacket();
   if (dxl_comm_result != COMM_SUCCESS)

arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core.ino

@@ -42,7 +42,7 @@ void setup()
 
   updateGyroCali(true);
 
-  ros2::init(NULL);
+  ros2::init(&RTPS_SERIAL);
 
   sprintf(imu_frame_id, "imu_link");
   sprintf(joint_state_header_frame_id, "base_link");
@@ -609,4 +609,4 @@ void subscribeTimeSync(builtin_interfaces::Time* msg, void* arg)
   (void)(arg);
 
   ros2::syncTimeFromRemote(msg);
-}
+}

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

@@ -20,22 +20,6 @@
 #define TURTLEBOT3_CORE_CONFIG_H_
 
 #include <ros2arduino.h>
-#include <std_msgs/Bool.hpp>
-#include <std_msgs/Empty.hpp>
-//#include <std_msgs/Int32.hpp>
-#include <sensor_msgs/Imu.hpp>
-#include <sensor_msgs/JointState.hpp>
-//#include <sensor_msgs/BatteryState.hpp>
-//#include <sensor_msgs/MagneticField.hpp>
-//#include <geometry_msgs/Vector3.hpp>
-#include <geometry_msgs/Twist.hpp>
-//#include <tf/tf.hpp>
-//#include <tf/transform_broadcaster.hpp>
-#include <nav_msgs/Odometry.hpp>
-
-#include <turtlebot3_msgs/SensorState.hpp>
-#include <turtlebot3_msgs/Sound.hpp>
-#include <turtlebot3_msgs/VersionInfo.hpp>
 
 #include <TurtleBot3_ROS2.h>
 #include "turtlebot3_burger.h"