Skip to content

Commit dea3138

Browse files
OpusKOpusK
authored
Merge pull request #158 from ROBOTIS-GIT/develop
Develop
2 parents 36153aa + fefed1e commit dea3138

File tree

11 files changed

+652
-49
lines changed

11 files changed

+652
-49
lines changed

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

+315
Large diffs are not rendered by default.

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

+49
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
#include "turtlebot3_conveyor.h"
2+
#include "turtlebot3_conveyor_motor_driver.h"
3+
4+
#define LOOP_TIME_SEC 0.010f
5+
6+
RC100 rc100;
7+
DynamixelStatus conveyor;
8+
Turtlebot3MotorDriver motor_driver;
9+
10+
uint8_t conveyor_joint[4] = {JOINT_L_R, JOINT_R_R, JOINT_L_F, JOINT_R_F};
11+
uint8_t conveyor_wheel[4] = {WHEEL_L_R, WHEEL_R_R, WHEEL_L_F, WHEEL_R_F};
12+
13+
void setup()
14+
{
15+
Serial.begin(57600);
16+
// while(!Serial);
17+
18+
rc100.begin(1);
19+
motor_driver.init();
20+
}
21+
22+
void loop()
23+
{
24+
static uint32_t previous_time = 0;
25+
uint32_t present_time = millis();
26+
27+
getRC100Data();
28+
29+
if((present_time - previous_time) >= (LOOP_TIME_SEC * 1000))
30+
{
31+
motor_driver.controlJoints(conveyor.setJointAngle());
32+
motor_driver.controlWheels(conveyor.setWheelVel());
33+
34+
previous_time = millis();
35+
}
36+
}
37+
38+
void getRC100Data()
39+
{
40+
if (rc100.available())
41+
{
42+
int rcData = rc100.readData();
43+
44+
conveyor.getDirection(rcData);
45+
delay(1);
46+
47+
conveyor.setParams();
48+
}
49+
}

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

+173
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
/*******************************************************************************
2+
* Copyright 2016 ROBOTIS CO., LTD.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*******************************************************************************/
16+
17+
/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim, HanCheol Cho */
18+
19+
#include "turtlebot3_conveyor_motor_driver.h"
20+
21+
Turtlebot3MotorDriver::Turtlebot3MotorDriver()
22+
: baudrate_(BAUDRATE),
23+
protocol_version_(PROTOCOL_VERSION)
24+
{
25+
}
26+
27+
Turtlebot3MotorDriver::~Turtlebot3MotorDriver()
28+
{
29+
closeDynamixel();
30+
}
31+
32+
bool Turtlebot3MotorDriver::init(void)
33+
{
34+
portHandler_ = dynamixel::PortHandler::getPortHandler(DEVICENAME);
35+
packetHandler_ = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
36+
37+
// Open port
38+
if (portHandler_->openPort())
39+
{
40+
ERROR_PRINT("Port is opened");
41+
}
42+
else
43+
{
44+
ERROR_PRINT("Port couldn't be opened");
45+
46+
return false;
47+
}
48+
49+
// Set port baudrate
50+
if (portHandler_->setBaudRate(baudrate_))
51+
{
52+
ERROR_PRINT("Baudrate is set");
53+
}
54+
else
55+
{
56+
ERROR_PRINT("Baudrate couldn't be set");
57+
58+
return false;
59+
}
60+
61+
// Enable Dynamixel Torque
62+
setTorque(WHEEL_L_R, true);
63+
setTorque(WHEEL_R_R, true);
64+
setTorque(WHEEL_L_F, true);
65+
setTorque(WHEEL_R_F, true);
66+
67+
setTorque(JOINT_L_R, true);
68+
setTorque(JOINT_R_R, true);
69+
setTorque(JOINT_L_F, true);
70+
setTorque(JOINT_R_F, true);
71+
72+
groupSyncWriteVelocity_ = new dynamixel::GroupSyncWrite(portHandler_, packetHandler_, ADDR_X_GOAL_VELOCITY, LEN_X_GOAL_VELOCITY);
73+
groupSyncWritePosition_ = new dynamixel::GroupSyncWrite(portHandler_, packetHandler_, ADDR_X_GOAL_POSITION, LEN_X_GOAL_POSITION);
74+
75+
return true;
76+
}
77+
78+
bool Turtlebot3MotorDriver::setTorque(uint8_t id, bool onoff)
79+
{
80+
uint8_t dxl_error = 0;
81+
int dxl_comm_result = COMM_TX_FAIL;
82+
83+
dxl_comm_result = packetHandler_->write1ByteTxRx(portHandler_, id, ADDR_X_TORQUE_ENABLE, onoff, &dxl_error);
84+
if(dxl_comm_result != COMM_SUCCESS)
85+
{
86+
packetHandler_->getTxRxResult(dxl_comm_result);
87+
}
88+
else if(dxl_error != 0)
89+
{
90+
packetHandler_->getRxPacketError(dxl_error);
91+
}
92+
}
93+
94+
void Turtlebot3MotorDriver::closeDynamixel(void)
95+
{
96+
// Disable Dynamixel Torque
97+
setTorque(WHEEL_L_R, false);
98+
setTorque(WHEEL_R_R, false);
99+
setTorque(WHEEL_L_F, false);
100+
setTorque(WHEEL_R_F, false);
101+
102+
setTorque(JOINT_L_R, false);
103+
setTorque(JOINT_R_R, false);
104+
setTorque(JOINT_L_F, false);
105+
setTorque(JOINT_R_F, false);
106+
107+
// Close port
108+
portHandler_->closePort();
109+
}
110+
111+
bool Turtlebot3MotorDriver::controlJoints(int32_t *value)
112+
{
113+
bool dxl_addparam_result_;
114+
int8_t dxl_comm_result_;
115+
116+
dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_L_R, (uint8_t*)&value[0]);
117+
if (dxl_addparam_result_ != true)
118+
return false;
119+
120+
dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_R_R, (uint8_t*)&value[1]);
121+
if (dxl_addparam_result_ != true)
122+
return false;
123+
124+
dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_L_F, (uint8_t*)&value[2]);
125+
if (dxl_addparam_result_ != true)
126+
return false;
127+
128+
dxl_addparam_result_ = groupSyncWritePosition_->addParam(JOINT_R_F, (uint8_t*)&value[3]);
129+
if (dxl_addparam_result_ != true)
130+
return false;
131+
132+
dxl_comm_result_ = groupSyncWritePosition_->txPacket();
133+
if (dxl_comm_result_ != COMM_SUCCESS)
134+
{
135+
packetHandler_->getTxRxResult(dxl_comm_result_);
136+
return false;
137+
}
138+
139+
groupSyncWritePosition_->clearParam();
140+
return true;
141+
}
142+
143+
bool Turtlebot3MotorDriver::controlWheels(int32_t *value)
144+
{
145+
bool dxl_addparam_result_;
146+
int8_t dxl_comm_result_;
147+
148+
dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_L_R, (uint8_t*)&value[0]);
149+
if (dxl_addparam_result_ != true)
150+
return false;
151+
152+
dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_R_R, (uint8_t*)&value[1]);
153+
if (dxl_addparam_result_ != true)
154+
return false;
155+
156+
dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_L_F, (uint8_t*)&value[2]);
157+
if (dxl_addparam_result_ != true)
158+
return false;
159+
160+
dxl_addparam_result_ = groupSyncWriteVelocity_->addParam(WHEEL_R_F, (uint8_t*)&value[3]);
161+
if (dxl_addparam_result_ != true)
162+
return false;
163+
164+
dxl_comm_result_ = groupSyncWriteVelocity_->txPacket();
165+
if (dxl_comm_result_ != COMM_SUCCESS)
166+
{
167+
packetHandler_->getTxRxResult(dxl_comm_result_);
168+
return false;
169+
}
170+
171+
groupSyncWriteVelocity_->clearParam();
172+
return true;
173+
}

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

+88
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
/*******************************************************************************
2+
* Copyright 2016 ROBOTIS CO., LTD.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*******************************************************************************/
16+
17+
/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim, HanCheol Cho */
18+
19+
#ifndef TURTLEBOT3_CONVEYOR_MOTOR_DRIVER_H_
20+
#define TURTLEBOT3_CONVEYOR_MOTOR_DRIVER_H_
21+
22+
#include <DynamixelSDK.h>
23+
24+
// Control table address (Dynamixel X-series)
25+
#define ADDR_X_TORQUE_ENABLE 64
26+
#define ADDR_X_GOAL_VELOCITY 104
27+
#define ADDR_X_PROFILE_ACCELERATION 108
28+
#define ADDR_X_PROFILE_VELOCITY 112
29+
#define ADDR_X_GOAL_POSITION 116
30+
#define ADDR_X_REALTIME_TICK 120
31+
#define ADDR_X_PRESENT_VELOCITY 128
32+
#define ADDR_X_PRESENT_POSITION 132
33+
34+
// Limit values (XM430-W210-T)
35+
#define LIMIT_X_MAX_VELOCITY 240
36+
37+
// Data Byte Length
38+
#define LEN_X_TORQUE_ENABLE 1
39+
#define LEN_X_GOAL_VELOCITY 4
40+
#define LEN_X_GOAL_POSITION 4
41+
#define LEN_X_REALTIME_TICK 2
42+
#define LEN_X_PRESENT_VELOCITY 4
43+
#define LEN_X_PRESENT_POSITION 4
44+
45+
#define PROTOCOL_VERSION 2.0 // Dynamixel protocol version 2.0
46+
47+
#define WHEEL_L_R 1
48+
#define WHEEL_R_R 2
49+
#define WHEEL_L_F 3
50+
#define WHEEL_R_F 4
51+
52+
#define JOINT_L_R 5
53+
#define JOINT_R_R 6
54+
#define JOINT_L_F 7
55+
#define JOINT_R_F 8
56+
57+
#define BAUDRATE 1000000 // baud rate of Dynamixel
58+
#define DEVICENAME "" // no need setting on OpenCR
59+
60+
#define TORQUE_ENABLE 1 // Value for enabling the torque
61+
#define TORQUE_DISABLE 0 // Value for disabling the torque
62+
63+
class Turtlebot3MotorDriver
64+
{
65+
public:
66+
Turtlebot3MotorDriver();
67+
~Turtlebot3MotorDriver();
68+
bool init(void);
69+
void closeDynamixel(void);
70+
bool setTorque(uint8_t id, bool onoff);
71+
bool setProfileAcceleration(uint8_t id, uint32_t value);
72+
bool setProfileVelocity(uint8_t id, uint32_t value);
73+
bool controlJoints(int32_t *value);
74+
bool controlWheels(int32_t *value);
75+
76+
77+
private:
78+
uint32_t baudrate_;
79+
float protocol_version_;
80+
81+
dynamixel::PortHandler *portHandler_;
82+
dynamixel::PacketHandler *packetHandler_;
83+
84+
dynamixel::GroupSyncWrite *groupSyncWriteVelocity_;
85+
dynamixel::GroupSyncWrite *groupSyncWritePosition_;
86+
};
87+
88+
#endif // TURTLEBOT3_CONVEYOR_MOTOR_DRIVER_H_

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

+20-13
Original file line numberDiff line numberDiff line change
@@ -161,20 +161,27 @@ bool Turtlebot3MotorDriver::writeVelocity(int64_t left_value, int64_t right_valu
161161
bool dxl_addparam_result;
162162
int8_t dxl_comm_result;
163163

164-
int64_t value[2] = {left_value, right_value};
165-
uint8_t data_byte[4] = {0, };
164+
uint8_t left_data_byte[4] = {0, };
165+
uint8_t right_data_byte[4] = {0, };
166166

167-
for (uint8_t index = 0; index < 2; index++)
168-
{
169-
data_byte[0] = DXL_LOBYTE(DXL_LOWORD(value[index]));
170-
data_byte[1] = DXL_HIBYTE(DXL_LOWORD(value[index]));
171-
data_byte[2] = DXL_LOBYTE(DXL_HIWORD(value[index]));
172-
data_byte[3] = DXL_HIBYTE(DXL_HIWORD(value[index]));
173-
174-
dxl_addparam_result = groupSyncWriteVelocity_->addParam(index+1, (uint8_t*)&data_byte);
175-
if (dxl_addparam_result != true)
176-
return false;
177-
}
167+
168+
left_data_byte[0] = DXL_LOBYTE(DXL_LOWORD(left_value));
169+
left_data_byte[1] = DXL_HIBYTE(DXL_LOWORD(left_value));
170+
left_data_byte[2] = DXL_LOBYTE(DXL_HIWORD(left_value));
171+
left_data_byte[3] = DXL_HIBYTE(DXL_HIWORD(left_value));
172+
173+
dxl_addparam_result = groupSyncWriteVelocity_->addParam(left_wheel_id_, (uint8_t*)&left_data_byte);
174+
if (dxl_addparam_result != true)
175+
return false;
176+
177+
right_data_byte[0] = DXL_LOBYTE(DXL_LOWORD(right_value));
178+
right_data_byte[1] = DXL_HIBYTE(DXL_LOWORD(right_value));
179+
right_data_byte[2] = DXL_LOBYTE(DXL_HIWORD(right_value));
180+
right_data_byte[3] = DXL_HIBYTE(DXL_HIWORD(right_value));
181+
182+
dxl_addparam_result = groupSyncWriteVelocity_->addParam(right_wheel_id_, (uint8_t*)&right_data_byte);
183+
if (dxl_addparam_result != true)
184+
return false;
178185

179186
dxl_comm_result = groupSyncWriteVelocity_->txPacket();
180187
if (dxl_comm_result != COMM_SUCCESS)

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

+2-2
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ void setup()
4242

4343
updateGyroCali(true);
4444

45-
ros2::init(NULL);
45+
ros2::init(&RTPS_SERIAL);
4646

4747
sprintf(imu_frame_id, "imu_link");
4848
sprintf(joint_state_header_frame_id, "base_link");
@@ -609,4 +609,4 @@ void subscribeTimeSync(builtin_interfaces::Time* msg, void* arg)
609609
(void)(arg);
610610

611611
ros2::syncTimeFromRemote(msg);
612-
}
612+
}

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

-16
Original file line numberDiff line numberDiff line change
@@ -20,22 +20,6 @@
2020
#define TURTLEBOT3_CORE_CONFIG_H_
2121

2222
#include <ros2arduino.h>
23-
#include <std_msgs/Bool.hpp>
24-
#include <std_msgs/Empty.hpp>
25-
//#include <std_msgs/Int32.hpp>
26-
#include <sensor_msgs/Imu.hpp>
27-
#include <sensor_msgs/JointState.hpp>
28-
//#include <sensor_msgs/BatteryState.hpp>
29-
//#include <sensor_msgs/MagneticField.hpp>
30-
//#include <geometry_msgs/Vector3.hpp>
31-
#include <geometry_msgs/Twist.hpp>
32-
//#include <tf/tf.hpp>
33-
//#include <tf/transform_broadcaster.hpp>
34-
#include <nav_msgs/Odometry.hpp>
35-
36-
#include <turtlebot3_msgs/SensorState.hpp>
37-
#include <turtlebot3_msgs/Sound.hpp>
38-
#include <turtlebot3_msgs/VersionInfo.hpp>
3923

4024
#include <TurtleBot3_ROS2.h>
4125
#include "turtlebot3_burger.h"

0 commit comments

Comments
 (0)