v## For instruction on writing tutorials
 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 G120 - User node
Description: This tutorial teaches you how to write simple ROS C++ node to control the speed of SINAMICS G120 driveKeywords: SIEMENS, CP1616, SINAMICS, PROFINET, User node
Tutorial Level: INTERMEDIATE
Next Tutorial: siemens_cp1616/Tutorials/SINAMICS G120 - 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 = 4;
6 const int N_MAX = 1500;
7
8 //declarations
9 void driveTelegramCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
10 void setDriveTelegram(unsigned char drive_telegram[], int velocity);
11
12 //Main function
13 int main(int argc, char *argv[])
14 {
15 ros::init(argc, argv, "cp1616_sinamics_tutorial_node");
16 ros::NodeHandle nh;
17
18 //Subscribe to drive topic defined in yaml config file
19 ros::Subscriber sub_drive = nh.subscribe("/drive_1_input_topic", 1,
20 &driveTelegramCallback);
21
22 //Create publihser for drive topic defined in yaml config file
23 ros::Publisher pub_drive = nh.advertise<std_msgs::UInt8MultiArray>
24 ("/drive_1_output_topic", 1);
25
26 //Telegram 1
27 unsigned char drive_telegram_1[DRIVE_TELEGRAM_SIZE];
28
29 //std_msgs::MultiArray variables
30 std_msgs::MultiArrayDimension msg_dim;
31 std_msgs::UInt8MultiArray msg;
32
33 //Wait until communication starts properly
34 ros::Duration(10).sleep();
35
36 //-------------------------------------------------
37 //First telegram - Master control by PLC
38 //-------------------------------------------------
39 msg_dim.label = "Drive_1_output";
40 msg_dim.size = DRIVE_TELEGRAM_SIZE;
41 msg.layout.dim.clear();
42 msg.layout.dim.push_back(msg_dim);
43
44 msg.data.clear();
45
46 drive_telegram_1[0] = 0b00000100; //STW1 high
47 drive_telegram_1[1] = 0b00000000; //STW1 low
48 drive_telegram_1[2] = 0b00000000; //NSOLL_A high
49 drive_telegram_1[3] = 0b00000000; //NSOLL_A low
50
51 msg.data.clear();
52 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
53 msg.data.push_back(drive_telegram_1[i]);
54
55 pub_drive.publish(msg);
56
57 //Wait for 2 seconds
58 ros::Duration(2).sleep();
59
60 //--------------------------------------------------
61 //Second telegram - set ON/OFF2 ON/OFF/3 to true
62 //--------------------------------------------------
63 msg_dim.label = "Drive_1_output";
64 msg_dim.size = DRIVE_TELEGRAM_SIZE;
65 msg.layout.dim.clear();
66 msg.layout.dim.push_back(msg_dim);
67
68 msg.data.clear();
69
70 drive_telegram_1[0] = 0b00000100; //STW1 high
71 drive_telegram_1[1] = 0b10000110; //STW1 low
72 drive_telegram_1[2] = 0b00000000; //NSOLL_A high
73 drive_telegram_1[3] = 0b00000000; //NSOLL_A low
74
75 msg.data.clear();
76 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
77 msg.data.push_back(drive_telegram_1[i]);
78
79 pub_drive.publish(msg);
80
81 //Wait for 2 seconds
82 ros::Duration(2).sleep();
83
84 //---------------------------------------------------------------
85 //Third telegram - set all necessary bytes except ON/OFF1 to true
86 //----------------------------------------------------------------
87 msg_dim.label = "Drive_1_output";
88 msg_dim.size = DRIVE_TELEGRAM_SIZE;
89 msg.layout.dim.clear();
90 msg.layout.dim.push_back(msg_dim);
91
92 msg.data.clear();
93
94 drive_telegram_1[0] = 0b00000100; //STW1 high
95 drive_telegram_1[1] = 0b11111110; //STW1 low
96 drive_telegram_1[2] = 0b00000000; //NSOLL_A high
97 drive_telegram_1[3] = 0b00000000; //NSOLL_A low
98
99 msg.data.clear();
100 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
101 msg.data.push_back(drive_telegram_1[i]);
102
103 pub_drive.publish(msg);
104
105 //Wait for 2 seconds
106 ros::Duration(2).sleep();
107
108 //-------------------------------------------
109 // Main loop
110 //-------------------------------------------
111 while(ros::ok())
112 {
113 ROS_INFO("Goal speed: 500");
114
115 //message header
116 msg_dim.label = "Drive_1_output";
117 msg_dim.size = DRIVE_TELEGRAM_SIZE;
118 msg.layout.dim.clear();
119 msg.layout.dim.push_back(msg_dim);
120
121 setDriveTelegram(drive_telegram_1, 500);
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_1[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
137 ROS_INFO("Goal speed: 1000");
138 //message header
139 msg_dim.label = "Drive_1_output";
140 msg_dim.size = DRIVE_TELEGRAM_SIZE;
141 msg.layout.dim.clear();
142 msg.layout.dim.push_back(msg_dim);
143
144 setDriveTelegram(drive_telegram_1, 1000);
145
146 //copy telegram data
147 msg.data.clear();
148 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
149 msg.data.push_back(drive_telegram_1[i]);
150
151 //publish
152 pub_drive.publish(msg);
153
154 //wait for 5 seconds
155 ros::Duration(5).sleep();
156 ros::spinOnce();
157 }
158 return (EXIT_SUCCESS);
159 }
160
161 void setDriveTelegram(unsigned char drive_telegram[], int velocity)
162 {
163 //Scale to 0x4000 = 100%
164 int velocity_rel = (velocity * 0x4000) / N_MAX;
165
166 //Decompose velocity from int to 2*unsigned char
167 unsigned char velocity_bytes[2];
168
169 velocity_bytes[0] = (velocity_rel >> 8) & 0xFF;
170 velocity_bytes[1] = velocity_rel & 0xFF;
171
172 //Set telegram data
173 drive_telegram[0] = 0b00000100; //STW1 high
174 drive_telegram[1] = 0b11111111; //STW1 low
175 drive_telegram[2] = velocity_bytes[0]; //NSOLL_A high
176 drive_telegram[3] = velocity_bytes[1]; //NSOLL_A low
177 }
178
179 void driveTelegramCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
180 {
181 unsigned int received_byte_array[DRIVE_TELEGRAM_SIZE];
182
183 for(unsigned int i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
184 received_byte_array[i] = msg->data[i];
185 }
The code explained
In addition to ros.h we also need to include std_msgs::UInt8MultiArray headers:
Declare callback function for input wrapper topic:
9 void driveTelegramCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
Subscribe to input wrapper topic (CU240 -> CP1616) defined in yaml config file:
Create publisher for output wrapper topic (CP1616 -> CU240) as defined in yaml config file:
Create drive_telegram_1 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 telegram 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 drive_telegram_1 data with intial values - set Master control by PLC bit on:
Fill msg variable and publish First telegram to output wrapper topic:
To switch SINAMICS drive on, system needs to pass several initialization states. First three telegrams handle this initialization by switching bits of control word in correct order. While telegram sets Master control by PLC on, second switch on ON/OFF2,3 bits and third swtich on all remaining bits except ON/OFF1:
60 //--------------------------------------------------
61 //Second telegram - set ON/OFF2 ON/OFF/3 to true
62 //--------------------------------------------------
63 msg_dim.label = "Drive_1_output";
64 msg_dim.size = DRIVE_TELEGRAM_SIZE;
65 msg.layout.dim.clear();
66 msg.layout.dim.push_back(msg_dim);
67
68 msg.data.clear();
69
70 drive_telegram_1[0] = 0b00000100; //STW1 high
71 drive_telegram_1[1] = 0b10000110; //STW1 low
72 drive_telegram_1[2] = 0b00000000; //NSOLL_A high
73 drive_telegram_1[3] = 0b00000000; //NSOLL_A low
74
75 msg.data.clear();
76 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
77 msg.data.push_back(drive_telegram_1[i]);
78
79 pub_drive.publish(msg);
80
81 //Wait for 2 seconds
82 ros::Duration(2).sleep();
83
84 //---------------------------------------------------------------
85 //Third telegram - set all necessary bytes except ON/OFF1 to true
86 //----------------------------------------------------------------
87 msg_dim.label = "Drive_1_output";
88 msg_dim.size = DRIVE_TELEGRAM_SIZE;
89 msg.layout.dim.clear();
90 msg.layout.dim.push_back(msg_dim);
91
92 msg.data.clear();
93
94 drive_telegram_1[0] = 0b00000100; //STW1 high
95 drive_telegram_1[1] = 0b11111110; //STW1 low
96 drive_telegram_1[2] = 0b00000000; //NSOLL_A high
97 drive_telegram_1[3] = 0b00000000; //NSOLL_A low
98
99 msg.data.clear();
100 for(size_t i = 0; i < DRIVE_TELEGRAM_SIZE; i++)
101 msg.data.push_back(drive_telegram_1[i]);
102
103 pub_drive.publish(msg);
104
105 //Wait for 2 seconds
106 ros::Duration(2).sleep();
After entering main loop, setDriveTelegram() function is used to set ON/OFF1 bit to true and for calculation and scalling of speed setpoint:
115 //message header
116 msg_dim.label = "Drive_1_output";
117 msg_dim.size = DRIVE_TELEGRAM_SIZE;
118 msg.layout.dim.clear();
119 msg.layout.dim.push_back(msg_dim);
120
121 setDriveTelegram(drive_telegram_1, 500);
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_1[i]);
127
128 //publish
129 pub_drive.publish(msg);
Speed setpoint scalling: Siemens uses 0x4000 = 100% notation. It means that if max speed is for example 1500, in order to reach maximum you need to send 0x4000 hex = 16384 dec velocity command (NSOLL_A). setDriveTelegram does this scalling, accepting goal speed in rpm as an input.
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 G120 - Runtime
