Activities About Capstone Design

Sorting robot
Arduino, Dynamixel Shield Used
KMU Robotics Lab × Raionbow Robotics Inc
My part was developing gripper which connected to main control system
For example Structing gripper, 3D modeling, assembling, programming the gripper with my own
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
#include "gripper_control.h"
#include "gripper_controller/rb_data.h"
#include "gripper_controller/ObjectPosition.h"
 
GripperControl *g_gripperControlPtr = NULL;
 
string motor_cmd;
string g_motorServiceName;
string g_subTopicPersonLocation;
string g_motorServiceCall;
bool isSimulation = false;
ros::Subscriber rb_data_sub, object_pos_sub;
 
systemSTAT systemStat;
int ignore_cnt = 0;
float joint_simulation[6];
float tcp_simulation[6];
float object_position[6];
 
float *pick_arr[6];
int pickAndPlace = 1;
 
////////////////////////////////
//Global Parameter Initialize//
float cupPick_x;
float cupPick_y;
float cupPick_z;
float cupPick_rx;
float cupPick_ry;
float cupPick_rz;
 
float cupPlaceSupport_x;
float cupPlaceSupport_y;
float cupPlaceSupport_z;
float cupPlaceSupport_rx;
float cupPlaceSupport_ry;
float cupPlaceSupport_rz;
 
float holderPick_x;
float holderPick_y;
float holderPick_z;
float holderPick_rx;
float holderPick_ry;
float holderPick_rz;
 
float holderPlace_x;
float holderPlace_y;
float holderPlace_z;
float holderPlace_rx;
float holderPlace_ry;
float holderPlace_rz;
 
float cupInHolderPick_x;
float cupInHolderPick_y;
float cupInHolderPick_z;
float cupInHolderPick_rx;
float cupInHolderPick_ry;
float cupInHolderPick_rz;
 
float trashCupPlace_x;
float trashCupPlace_y;
float trashCupPlace_z;
float trashCupPlace_rx;
float trashCupPlace_ry;
float trashCupPlace_rz;
 
float trashHolderPick_x;
float trashHolderPick_y;
float trashHolderPick_z;
float trashHolderPick_rx;
float trashHolderPick_ry;
float trashHolderPick_rz;
 
float trashHolderPlace_x;
float trashHolderPlace_y;
float trashHolderPlace_z;
float trashHolderPlace_rx;
float trashHolderPlace_ry;
float trashHolderPlace_rz;
 
////////////////////////////////
 
 
 
void motorServiceCall(uint16_t motor_id, uint16_t position_value, ros::ServiceClient &_motor_client)
{
    g_motorServiceCall = std::to_string(motor_id) + "_service_call";
    dynamixel_workbench_msgs::DynamixelCommand g_motorServiceCall;
    
    g_motorServiceCall.request.command= "";
    g_motorServiceCall.request.id = motor_id;
    g_motorServiceCall.request.addr_name = motor_cmd;
    g_motorServiceCall.request.value = position_value;
    
    ROS_INFO("Motor Id: %u", g_motorServiceCall.request.id);
    if (_motor_client.call(g_motorServiceCall))
    {
        ROS_INFO("Position Value: %u", position_value);
    } 
    else 
    {
        ROS_ERROR("Can't Request Service");
    }
}
 
 
void rb_data_callback(const gripper_controller::rb_data::ConstPtr &msg){
 
    systemStat.sdata.time = msg->time;
 
    for(int i=0; i<6; i++){
        systemStat.sdata.jnt_ref[i] = msg->joint_reference[i];
        systemStat.sdata.jnt_ang[i] = msg->joint_encoder[i];
        systemStat.sdata.cur[i] = msg->joint_current[i];
        systemStat.sdata.tcp_ref[i] = msg->tcp_reference[i];
        // pick_arr[i] = msg->tcp_reference[i];
        systemStat.sdata.temperature_mc[i] = msg->temperature[i];
        systemStat.sdata.jnt_info[i] = msg->joint_information[i];
        joint_simulation[i] = msg->joint_simulation[i];
        tcp_simulation[i] = msg->tcp_simulation[i];
    }
 
    systemStat.sdata.task_state = msg->task_state;
    systemStat.sdata.robot_state = msg->robot_state;
    systemStat.sdata.power_state = msg->power_state;
    systemStat.sdata.collision_detect_onoff = msg->collision_detect;
    systemStat.sdata.is_freedrive_mode = msg->freedrive_mode;
    systemStat.sdata.program_mode = msg->program_mode;
 
    systemStat.sdata.op_stat_collision_occur = msg->op_stat_collision_occur;
    systemStat.sdata.op_stat_sos_flag = msg->op_stat_sos_flag;
    systemStat.sdata.op_stat_self_collision = msg->op_stat_self_collision;
    systemStat.sdata.op_stat_soft_estop_occur = msg->op_stat_soft_estop_occur;
    systemStat.sdata.op_stat_ems_flag = msg->op_stat_ems_flag;
 
    for(int i=0; i<4; i++){
        systemStat.sdata.analog_in[i] = msg->analog_in[i];
        systemStat.sdata.analog_out[i] = msg->analog_out[i];
    }
    for(int i=0; i<16; i++){
        systemStat.sdata.digital_in[i] = msg->digital_in[i];
        systemStat.sdata.digital_out[i] = msg->digital_out[i];
    }
    for(int i=0; i<2; i++){
        systemStat.sdata.tfb_analog_in[i] = msg->tfb_analog_in[i];
        systemStat.sdata.tfb_digital_in[i] = msg->tfb_digital_in[i];
        systemStat.sdata.tfb_digital_out[i] = msg->tfb_digital_out[i];
    }
    systemStat.sdata.tfb_voltage_out = msg->tfb_voltage_out;
 
    systemStat.sdata.default_speed = msg->default_speed;
}
 
void object_position_callback(const gripper_controller::ObjectPosition::ConstPtr &msg)
{
    object_position[0] = msg->x;
    object_position[1] = msg->y;
    object_position[2] = msg->z;
    object_position[3] = msg->rx;
    object_position[4] = msg->ry;
    object_position[5] = msg->rz;
}
float euclidean_distance(float x1, float x2)
{
    return abs(x2-x1);
}
void setMotorThread()
{
    ros::NodeHandle nh;
    ros::ServiceClient gripper_client = nh.serviceClient<dynamixel_workbench_msgs::DynamixelCommand>(g_motorServiceName);
 
    ros::Rate rate(10);
    
    GripperControl gripperControl;
    g_gripperControlPtr = &gripperControl;
 
    /*===========================================*/
    //  simulation mode
    /*===========================================*/
    // pub
    ros::Publisher pubPanMotorValue = nh.advertise<std_msgs::Float64>("/zed_pan_module/zed_pan_joint_position_controller/command", 1);
    std_msgs::Float64 panMotorValue;
    // send initial pan motor value
    
    panMotorValue.data = 0.0;
    ros::Duration(0.5).sleep();
    pubPanMotorValue.publish(panMotorValue);
    /*===========================================*/
 
    while(ros::ok())
    {   
        if(abs(euclidean_distance(object_position[0],systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(object_position[1],systemStat.sdata.tcp_ref[1]))<0.01 && abs(euclidean_distance(object_position[2],systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(object_position[3],systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(object_position[4],systemStat.sdata.tcp_ref[4]))<0.01 && abs(euclidean_distance(object_position[5],systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 0;
        else if(abs(euclidean_distance(cupPlaceSupport_x,systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(cupPlaceSupport_y,systemStat.sdata.tcp_ref[1]))<1 && abs(euclidean_distance(cupPlaceSupport_z,systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(cupPlaceSupport_rx,systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(cupPlaceSupport_ry,systemStat.sdata.tcp_ref[4]))<1 && abs(euclidean_distance(cupPlaceSupport_rz,systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 1;
        else if(abs(euclidean_distance(holderPick_x,systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(holderPick_y,systemStat.sdata.tcp_ref[1]))<0.01 && abs(euclidean_distance(holderPick_z,systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(holderPick_rx,systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(holderPick_ry,systemStat.sdata.tcp_ref[4]))<0.01 && abs(euclidean_distance(holderPick_rz,systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 2;
        else if(abs(euclidean_distance(holderPlace_x,systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(holderPlace_y,systemStat.sdata.tcp_ref[1]))<0.01 && abs(euclidean_distance(holderPlace_z,systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(holderPlace_rx,systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(holderPlace_ry,systemStat.sdata.tcp_ref[4]))<0.01 && abs(euclidean_distance(holderPlace_rz,systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 3;
        else if(abs(euclidean_distance(cupInHolderPick_x,systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(cupInHolderPick_y,systemStat.sdata.tcp_ref[1]))<0.01 && abs(euclidean_distance(cupInHolderPick_z,systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(cupInHolderPick_rx,systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(cupInHolderPick_ry,systemStat.sdata.tcp_ref[4]))<0.01 && abs(euclidean_distance(cupInHolderPick_rz,systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 4;
        else if(abs(euclidean_distance(trashCupPlace_x,systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(trashCupPlace_y,systemStat.sdata.tcp_ref[1]))<0.01 && abs(euclidean_distance(trashCupPlace_z,systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(trashCupPlace_rx,systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(trashCupPlace_ry,systemStat.sdata.tcp_ref[4]))<0.01 && abs(euclidean_distance(trashCupPlace_rz,systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 5;
        else if(abs(euclidean_distance(trashHolderPick_x,systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(trashHolderPick_y,systemStat.sdata.tcp_ref[1]))<0.01 && abs(euclidean_distance(trashHolderPick_z,systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(trashHolderPick_rx,systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(trashHolderPick_ry,systemStat.sdata.tcp_ref[4]))<0.01 && abs(euclidean_distance(trashHolderPick_rz,systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 6;
        else if(abs(euclidean_distance(trashHolderPlace_x,systemStat.sdata.tcp_ref[0])<0.01 && abs(euclidean_distance(trashHolderPlace_y,systemStat.sdata.tcp_ref[1]))<0.01 && abs(euclidean_distance(trashHolderPlace_z,systemStat.sdata.tcp_ref[2]))<0.01
        && abs(euclidean_distance(trashHolderPlace_rx,systemStat.sdata.tcp_ref[3]))<0.01 && abs(euclidean_distance(trashHolderPlace_ry,systemStat.sdata.tcp_ref[4]))<0.01 && abs(euclidean_distance(trashHolderPlace_rz,systemStat.sdata.tcp_ref[5]))<0.01))
            pickAndPlace = 7;
 
        if(pickAndPlace==0){
            // Cup Pick
            motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMaxPosition, gripper_client);
            motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMinPosition, gripper_client);
            cout<<"cup pick \n";
        }
         if(pickAndPlace==1)
        {
            // Cup Holder Place
            // ros::Duration(7.0).sleep();
            motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMinPosition, gripper_client);
            motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMaxPosition, gripper_client);
            cout<<"cup place \n";
        }
        
        else if(pickAndPlace==2)
        {
            //cup Holder Pick
 
            // motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMaxPosition, gripper_client);
            // motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMinPosition, gripper_client);
            motorServiceCall(1, 240, gripper_client);
            motorServiceCall(2, 170, gripper_client);
            cout<<"Holder pick 2\n";
        }
        else if(pickAndPlace==3)
        {
            //cup Holder Place
            motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMinPosition, gripper_client);
            motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMaxPosition, gripper_client);
            cout<<"cup Holder Place \n";
        }
        else if(pickAndPlace==4)
        {   
            //Cup Pick
            motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMaxPosition, gripper_client);
            motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMinPosition, gripper_client);
            cout<<"Cup In holder Pick \n";
        }
        else if(pickAndPlace==5)
        {    
            //cup Trash Place
 
            motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMinPosition, gripper_client);
            motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMaxPosition, gripper_client);
            cout<<"cup Trash Place \n";
        }
        else if(pickAndPlace==6)
        {   
            //Holder Pick
            motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMaxPosition, gripper_client);
            motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMinPosition, gripper_client);
            cout<<"Cup In holder Pick \n";
        }
        else if(pickAndPlace==7)
        {   
            //Holder Place
            motorServiceCall(1, g_gripperControlPtr->m_leftMotorParam._motorMinPosition, gripper_client);
            motorServiceCall(2, g_gripperControlPtr->m_rightMotorParam._motorMaxPosition, gripper_client);
            cout<<"cup Trash Place \n";
        }
 
        else{
            
            ros::Duration(3.5).sleep();
        }
        
 
        // ros::spinOnce();
 
        rate.sleep();
    }    
    // cout << pick_x << endl;
}
 
 
int main(int argc, char** argv)
{
 
    //Initialize Ros
    ros::init(argc, argv, "gripper_control");
    ros::NodeHandle nh;
 
    int loop_rate;
    nh.getParam("GripperControl/common/loopRate",loop_rate);
    nh.getParam("GripperControl/common/motorCmd",motor_cmd);
    nh.getParam("GripperControl/common/motorServiceName",g_motorServiceName);
    nh.getParam("GripperControl/common/subTopicPersonLocationName",g_subTopicPersonLocation);
    nh.getParam("personFollowBot/common/simulation", isSimulation);
 
    nh.getParam("GripperControl/CupPickParam/tcp_X", cupPick_x);
    nh.getParam("GripperControl/CupPickParam/tcp_Y", cupPick_y);
    nh.getParam("GripperControl/CupPickParam/tcp_Z", cupPick_z);
    nh.getParam("GripperControl/CupPickParam/tcp_RX", cupPick_rx);
    nh.getParam("GripperControl/CupPickParam/tcp_RY", cupPick_ry);
    nh.getParam("GripperControl/CupPickParam/tcp_RZ", cupPick_rz);
 
    nh.getParam("GripperControl/CupPlaceSupporter/tcp_X", cupPlaceSupport_x);
    nh.getParam("GripperControl/CupPlaceSupporter/tcp_Y", cupPlaceSupport_y);
    nh.getParam("GripperControl/CupPlaceSupporter/tcp_Z", cupPlaceSupport_z);
    nh.getParam("GripperControl/CupPlaceSupporter/tcp_RX", cupPlaceSupport_rx);
    nh.getParam("GripperControl/CupPlaceSupporter/tcp_RY", cupPlaceSupport_ry);
    nh.getParam("GripperControl/CupPlaceSupporter/tcp_RZ", cupPlaceSupport_rz);
 
    nh.getParam("GripperControl/HolderPickParam/tcp_X", holderPick_x);
    nh.getParam("GripperControl/HolderPickParam/tcp_Y", holderPick_y);
    nh.getParam("GripperControl/HolderPickParam/tcp_Z", holderPick_z);
    nh.getParam("GripperControl/HolderPickParam/tcp_RX", holderPick_rx);
    nh.getParam("GripperControl/HolderPickParam/tcp_RY", holderPick_ry);
    nh.getParam("GripperControl/HolderPickParam/tcp_RZ", holderPick_rz);
 
    nh.getParam("GripperControl/HolderPlaceParam/tcp_X", holderPlace_x);
    nh.getParam("GripperControl/HolderPlaceParam/tcp_Y", holderPlace_y);
    nh.getParam("GripperControl/HolderPlaceParam/tcp_Z", holderPlace_z);
    nh.getParam("GripperControl/HolderPlaceParam/tcp_RX", holderPlace_rx);
    nh.getParam("GripperControl/HolderPlaceParam/tcp_RY", holderPlace_ry);
    nh.getParam("GripperControl/HolderPlaceParam/tcp_RZ", holderPlace_rz);
 
    nh.getParam("GripperControl/CupInHolderPick/tcp_X", cupInHolderPick_x);
    nh.getParam("GripperControl/CupInHolderPick/tcp_Y", cupInHolderPick_y);
    nh.getParam("GripperControl/CupInHolderPick/tcp_Z", cupInHolderPick_z);
    nh.getParam("GripperControl/CupInHolderPick/tcp_RX", cupInHolderPick_rx);
    nh.getParam("GripperControl/CupInHolderPick/tcp_RY", cupInHolderPick_ry);
    nh.getParam("GripperControl/CupInHolderPick/tcp_RZ", cupInHolderPick_rz);
 
    nh.getParam("GripperControl/TrashCupPlace/tcp_X", trashCupPlace_x);
    nh.getParam("GripperControl/TrashCupPlace/tcp_Y", trashCupPlace_y);
    nh.getParam("GripperControl/TrashCupPlace/tcp_Z", trashCupPlace_z);
    nh.getParam("GripperControl/TrashCupPlace/tcp_RX", trashCupPlace_rx);
    nh.getParam("GripperControl/TrashCupPlace/tcp_RY", trashCupPlace_ry);
    nh.getParam("GripperControl/TrashCupPlace/tcp_RZ", trashCupPlace_rz);
 
    nh.getParam("GripperControl/TrashHolderPick/tcp_X", trashHolderPick_x);
    nh.getParam("GripperControl/TrashHolderPick/tcp_Y", trashHolderPick_y);
    nh.getParam("GripperControl/TrashHolderPick/tcp_Z", trashHolderPick_z);
    nh.getParam("GripperControl/TrashHolderPick/tcp_RX", trashHolderPick_rx);
    nh.getParam("GripperControl/TrashHolderPick/tcp_RY", trashHolderPick_ry);
    nh.getParam("GripperControl/TrashHolderPick/tcp_RZ", trashHolderPick_rz);
 
    nh.getParam("GripperControl/TrashHolderPlace/tcp_X", trashHolderPlace_x);
    nh.getParam("GripperControl/TrashHolderPlace/tcp_Y", trashHolderPlace_y);
    nh.getParam("GripperControl/TrashHolderPlace/tcp_Z", trashHolderPlace_z);
    nh.getParam("GripperControl/TrashHolderPlace/tcp_RX", trashHolderPlace_rx);
    nh.getParam("GripperControl/TrashHolderPlace/tcp_RY", trashHolderPlace_ry);
    nh.getParam("GripperControl/TrashHolderPlace/tcp_RZ", trashHolderPlace_rz);
 
    
    ros::Rate rate(loop_rate);
    rb_data_sub = nh.subscribe("/rb_data", 10, rb_data_callback);
    object_pos_sub = nh.subscribe("/object_postion",10, object_position_callback);
    // thread
    boost::thread setPanTiltValueThread(setMotorThread);
    
    while(ros::ok())
    {   
          
        // for(int i=0; i<6; i++)
        // {
        //     // pick_arr[i] = systemStat.sdata.tcp_ref[i];
        //     cout << "pick " << i << ": " <<pick_arr[i] << endl;
        // }
        ros::spinOnce();
        rate.sleep();
    }
    setPanTiltValueThread.join();
    
 
    return 0;
}
 
cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
#include <DynamixelShield.h>
 
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560)
  #include <SoftwareSerial.h>
  SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
  #define DEBUG_SERIAL soft_serial
#elif defined(ARDUINO_SAM_DUE) || defined(ARDUINO_SAM_ZERO)
  #define DEBUG_SERIAL SerialUSB    
#else
  #define DEBUG_SERIAL Serial
#endif
 
const uint8_t DXL_ID_first = 1;
const uint8_t DXL_ID_second = 2;
const float DXL_PROTOCOL_VERSION = 1.0;
 
DynamixelShield dxl;
 
//This namespace is required to use Control table item names
using namespace ControlTableItem;
 
void setup() {
  // put your setup code here, to run once:
  
  // For Uno, Nano, Mini, and Mega, use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);
 
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(1000000);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  // Get DYNAMIXEL information
  dxl.ping(DXL_ID_first);
 
  // Turn off torque when configuring items in EEPROM area
  dxl.torqueOff(DXL_ID_first);
  dxl.setOperatingMode(DXL_ID_first, OP_POSITION);
  dxl.torqueOn(DXL_ID_first);
}
 
void loop() {
  // put your main code here, to run repeatedly:
  
  // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. 
  // Set Goal Position in RAW value
 
 
 
  // To catch  
  dxl.setGoalPosition(DXL_ID_first, 300);
  dxl.setGoalPosition(DXL_ID_second, 100); //
  delay(1000);
  // Print present position in raw value
  DEBUG_SERIAL.print("Present Position(raw) : ");
  DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID_first));
  delay(1000);
 
  // Set Goal Position in DEGREE value
 
  // To wide
  
  dxl.setGoalPosition(DXL_ID_first, 100);
  dxl.setGoalPosition(DXL_ID_second, 300); //
  delay(1000);
  // Print present position in degree value
  DEBUG_SERIAL.print("Present Position(degree) : ");
  DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID_first, UNIT_DEGREE));
  delay(1000);
}
cs