 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
SINAMICS S120 - User node
Description: This tutorial teaches you how to create a simple ROS C++ node for position control of SINAMICS S120 driveKeywords: SIEMENS, CP1616, PROFINET, SINAMICS, IO Controller, C++
Tutorial Level: INTERMEDIATE
Next Tutorial: siemens_cp1616/Tutorials/SINAMICS S120 - Runtime
This tutorial teaches you how to publish and subscribe data to topics generated by PROFINET IO Controller wrapper node. Following code is intended for use with hardware configuration described in previous tutorial. Complete IO Controller tutorial package is available in github siemens_tutorials repository.
The code
1 #include <ros/ros.h>
2 #include <std_msgs/MultiArrayDimension.h>
3 #include <std_msgs/UInt8MultiArray.h>
4
5 const int DRIVE_TELEGRAM_SIZE = 24;
6
7 //declarations
8 void driveTelegramCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
9 void setDriveTelegram(unsigned char drive_telegram[], int possition, int velocity);
10
11 //Main function
12 int main(int argc, char *argv[])
13 {
14 ros::init(argc, argv, "cp1616_sinamics_tutorial_node");
15 ros::NodeHandle nh;
16
17 //Subscribe to drive topic defined in yaml config file
18 ros::Subscriber sub_drive = nh.subscribe("/drive_1_input_topic",
19 1, &driveTelegramCallback);
20
21 //Create publihser for drive topic defined in yaml config file
22 ros::Publisher pub_drive = nh.advertise<std_msgs::UInt8MultiArray>
23 ("/drive_1_output_topic", 1);
24
25 //Telegram 111
26 unsigned char drive_telegram_111[DRIVE_TELEGRAM_SIZE];
27
28 //std_msgs::MultiArray variables
29 std_msgs::MultiArrayDimension msg_dim;
30 std_msgs::UInt8MultiArray msg;
31
32 //Wait until communication starts properly
33 ros::Duration(5).sleep();
34
35 //-------------------------------------------
36 //Send initialization DRIVE telegram
37 // - set all necessary bytes except ON/OFF1 to true
38 // - set reference point to true
39 //-------------------------------------------
40 msg_dim.label = "Drive_1_output";
41 msg_dim.size = DRIVE_TELEGRAM_SIZE;
42 msg.layout.dim.clear();
43 msg.layout.dim.push_back(msg_dim);
44
45 msg.data.clear();
46
47 drive_telegram_111[0] = 0b00000100; //STW1 high
48 drive_telegram_111[1] = 0b10111110; //STW1 low
49 drive_telegram_111[2] = 0b00000000; //POS_STW1 high
50 drive_telegram_111[3] = 0b00000000; //POS_STW1 low
51 drive_telegram_111[4] = 0b00000000; //POS_STW2 high
52 drive_telegram_111[5] = 0b00000010; //POS_STW2 low
53 drive_telegram_111[6] = 0b00000000; //STW2 high
54 drive_telegram_111[7] = 0b00000000; //STW2 low
55
56 drive_telegram_111[8] = 0; //OVERRIDE high
57 drive_telegram_111[9] = 0; //OVERRIDE low
58 drive_telegram_111[10] = 0; //MDI_TARPOS_6 high
59 drive_telegram_111[11] = 0; //MDI_TARPOS_6 low
60 drive_telegram_111[12] = 0; //MDI_TARPOS_7 high
61 drive_telegram_111[13] = 0; //MDI_TARPOS_7 low
62 drive_telegram_111[14] = 0; //MDI_VELOCITY_8 high
63 drive_telegram_111[15] = 0; //MDI_VELOCITY_8 low
64
65 drive_telegram_111[16] = 0; //MDI_VELOCITY_9 high
66 drive_telegram_111[17] = 0; //MDI_VELOCITY_9 low
67 drive_telegram_111[18] = 0; //MDI_ACC high
68 drive_telegram_111[19] = 0; //MDI_ACC low
69 drive_telegram_111[20] = 0; //MDI_DEC high
70 drive_telegram_111[21] = 0; //MDI_DEC low
71 drive_telegram_111[22] = 0; //Reserved
72 drive_telegram_111[23] = 0; //Reserved
73
74 msg.data.clear();
75 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
76 msg.data.push_back(drive_telegram_111[i]);
77
78 pub_drive.publish(msg);
79
80 //Wait for 5 seconds
81 ros::Duration(5).sleep();
82
83 //-------------------------------------------
84 // Main loop
85 //-------------------------------------------
86 while(ros::ok())
87 {
88
89 ROS_INFO("Moving to position 1");
90 //message header
91 msg_dim.label = "Drive_1_output";
92 msg_dim.size = DRIVE_TELEGRAM_SIZE;
93 msg.layout.dim.clear();
94 msg.layout.dim.push_back(msg_dim);
95
96 //fill telegram data with position and velocity values
97 setDriveTelegram(drive_telegram_111, 1000, 100);
98
99 //copy telegram data
100 msg.data.clear();
101 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
102 msg.data.push_back(drive_telegram_111[i]);
103
104 //publish
105 pub_drive.publish(msg);
106
107 //wait for 5 seconds
108 ros::Duration(5).sleep();
109 ros::spinOnce();
110
111 //======================================================
112
113 ROS_INFO("Moving to position 2");
114 //message header
115 msg_dim.label = "Drive_1_output";
116 msg_dim.size = DRIVE_TELEGRAM_SIZE;
117 msg.layout.dim.clear();
118 msg.layout.dim.push_back(msg_dim);
119
120 //fill telegram data with position and velocity values
121 setDriveTelegram(drive_telegram_111,-1000, 100);
122
123 //copy telegram data
124 msg.data.clear();
125 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
126 msg.data.push_back(drive_telegram_111[i]);
127
128 //publish
129 pub_drive.publish(msg);
130
131 //wait for 5 seconds
132 ros::Duration(5).sleep();
133 ros::spinOnce();
134 }
135
136 return (EXIT_SUCCESS);
137 }
138
139
140 void setDriveTelegram(unsigned char drive_telegram[], int position, int velocity)
141 {
142 //Decompose position from int to 4*unsigned char
143 unsigned char position_bytes[4];
144
145 position_bytes[0] = (position >> 24) & 0xFF;
146 position_bytes[1] = (position >> 16) & 0xFF;
147 position_bytes[2] = (position >> 8) & 0xFF;
148 position_bytes[3] = position & 0xFF;
149
150 //Decompose velocity from int to 4*unsigned char
151 unsigned char velocity_bytes[4];
152
153 velocity_bytes[0] = (velocity >> 24) & 0xFF;
154 velocity_bytes[1] = (velocity >> 16) & 0xFF;
155 velocity_bytes[2] = (velocity >> 8) & 0xFF;
156 velocity_bytes[3] = velocity & 0xFF;
157
158 //Set telegram data
159 drive_telegram[0] = 0b00000100; //STW1 low
160 drive_telegram[1] = 0b10111111; //STW1 high
161 drive_telegram[2] = 0b10010001; //POS_STW1 low
162 drive_telegram[3] = 0b00000000; //POS_STW1 high
163 drive_telegram[4] = 0b00000000; //POS_STW2 low
164 drive_telegram[5] = 0b00000000; //POS_STW2 high
165 drive_telegram[6] = 0b00000000; //STW2 low
166 drive_telegram[7] = 0b00000000; //STW2 high
167
168 drive_telegram[8] = 0x40; //OVERRIDE high = 0x4000 = 100%
169 drive_telegram[9] = 0x00; //OVERRIDE low
170 drive_telegram[10] = position_bytes[0]; //MDI_TARPOS_6 high
171 drive_telegram[11] = position_bytes[1]; //MDI_TARPOS_6 low
172 drive_telegram[12] = position_bytes[2]; //MDI_TARPOS_7 high
173 drive_telegram[13] = position_bytes[3]; //MDI_TARPOS_7 low
174 drive_telegram[14] = velocity_bytes[0]; //MDI_VELOCITY_8 high
175 drive_telegram[15] = velocity_bytes[1]; //MDI_VELOCITY_8 low
176
177 drive_telegram[16] = velocity_bytes[2]; //MDI_VELOCITY_9 high
178 drive_telegram[17] = velocity_bytes[3]; //MDI_VELOCITY_9 low
179 drive_telegram[18] = 0x40; //MDI_ACC high = 0x4000 = 100%
180 drive_telegram[19] = 0; //MDI_ACC low
181 drive_telegram[20] = 0x40; //MDI_DEC high = 0x4000 = 100%
182 drive_telegram[21] = 0; //MDI_DEC low
183 drive_telegram[22] = 0; //Reserved
184 drive_telegram[23] = 0; //Reserved
185
186 }
187
188 void driveTelegramCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
189 {
190 unsigned int received_byte_array[DRIVE_TELEGRAM_SIZE];
191
192 for(unsigned int i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
193 received_byte_array[i] = msg->data[i];
194 }
The code explained
In addition to ros.h we also need to include std_msgs::UInt8MultiArray headers:
Declare callback function for input wrapper topic:
8 void driveTelegramCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
Subscribe to input wrapper topic (CU320 -> CP1616) defined in yaml config file:
Create publisher for output wrapper topic (CP1616 -> CU320) as defined in yaml config file:
Create drive_telegram_111 and std_msgs::MultiArray variables:
Following "sleep" method is very important, since PROFINET communication takes some time to start after launch file was executed. If you are experiencing problems, e.g. motor is not willing to initialize even despite the fact, that all data are being transmitted properly, change duration parameter, or add one aditional message to the initialization step.
To send a message in MultiArray format to the wrapper, you need to fill the "label" and "size" in "msg.layout.dim" structure in accordance with yaml config file first.
Fill PZD telegram 111 data with intial values:
47 drive_telegram_111[0] = 0b00000100; //STW1 high
48 drive_telegram_111[1] = 0b10111110; //STW1 low
49 drive_telegram_111[2] = 0b00000000; //POS_STW1 high
50 drive_telegram_111[3] = 0b00000000; //POS_STW1 low
51 drive_telegram_111[4] = 0b00000000; //POS_STW2 high
52 drive_telegram_111[5] = 0b00000010; //POS_STW2 low
53 drive_telegram_111[6] = 0b00000000; //STW2 high
54 drive_telegram_111[7] = 0b00000000; //STW2 low
55
56 drive_telegram_111[8] = 0; //OVERRIDE high
57 drive_telegram_111[9] = 0; //OVERRIDE low
58 drive_telegram_111[10] = 0; //MDI_TARPOS_6 high
59 drive_telegram_111[11] = 0; //MDI_TARPOS_6 low
60 drive_telegram_111[12] = 0; //MDI_TARPOS_7 high
61 drive_telegram_111[13] = 0; //MDI_TARPOS_7 low
62 drive_telegram_111[14] = 0; //MDI_VELOCITY_8 high
63 drive_telegram_111[15] = 0; //MDI_VELOCITY_8 low
64
65 drive_telegram_111[16] = 0; //MDI_VELOCITY_9 high
66 drive_telegram_111[17] = 0; //MDI_VELOCITY_9 low
67 drive_telegram_111[18] = 0; //MDI_ACC high
68 drive_telegram_111[19] = 0; //MDI_ACC low
69 drive_telegram_111[20] = 0; //MDI_DEC high
70 drive_telegram_111[21] = 0; //MDI_DEC low
71 drive_telegram_111[22] = 0; //Reserved
72 drive_telegram_111[23] = 0; //Reserved
73
Fill msg variable and publish to topic to initialize the drive, set reference point and acknowledge faults:
Enter the loop and use setDriveTelegram function to set ON/OFF1 to 1 and fill drive_telegram_111 with goal position and velocity values. The while loop provide simple positioning sequence - half a turn clockwise and half a turn counter-clockwise with respect to reference position:
86 while(ros::ok())
87 {
88
89 ROS_INFO("Moving to position 1");
90 //message header
91 msg_dim.label = "Drive_1_output";
92 msg_dim.size = DRIVE_TELEGRAM_SIZE;
93 msg.layout.dim.clear();
94 msg.layout.dim.push_back(msg_dim);
95
96 //fill telegram data with position and velocity values
97 setDriveTelegram(drive_telegram_111, 1000, 100);
98
99 //copy telegram data
100 msg.data.clear();
101 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
102 msg.data.push_back(drive_telegram_111[i]);
103
104 //publish
105 pub_drive.publish(msg);
106
107 //wait for 5 seconds
108 ros::Duration(5).sleep();
109 ros::spinOnce();
110
111 //======================================================
112
113 ROS_INFO("Moving to position 2");
114 //message header
115 msg_dim.label = "Drive_1_output";
116 msg_dim.size = DRIVE_TELEGRAM_SIZE;
117 msg.layout.dim.clear();
118 msg.layout.dim.push_back(msg_dim);
119
120 //fill telegram data with position and velocity values
121 setDriveTelegram(drive_telegram_111,-1000, 100);
122
123 //copy telegram data
124 msg.data.clear();
125 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
126 msg.data.push_back(drive_telegram_111[i]);
127
128 //publish
129 pub_drive.publish(msg);
130
131 //wait for 5 seconds
132 ros::Duration(5).sleep();
133 ros::spinOnce();
134 }
135
136 return (EXIT_SUCCESS);
137 }
Notice the least significant bit in STW1 - it is ON/OFF1 bit:
driveTelegramCallback is called each time when data are recieved on input topic. Add your own code if you need to do some aditional processing:
If you completed and successfully built your node, you can proceed to following tutorial and test the PROFINET communication between CP1616 and your CU.
Next tutorial:SINAMICS S120 - Runtime
