├── .gitignore ├── README.md ├── ethercat_master ├── CMakeLists.txt ├── package.xml └── src │ ├── config │ ├── control │ │ └── control.xml │ └── io │ │ ├── pdo.xml │ │ └── topology.xml │ ├── control │ ├── DigitalIO.cpp │ ├── DigitalIO.h │ ├── Drive.cpp │ ├── Drive.h │ ├── DriveSdo.cpp │ ├── DriveSdo.h │ ├── MainControlLoop.cpp │ └── MainControlLoop.h │ └── main.cpp ├── motion_control ├── CMakeLists.txt ├── launch │ └── motion_control.launch ├── package.xml └── scripts │ ├── dio.py │ ├── drive.py │ ├── test_SDO_get_n_set.py │ ├── test_ctrl.py │ ├── test_get_SDO.py │ ├── test_restore_params.py │ ├── test_save_params.py │ └── test_set_SDO.py ├── motorcortex_msgs ├── CMakeLists.txt ├── msg │ ├── DigitalInputs.msg │ ├── DigitalInputsList.msg │ ├── DigitalOutputs.msg │ ├── DigitalOutputsList.msg │ ├── DriveIn.msg │ ├── DriveInList.msg │ ├── DriveOut.msg │ ├── DriveOutList.msg │ ├── PositionControllerCfg.msg │ ├── TorqueControllerCfg.msg │ └── VelocityControllerCfg.msg ├── package.xml └── srv │ ├── GetSDOCfg.srv │ ├── RestoreCfgParams.srv │ ├── SaveCfgParams.srv │ └── SetSDOCfg.srv └── ros_ethercat_igh ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | 106 | # PyCharm 107 | .idea/ 108 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ros_ethercat_igh 2 | ---------------- 3 | 4 | # Overview 5 | 6 | This is a generic implementation of a ROS-wrapped EtherCAT Master controller based on the [IgH EtherLab (R)](https://www.etherlab.org/en/what.php) EtherCAT Master Driver for Linux. The ROS EtherCAT master implementation is based on [MotorCortex (TM) Core Library](https://git.vectioneer.com/pub/motorcortex-dist/wikis/home) provided by [Vectioneer](http://www.vectioneer.com/) as an installable Debian package in a binary form. 7 | 8 | The maintained IgH driver installer maintained by [Synapticon](https://www.synapticon.com/) can be downloaded from [here](https://github.com/synapticon/Etherlab_EtherCAT_Master) 9 | 10 | MotorCortex (TM) Core Debian packages can be downloaded from [here](http://git.vectioneer.com:30000/pub/motorcortex-dist/wikis/home). To control EtherCAT slave devices you requre the following packages to be downloaded matching with your version of Ubuntu OS: 11 | 12 | * Nanomsg Ubuntu 13 | * Motorcortex-core Ubuntu 14 | 15 | # Installation procedures 16 | ## ROS 17 | 18 | To install ROS, please refer to the original ROS installation documentation at [http://www.ros.org/install/](http://www.ros.org/install/) 19 | 20 | We have developed and tested this package in Ubuntu 16.04 and ROS Kinetic. Although theoretically other versions should be supported, we recommned if possible to start with this configuration. 21 | 22 | ## IgH EtherCAT Master 23 | 24 | ```sh 25 | $ git clone https://github.com/synapticon/Etherlab_EtherCAT_Master.git 26 | $ cd Etherlab_EtherCAT_Master/sncn_installer 27 | $ chmod +x install.sh && ./install.sh 28 | ``` 29 | 30 | After the installation, if you have more than one Ethernet port, you may need to adjust the ethercat configuration file to use your port. 31 | 32 | ```sh 33 | $ sudo gedit /etc/sysconfig/ethercat 34 | ``` 35 | Find the line `MASTER0_DEVICE=""` and add your port's MAC address instead the one filled in automatically. 36 | 37 | To start the driver: 38 | ```sh 39 | $ sudo service ethercat start 40 | ``` 41 | or 42 | ```sh 43 | $ sudo /etc/init.d/ethercat start 44 | ``` 45 | 46 | To make sure your device is recognized and listed, please type 47 | 48 | ```sh 49 | $ ethercat slaves 50 | ``` 51 | Normally, you should have EtherCAT slave devices listed similar to that: 52 | 53 | ``` 54 | 0 0:0 PREOP + SOMANET CiA402 Drive 55 | 1 0:1 PREOP + SOMANET CiA402 Drive 56 | ``` 57 | 58 | ## MotorCortex (TM) Core Library 59 | 60 | Navigate to the directory containing the downloaded debian packages and then type: 61 | 62 | ```sh 63 | sudo dpkg -i nanomsg-1.1.4-Linux.deb 64 | sudo dpkg -i motorcortex-core-0.9.8-Linux.deb 65 | ``` 66 | Alternatively, double-clicking on the package should open the installation dialog. 67 | 68 | ## This ROS package 69 | 70 | The package contains several modules: 71 | * ethercat_master -> EtherCAT master server application 72 | * motion_control -> Set of Python test scripts demonstrating how to control devices and other useful features 73 | * motorcortex_msgs -> Custom messages for client applications 74 | * ros_ethercat_igh -> Metha package 75 | 76 | ### Building 77 | At this point you should be all set to start using this ROS package to control your devices. If you haven't yet cloned it, please do so into your [catkin configured workspace](http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment). 78 | ```sh 79 | $ cd ~/catkin_ws/src 80 | $ git clone https://github.com/synapticon/ros_ethercat_igh.git 81 | $ cd ~/catkin_ws 82 | $ catkin build 83 | ``` 84 | If you were using `catkin_make` command before, you may need to remove `build` and `devel` folders in your catkin workspace for `catkin build` command to work. Otherwise continue using `catkin_make`. If with `catkin_make` the build will fail first time because the custom message headers cannot be found, please repeat several times to build. We are working on a solution. 85 | 86 | #### Known issues with sncn_installer 87 | It may happen that EtherCAT library is installed in `/opt/etherlab/`. The package will not compile throwing usually 88 | ```sh 89 | fatal error: ecrt.h: No such file or directory 90 | ``` 91 | You can check the location of the header file using [locate](https://www.howtoforge.com/tutorial/linux-search-files-from-the-terminal/) command. 92 | 93 | As a solution, please copy the headers and libraries into the standard location expected by the previously installed motorcortex debian packages: 94 | ```sh 95 | $ cd /opt/etherlab/ 96 | $ sudo cp include/* /usr/include/ 97 | $ sudo cp -r lib/* /usr/lib/ 98 | ``` 99 | After that, please try to recompile the ros package using `catkin_make` or `catkin build` command. 100 | 101 | ### Configuring 102 | #### Bus topology 103 | The ethercat_master package requires the information about your EtherCAT slave devices topology. 104 | 105 | in `ethercat_master/src/config/io/` you'll find an example of such a script `topology.xml`. You don't need to write it by yourself but use IgH EtherLab (R) tool to generate it: 106 | 107 | ```sh 108 | $ ethercat xml > topology.xml 109 | ``` 110 | Now, if you are using Synapticon's [SOMANET Servo Drives](https://www.synapticon.com/products/somanet/somanet-servo-drives) you can just extend the content of the `pdo.xml` to the number of devices you are using. 111 | Please be cautious about the tags. There are two types of slave devices are supported: 112 | * Servo Drives: `````` with the `Id` corresponding to the topology of the bus starting from `0`, and the associated name starting from `axis1` 113 | * DIO modules ``` ``` with the `Id` corresponding to the topology of the bus, and the associated name starting from `device1` 114 | 115 | #### Linking your application variables to EtherCAT slave devices' Object Dictionary 116 | 117 | ##### 1. Process Data Objects (PDO) - realtime data 118 | 119 | After updating the XML configurations, if you are not using Synapticon devices, you need to register paths on the master application. For this, please edit the `ethercat_master/src/control/Drive.cpp` file. The only code you need to modify there is in the `initPhase1_()` method. 120 | Let's explain it on the `Position Value` parameter and Process Data Object. Please exemine the line: 121 | ``` 122 | addParameter("positionValue", ParameterType::INPUT, &driveFeedback_.position_value); 123 | ``` 124 | We are adding a parameter of a type INPUT (it is a value transfered from the slave to master, so it is considered as input from the master side) named `"positionValue"` and linking it to our internal variable `driveFeedback_.position_value` in this case being a member of the Drive Feedback ROS message (`motorcortex_msgs::DriveIn driveFeedback_`). This procedure creates an internal path ```root/Control/axisX/positionValue```, where `X` in `axisX` is automatically assigned enumeration starting from 1. 125 | 126 | Please now navigate to your `pdo.xml` file (`thercat_master/src/config/io/pdo.xml`). There you'll find the following entry: 127 | ``` 128 | 129 | INT32 130 | root/Control/axis1/positionValue 131 | 132 | ``` 133 | You may spot the already familiar line `root/Control/axis1/positionValue` linking your registered `"positionValue"` variable to the PDO Entry `Entry="6064:0"` At this point you should become familiar enough how to repeate the same procedure for all other variables and Process Data Objects you would like to link. 134 | 135 | ##### 2. Service Data Objects (SDO) - configuration parameters 136 | The same logic exists for Service Data Objects (SDO). Please navigate to `ethercat_master/src/control/DriveSdo.cpp` file. Let's examine this line: 137 | ``` 138 | param = addParameter("velocityControllerKp", ParameterType::PARAMETER, &sdoCfg_.velocityControllerCfg.controller_Kp); 139 | handles_.push_back(param); 140 | ``` 141 | We are adding a parameter of a type PARAMETER named `"velocityControllerKp"` and linking it to our internal variable `sdoCfg_.velocityControllerCfg.controller_Kp` in this case being a member of the `SDOCfg` service and contained data structure: 142 | ``` 143 | struct SDOCfg { 144 | motorcortex_msgs::TorqueControllerCfg torqueControllerCfg; 145 | motorcortex_msgs::VelocityControllerCfg velocityControllerCfg; 146 | motorcortex_msgs::PositionControllerCfg positionControllerCfg; 147 | }; 148 | ``` 149 | This procedure creates two internal pathes `root/Control/axisX/SDORead/velocityControllerKp` and `root/Control/axisX/SDOWrite/velocityControllerKp`, where `X` in `axisX` is automatically assigned enumeration starting from 1. This is a result of creating two submodules in `ethercat_master/src/control/Drive.cpp` `create_()` method. One is used for writing the value and one for reading both linked to the same variable `"velocityControllerKp"`: 150 | 151 | ``` 152 | createSubmodule(&drive_sdo_read_, "SDORead"); 153 | createSubmodule(&drive_sdo_write_, "SDOWrite"); 154 | ``` 155 | 156 | Please now navigate to your `pdo.xml` file (`thercat_master/src/config/io/pdo.xml`). There you'll find the following entry: 157 | ``` 158 | 159 | FLOAT 160 | root/Control/axis1/SDORead/velocityControllerKp 161 | root/Control/axis1/SDOWrite/velocityControllerKp 162 | 163 | ``` 164 | You may spot again the already familiar lines `root/Control/axis1/SDORead/velocityControllerKp` and `root/Control/axis1/SDOWrite/velocityControllerKp`, one is linking the `Sdo Entry="2011:1"` on writing, and one on reading. From this point you should be able to link your other parameters in a similar way. 165 | 166 | ##### 3. Defining the amount of devices to be used 167 | 168 | Once you've configured and linked your variables, you need tel the server application how many instances to control to be created. For this please navigate to `main.cpp` file inside the `ethercat_master/src/` folder. You'll find the following lines: 169 | ``` 170 | #define NUM_OF_DRIVES 2 171 | #define NUM_OD_DIOS 0 172 | ``` 173 | 174 | Please adjust the number according to the amount of devices you want to control. It is possible to have more devices included in the `topology.xml` and `pdo.xml` but control fewer of them. 175 | 176 | ##### 4. Rebuild the server application after making the changes 177 | ```sh 178 | $ cd ~/catkin_ws 179 | $ catkin build 180 | ``` 181 | ##### 5. Run the `ehtercat_master` server application 182 | 183 | For your convinience there is a launch file. Please execute: 184 | ```sh 185 | roslaunch motion_control motion_control.launch 186 | ``` 187 | 188 | ### Motion control test applications 189 | 190 | The `motion_control` package contains a set of python scripts to test basic functionality and to guide you how to develop your own application. 191 | 192 | * test_ctrl.py -> main Drives and DIO control application 193 | * Uses classes drive.py and dio.py 194 | * test_get_SDO.py -> service example to read your configuration parameters over SDOs 195 | * test_set_SDO.py -> service example to write your configuration parameters over SDOs 196 | * test_SDO_get_n_set.py -> service example to read, modify, and then write your configuration parameters over SDOs 197 | * test_restore_params.py -> service example to restore your configuration parameters from saved on a device 198 | * test_save_params.py -> service example to save your configuration parameters to a device 199 | 200 | #### Examining `test_ctrl.py` 201 | 202 | This is a simple application to make your motors turn in different operational modes and generate pulses on outputs of your DIO module. Let's exemine the code line by line. 203 | 204 | ``` 205 | from __future__ import print_function, division 206 | 207 | import rospy 208 | from drive import Drive, OpModesCiA402 209 | from dio import DIO 210 | 211 | from motorcortex_msgs.msg import DriveOutList, DriveInList, DigitalInputsList, DigitalOutputsList 212 | ``` 213 | Here we are importing all the dependancies and our custom ROS messages to be used by the application. The messages are defined in the `motorcortex_msgs` package. 214 | 215 | ``` 216 | # list your slave devices here 217 | drives = [Drive(0), Drive(1)] 218 | dios = [] # for example, [DIO(0)] 219 | ``` 220 | The first think you need to do is to list your devices. In this example two Drives are listed to be controlled and no DIO module. 221 | 222 | ``` 223 | def drives_feedback_callback(drive_feedback): 224 | for drive in drives: 225 | drive.update(drive_feedback.drives_feedback[drive.getId()]) 226 | 227 | def digital_inputs_callback(digital_inputs): 228 | if not dios: 229 | rospy.logerr("The list of DIOs is empty. Please unsubscribe or list the devices") 230 | else: 231 | for dio in dios: 232 | dio.update(digital_inputs.devices_feedback[dio.getId()]) 233 | ``` 234 | Defining callback functions for Drives and DIOs. 235 | 236 | ``` 237 | # publish control commands 238 | drives_pub = rospy.Publisher('/drive_control', DriveOutList, queue_size = 1) 239 | if dios: 240 | dios_pub = rospy.Publisher('/digital_outputs', DigitalOutputsList, queue_size = 1) 241 | 242 | # subscribe to feedback topics 243 | rospy.Subscriber("/drive_feedback", DriveInList, drives_feedback_callback) 244 | if dios: 245 | rospy.Subscriber("/digital_inputs", DigitalInputsList, digital_inputs_callback) 246 | ``` 247 | Creating publishers and subscribers. In this version the DIO module is optional. 248 | 249 | ``` 250 | def safe_off(): 251 | drivesControlMsg = DriveOutList() 252 | for drive in drives: 253 | drive.switchOff() 254 | drivesControlMsg.drive_command.append(drive.encode()) 255 | 256 | digitalOutputsControlMsg = DigitalOutputsList() 257 | if dios: 258 | for dio in dios: 259 | digital_outputs = [False] * 12 260 | dio.setDigitalOutputs(digital_outputs) 261 | digitalOutputsControlMsg.devices_command.append(dio.encode()) 262 | 263 | drives_pub.publish(drivesControlMsg) 264 | if dios: 265 | dios_pub.publish(digitalOutputsControlMsg) 266 | ``` 267 | Operation to be performed on the application termination. For drives we are commanding switch off which is triggered over a quick-stop internally. The outputs of DIO modules are set to zero. 268 | 269 | Now we come to the main `controller()` function definition. 270 | ``` 271 | def controller(): 272 | rospy.init_node('drive_control', anonymous=True) 273 | rospy.on_shutdown(safe_off) 274 | r = rospy.Rate(100)#Hz 275 | ``` 276 | Here we are initializing the ROS node, defining what function to execute when we shutting it down, and defining the control loop rate at 100Hz. 277 | 278 | ``` 279 | # set your test parameters here 280 | opMode = OpModesCiA402.CSV.value 281 | positionIncrement = 1000 282 | referenceVelocity = 500 283 | referenceTorque = 50 284 | ``` 285 | Here you can select you control mode. Options are: 286 | * OpModesCiA402.CSP.value -> Cyclic Synchronous Position 287 | * OpModesCiA402.CSV.value -> Cyclic Synchronous Velocity 288 | * OpModesCiA402.CST.value -> Cyclic Synchronous Torque 289 | 290 | Then depending on your prefered control mode you can select target values. Please note, there is no ramp implemented! Sending high torque or velocity references can result in drives overcurrents. Please start with small values. 291 | 292 | ``` 293 | # switch on here 294 | while not rospy.is_shutdown(): 295 | ``` 296 | Beginning of the control routine. 297 | ``` 298 | # control here 299 | drivesControlMsg = DriveOutList() 300 | for drive in drives: 301 | if drive.hasError(): 302 | print("drive %d has error"%drive.getId()) 303 | drive.resetError() 304 | else: 305 | drive.setMode(opMode) 306 | drive.switchOn() 307 | 308 | if drive.isEnabled(): 309 | if opMode == OpModesCiA402.CSP.value: 310 | drive.setPosition(drive.getPosition() + positionIncrement) 311 | elif opMode == OpModesCiA402.CSV.value: 312 | drive.setVelocity(referenceVelocity) 313 | elif opMode == OpModesCiA402.CST.value: 314 | drive.setTorque(referenceTorque) 315 | else: 316 | drive.setPosition(drive.getPosition()) 317 | drive.setVelocity(0) 318 | drive.setTorque(0) 319 | 320 | drivesControlMsg.drive_command.append(drive.encode()) 321 | ``` 322 | The main drives control is happening here. The logic is simple. If the drives in error state, we are resetting the error. Once the error is reset, we are switching them into operational state and selected operational mode. When the drives are operational, depending on the operational mode we are commanding reference values. At the end, we are combining commands for all the drives to be published on a ROS topic. 323 | 324 | ``` 325 | digitalOutputsControlMsg = DigitalOutputsList() 326 | for dio in dios: 327 | # blink 328 | if counter < 100: 329 | output_state = True 330 | else: 331 | output_state = False 332 | 333 | digital_outputs = [output_state] * 12 334 | dio.setDigitalOutputs(digital_outputs) 335 | digitalOutputsControlMsg.devices_command.append(dio.encode()) 336 | ``` 337 | For simple Digital IO devices the routing is simpler. We are simply updating all outputs with 1 or 0 every second (loop time is 100Hz and when our counter is reaching 100 the value changes). At the end, we are combining commands for all the DIO devices to be published on a ROS topic. 338 | 339 | ``` 340 | # send here 341 | drives_pub.publish(drivesControlMsg) 342 | if dios: 343 | dios_pub.publish(digitalOutputsControlMsg) 344 | ``` 345 | Here we are sending the previously assembled messages on ROS topics. 346 | 347 | ``` counter += 1 348 | if counter > 200: 349 | counter = 0 350 | 351 | r.sleep() 352 | ``` 353 | And finally, we are updating our counter and sleep. 354 | 355 | ``` 356 | if __name__ == '__main__': 357 | try: 358 | controller() 359 | except rospy.ROSInterruptException: pass 360 | ``` 361 | Executing the controller in main. 362 | -------------------------------------------------------------------------------- /ethercat_master/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ethercat_master) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | message_generation 8 | motorcortex_msgs 9 | roscpp 10 | std_msgs 11 | roslib 12 | ) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS src/control 16 | CATKIN_DEPENDS roscpp message_runtime roslib 17 | ) 18 | 19 | include_directories( 20 | src/control 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | #add_subdirectory(control) 25 | #add_subdirectory(logic) 26 | 27 | add_executable(ethercat_master_server src/main.cpp src/control/MainControlLoop.cpp src/control/Drive.cpp 28 | src/control/DigitalIO.cpp src/control/DriveSdo.cpp) 29 | target_compile_options(ethercat_master_server PUBLIC -DPB_FIELD_32BIT -DLOG_LEVEL_INFO -DLOG_LEVEL_SM -DENABLE_ASSERT) 30 | target_link_libraries(ethercat_master_server mcx-core ${catkin_LIBRARIES}) 31 | 32 | #install(TARGETS ethercat_master_server DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) 33 | #install(DIRECTORY config DESTINATION ${CMAKE_INSTALL_PREFIX}/share/ethercat_master_server) -------------------------------------------------------------------------------- /ethercat_master/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ethercat_master 4 | 0.1.0 5 | EtherCAT Master Server package based on EtherLab IgH and Motorcortex(T) libraries 6 | 7 | Viatcheslav Tretyakov 8 | Alexey Zakharov 9 | 10 | Synapticon GmbH 11 | 12 | BSD 13 | 14 | message_generation 15 | catkin 16 | std_msgs 17 | motorcortex_msgs 18 | roscpp 19 | roslib 20 | roscpp 21 | roscpp 22 | std_msgs 23 | message_runtime 24 | roslib 25 | motorcortex_msgs 26 | 27 | 28 | -------------------------------------------------------------------------------- /ethercat_master/src/config/control/control.xml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /ethercat_master/src/config/io/pdo.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | UINT32 8 | root/Control/axis1/slaveTimestamp 9 | 10 | 11 | INT32 12 | root/Control/axis1/positionValue 13 | 14 | 15 | INT32 16 | root/Control/axis1/velocityValue 17 | 18 | 19 | INT16 20 | root/Control/axis1/torqueValue 21 | 22 | 23 | INT32 24 | root/Control/axis1/secondaryPositionValue 25 | 26 | 27 | INT32 28 | root/Control/axis1/secondaryVelocityValue 29 | 30 | 31 | UINT16 32 | root/Control/axis1/analogInput1 33 | 34 | 35 | UINT16 36 | root/Control/axis1/analogInput2 37 | 38 | 39 | UINT16 40 | root/Control/axis1/analogInput3 41 | 42 | 43 | UINT16 44 | root/Control/axis1/analogInput4 45 | 46 | 47 | BOOL 48 | root/Control/axis1/digitalInput1 49 | 50 | 51 | BOOL 52 | root/Control/axis1/digitalInput2 53 | 54 | 55 | BOOL 56 | root/Control/axis1/digitalInput3 57 | 58 | 59 | BOOL 60 | root/Control/axis1/digitalInput4 61 | 62 | 63 | 64 | INT32 65 | root/Control/axis1/targetPosition 66 | 67 | 68 | INT32 69 | root/Control/axis1/targetVelocity 70 | 71 | 72 | INT16 73 | root/Control/axis1/targetTorque 74 | 75 | 76 | INT16 77 | root/Control/axis1/torqueOffset 78 | 79 | 80 | BOOL 81 | root/Control/axis1/digitalOutput1 82 | 83 | 84 | BOOL 85 | root/Control/axis1/digitalOutput2 86 | 87 | 88 | BOOL 89 | root/Control/axis1/digitalOutput3 90 | 91 | 92 | BOOL 93 | root/Control/axis1/digitalOutput4 94 | 95 | 96 | 97 | root/cia402/driveStatusWord 98 | 99 | 100 | root/cia402/driveControlWord 101 | 102 | 103 | root/cia402/driveOpMode 104 | 105 | 106 | 107 | 108 | root/cia402/uploadErrorCode 109 | root/Control/axis1/driveErrorCode 110 | 111 | 112 | 113 | UINT32 114 | root/Control/axis1/SDOWrite/saveAllParameters 115 | 116 | 117 | UINT32 118 | root/Control/axis1/SDOWrite/restoreAllParameters 119 | 120 | 121 | 122 | root/Control/axis1/SDORead/torqueControllerKp 123 | 124 | FLOAT 125 | 126 | 127 | FLOAT 128 | root/Control/axis1/SDORead/torqueControllerKi 129 | 130 | 131 | 132 | FLOAT 133 | root/Control/axis1/SDORead/torqueControllerKd 134 | 135 | 136 | 137 | UINT32 138 | root/Control/axis1/SDORead/fieldWeakeningEnable 139 | 140 | 141 | 142 | UINT32 143 | root/Control/axis1/SDORead/fieldWeakeningPercentage 144 | 145 | 146 | 147 | UINT32 148 | root/Control/axis1/SDORead/fieldWeakeningStartingSpeed 149 | 150 | 151 | 152 | UINT32 153 | root/Control/axis1/SDORead/fieldWeakeningEndingSpeed 154 | 155 | 156 | 157 | UINT32 158 | root/Control/axis1/SDORead/commutationAngleMeasurementDelay 159 | 160 | 161 | 162 | 163 | 164 | FLOAT 165 | root/Control/axis1/SDORead/velocityControllerKp 166 | root/Control/axis1/SDOWrite/velocityControllerKp 167 | 168 | 169 | FLOAT 170 | root/Control/axis1/SDORead/velocityControllerKi 171 | root/Control/axis1/SDOWrite/velocityControllerKi 172 | 173 | 174 | FLOAT 175 | root/Control/axis1/SDORead/velocityControllerKd 176 | root/Control/axis1/SDOWrite/velocityControllerKd 177 | 178 | 179 | UINT32 180 | root/Control/axis1/SDORead/velocityControllerIntegralLimit 181 | root/Control/axis1/SDOWrite/velocityControllerIntegralLimit 182 | 183 | 184 | 185 | 186 | FLOAT 187 | root/Control/axis1/SDORead/positionControllerPositionLoopKp 188 | root/Control/axis1/SDOWrite/positionControllerPositionLoopKp 189 | 190 | 191 | FLOAT 192 | root/Control/axis1/SDORead/positionControllerPositionLoopKi 193 | root/Control/axis1/SDOWrite/positionControllerPositionLoopKi 194 | 195 | 196 | FLOAT 197 | root/Control/axis1/SDORead/positionControllerPositionLoopKd 198 | root/Control/axis1/SDOWrite/positionControllerPositionLoopKd 199 | 200 | 201 | UINT32 202 | root/Control/axis1/SDORead/positionControllerPositionLoopIntegralLimit 203 | root/Control/axis1/SDOWrite/positionControllerPositionLoopIntegralLimit 204 | 205 | 206 | FLOAT 207 | root/Control/axis1/SDORead/positionControllerVelocityLoopKp 208 | root/Control/axis1/SDOWrite/positionControllerVelocityLoopKp 209 | 210 | 211 | FLOAT 212 | root/Control/axis1/SDORead/positionControllerVelocityLoopKi 213 | root/Control/axis1/SDOWrite/positionControllerVelocityLoopKi 214 | 215 | 216 | FLOAT 217 | root/Control/axis1/SDORead/positionControllerVelocityLoopKd 218 | root/Control/axis1/SDOWrite/positionControllerVelocityLoopKd 219 | 220 | 221 | UINT32 222 | root/Control/axis1/SDORead/positionControllerVelocityLoopIntegralLimit 223 | root/Control/axis1/SDOWrite/positionControllerVelocityLoopIntegralLimit 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | UINT32 232 | root/Control/axis2/slaveTimestamp 233 | 234 | 235 | INT32 236 | root/Control/axis2/positionValue 237 | 238 | 239 | INT32 240 | root/Control/axis2/velocityValue 241 | 242 | 243 | INT16 244 | root/Control/axis2/torqueValue 245 | 246 | 247 | INT32 248 | root/Control/axis2/secondaryPositionValue 249 | 250 | 251 | INT32 252 | root/Control/axis2/secondaryVelocityValue 253 | 254 | 255 | UINT16 256 | root/Control/axis2/analogInput1 257 | 258 | 259 | UINT16 260 | root/Control/axis2/analogInput2 261 | 262 | 263 | UINT16 264 | root/Control/axis2/analogInput3 265 | 266 | 267 | UINT16 268 | root/Control/axis2/analogInput4 269 | 270 | 271 | BOOL 272 | root/Control/axis2/digitalInput1 273 | 274 | 275 | BOOL 276 | root/Control/axis2/digitalInput2 277 | 278 | 279 | BOOL 280 | root/Control/axis2/digitalInput3 281 | 282 | 283 | BOOL 284 | root/Control/axis2/digitalInput4 285 | 286 | 287 | 288 | INT32 289 | root/Control/axis2/targetPosition 290 | 291 | 292 | INT32 293 | root/Control/axis2/targetVelocity 294 | 295 | 296 | INT16 297 | root/Control/axis2/targetTorque 298 | 299 | 300 | INT16 301 | root/Control/axis2/torqueOffset 302 | 303 | 304 | BOOL 305 | root/Control/axis2/digitalOutput1 306 | 307 | 308 | BOOL 309 | root/Control/axis2/digitalOutput2 310 | 311 | 312 | BOOL 313 | root/Control/axis2/digitalOutput3 314 | 315 | 316 | BOOL 317 | root/Control/axis2/digitalOutput4 318 | 319 | 320 | 321 | root/cia402/driveStatusWord 322 | 323 | 324 | root/cia402/driveControlWord 325 | 326 | 327 | root/cia402/driveOpMode 328 | 329 | 330 | 331 | 332 | root/cia402/uploadErrorCode 333 | root/Control/axis2/driveErrorCode 334 | 335 | 336 | 337 | UINT32 338 | root/Control/axis2/SDOWrite/saveAllParameters 339 | 340 | 341 | UINT32 342 | root/Control/axis2/SDOWrite/restoreAllParameters 343 | 344 | 345 | 346 | root/Control/axis2/SDORead/torqueControllerKp 347 | 348 | FLOAT 349 | 350 | 351 | FLOAT 352 | root/Control/axis2/SDORead/torqueControllerKi 353 | 354 | 355 | 356 | FLOAT 357 | root/Control/axis2/SDORead/torqueControllerKd 358 | 359 | 360 | 361 | UINT32 362 | root/Control/axis2/SDORead/fieldWeakeningEnable 363 | 364 | 365 | 366 | UINT32 367 | root/Control/axis2/SDORead/fieldWeakeningPercentage 368 | 369 | 370 | 371 | UINT32 372 | root/Control/axis2/SDORead/fieldWeakeningStartingSpeed 373 | 374 | 375 | 376 | UINT32 377 | root/Control/axis2/SDORead/fieldWeakeningEndingSpeed 378 | 379 | 380 | 381 | UINT32 382 | root/Control/axis2/SDORead/commutationAngleMeasurementDelay 383 | 384 | 385 | 386 | 387 | 388 | FLOAT 389 | root/Control/axis2/SDORead/velocityControllerKp 390 | root/Control/axis2/SDOWrite/velocityControllerKp 391 | 392 | 393 | FLOAT 394 | root/Control/axis2/SDORead/velocityControllerKi 395 | root/Control/axis2/SDOWrite/velocityControllerKi 396 | 397 | 398 | FLOAT 399 | root/Control/axis2/SDORead/velocityControllerKd 400 | root/Control/axis2/SDOWrite/velocityControllerKd 401 | 402 | 403 | UINT32 404 | root/Control/axis2/SDORead/velocityControllerIntegralLimit 405 | root/Control/axis2/SDOWrite/velocityControllerIntegralLimit 406 | 407 | 408 | 409 | 410 | FLOAT 411 | root/Control/axis2/SDORead/positionControllerPositionLoopKp 412 | root/Control/axis2/SDOWrite/positionControllerPositionLoopKp 413 | 414 | 415 | FLOAT 416 | root/Control/axis2/SDORead/positionControllerPositionLoopKi 417 | root/Control/axis2/SDOWrite/positionControllerPositionLoopKi 418 | 419 | 420 | FLOAT 421 | root/Control/axis2/SDORead/positionControllerPositionLoopKd 422 | root/Control/axis2/SDOWrite/positionControllerPositionLoopKd 423 | 424 | 425 | UINT32 426 | root/Control/axis2/SDORead/positionControllerPositionLoopIntegralLimit 427 | root/Control/axis2/SDOWrite/positionControllerPositionLoopIntegralLimit 428 | 429 | 430 | FLOAT 431 | root/Control/axis2/SDORead/positionControllerVelocityLoopKp 432 | root/Control/axis2/SDOWrite/positionControllerVelocityLoopKp 433 | 434 | 435 | FLOAT 436 | root/Control/axis2/SDORead/positionControllerVelocityLoopKi 437 | root/Control/axis2/SDOWrite/positionControllerVelocityLoopKi 438 | 439 | 440 | FLOAT 441 | root/Control/axis2/SDORead/positionControllerVelocityLoopKd 442 | root/Control/axis2/SDOWrite/positionControllerVelocityLoopKd 443 | 444 | 445 | UINT32 446 | root/Control/axis2/SDORead/positionControllerVelocityLoopIntegralLimit 447 | root/Control/axis2/SDOWrite/positionControllerVelocityLoopIntegralLimit 448 | 449 | 450 | 451 | 452 | BOOL 453 | root/Control/device1/digitalInput1 454 | 455 | 456 | BOOL 457 | root/Control/device1/digitalInput2 458 | 459 | 460 | BOOL 461 | root/Control/device1/digitalInput3 462 | 463 | 464 | BOOL 465 | root/Control/device1/digitalInput4 466 | 467 | 468 | BOOL 469 | root/Control/device1/digitalInput5 470 | 471 | 472 | BOOL 473 | root/Control/device1/digitalInput6 474 | 475 | 476 | BOOL 477 | root/Control/device1/digitalInput7 478 | 479 | 480 | BOOL 481 | root/Control/device1/digitalInput8 482 | 483 | 484 | BOOL 485 | root/Control/device1/digitalInput9 486 | 487 | 488 | BOOL 489 | root/Control/device1/digitalInput10 490 | 491 | 492 | BOOL 493 | root/Control/device1/digitalInput11 494 | 495 | 496 | BOOL 497 | root/Control/device1/digitalInput12 498 | 499 | 500 | 501 | BOOL 502 | root/Control/device1/digitalOutput1 503 | 504 | 505 | BOOL 506 | root/Control/device1/digitalOutput2 507 | 508 | 509 | BOOL 510 | root/Control/device1/digitalOutput3 511 | 512 | 513 | BOOL 514 | root/Control/device1/digitalOutput4 515 | 516 | 517 | BOOL 518 | root/Control/device1/digitalOutput5 519 | 520 | 521 | BOOL 522 | root/Control/device1/digitalOutput6 523 | 524 | 525 | BOOL 526 | root/Control/device1/digitalOutput7 527 | 528 | 529 | BOOL 530 | root/Control/device1/digitalOutput8 531 | 532 | 533 | BOOL 534 | root/Control/device1/digitalOutput9 535 | 536 | 537 | BOOL 538 | root/Control/device1/digitalOutput10 539 | 540 | 541 | BOOL 542 | root/Control/device1/digitalOutput11 543 | 544 | 545 | BOOL 546 | root/Control/device1/digitalOutput12 547 | 548 | 549 | 550 | -------------------------------------------------------------------------------- /ethercat_master/src/config/io/topology.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8914 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | #x1600 19 | Rx PDO Mapping 20 | 21 | #x6040 22 | 0 23 | 16 24 | Controlword 25 | UINT16 26 | 27 | 28 | #x6060 29 | 0 30 | 8 31 | Op Mode 32 | UINT8 33 | 34 | 35 | #x6071 36 | 0 37 | 16 38 | Target Torque 39 | UINT16 40 | 41 | 42 | #x607a 43 | 0 44 | 32 45 | Target Position 46 | UINT32 47 | 48 | 49 | #x60ff 50 | 0 51 | 32 52 | Target Velocity 53 | UINT32 54 | 55 | 56 | #x60b2 57 | 0 58 | 16 59 | Torque offset 60 | UINT16 61 | 62 | 63 | #x2701 64 | 0 65 | 32 66 | Tuning command 67 | UINT32 68 | 69 | 70 | #x2601 71 | 0 72 | 1 73 | Digital Output 1 74 | BOOL 75 | 76 | 77 | #x2007 78 | 0 79 | 7 80 | 81 | BIT7 82 | 83 | 84 | #x2602 85 | 0 86 | 1 87 | Digital Output 2 88 | BOOL 89 | 90 | 91 | #x2007 92 | 0 93 | 7 94 | 95 | BIT7 96 | 97 | 98 | #x2603 99 | 0 100 | 1 101 | Digital Output 3 102 | BOOL 103 | 104 | 105 | #x2007 106 | 0 107 | 7 108 | 109 | BIT7 110 | 111 | 112 | #x2604 113 | 0 114 | 1 115 | Digital Output 4 116 | BOOL 117 | 118 | 119 | #x2007 120 | 0 121 | 7 122 | 123 | BIT7 124 | 125 | 126 | #x2703 127 | 0 128 | 32 129 | User MOSI 130 | UINT32 131 | 132 | 133 | 134 | #x1a00 135 | Tx PDO Mapping 136 | 137 | #x6041 138 | 0 139 | 16 140 | Statusword 141 | UINT16 142 | 143 | 144 | #x6061 145 | 0 146 | 8 147 | Op Mode Display 148 | UINT8 149 | 150 | 151 | #x6064 152 | 0 153 | 32 154 | Position Value 155 | UINT32 156 | 157 | 158 | #x606c 159 | 0 160 | 32 161 | Velocity Value 162 | UINT32 163 | 164 | 165 | #x6077 166 | 0 167 | 16 168 | Torque Value 169 | UINT16 170 | 171 | 172 | #x230a 173 | 0 174 | 32 175 | Secondary position value 176 | UINT32 177 | 178 | 179 | #x230b 180 | 0 181 | 32 182 | Secondary velocity value 183 | UINT32 184 | 185 | 186 | #x2401 187 | 0 188 | 16 189 | Analog input 1 190 | UINT16 191 | 192 | 193 | #x2402 194 | 0 195 | 16 196 | Analog input 2 197 | UINT16 198 | 199 | 200 | #x2403 201 | 0 202 | 16 203 | Analog input 3 204 | UINT16 205 | 206 | 207 | #x2404 208 | 0 209 | 16 210 | Analog input 4 211 | UINT16 212 | 213 | 214 | #x2702 215 | 0 216 | 32 217 | Tuning status 218 | UINT32 219 | 220 | 221 | #x2501 222 | 0 223 | 1 224 | Digital Input 1 225 | BOOL 226 | 227 | 228 | #x2007 229 | 0 230 | 7 231 | 232 | BIT7 233 | 234 | 235 | #x2502 236 | 0 237 | 1 238 | Digital Input 2 239 | BOOL 240 | 241 | 242 | #x2007 243 | 0 244 | 7 245 | 246 | BIT7 247 | 248 | 249 | #x2503 250 | 0 251 | 1 252 | Digital Input 3 253 | BOOL 254 | 255 | 256 | #x2007 257 | 0 258 | 7 259 | 260 | BIT7 261 | 262 | 263 | #x2504 264 | 0 265 | 1 266 | Digital Input 4 267 | BOOL 268 | 269 | 270 | #x2007 271 | 0 272 | 7 273 | 274 | BIT7 275 | 276 | 277 | #x2704 278 | 0 279 | 32 280 | User MISO 281 | UINT32 282 | 283 | 284 | #x20f0 285 | 0 286 | 32 287 | Timestamp 288 | UINT32 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 8914 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | #x1600 311 | Rx PDO Mapping 312 | 313 | #x6040 314 | 0 315 | 16 316 | Controlword 317 | UINT16 318 | 319 | 320 | #x6060 321 | 0 322 | 8 323 | Op Mode 324 | UINT8 325 | 326 | 327 | #x6071 328 | 0 329 | 16 330 | Target Torque 331 | UINT16 332 | 333 | 334 | #x607a 335 | 0 336 | 32 337 | Target Position 338 | UINT32 339 | 340 | 341 | #x60ff 342 | 0 343 | 32 344 | Target Velocity 345 | UINT32 346 | 347 | 348 | #x60b2 349 | 0 350 | 16 351 | Torque offset 352 | UINT16 353 | 354 | 355 | #x2701 356 | 0 357 | 32 358 | Tuning command 359 | UINT32 360 | 361 | 362 | #x2601 363 | 0 364 | 1 365 | Digital Output 1 366 | BOOL 367 | 368 | 369 | #x2007 370 | 0 371 | 7 372 | 373 | BIT7 374 | 375 | 376 | #x2602 377 | 0 378 | 1 379 | Digital Output 2 380 | BOOL 381 | 382 | 383 | #x2007 384 | 0 385 | 7 386 | 387 | BIT7 388 | 389 | 390 | #x2603 391 | 0 392 | 1 393 | Digital Output 3 394 | BOOL 395 | 396 | 397 | #x2007 398 | 0 399 | 7 400 | 401 | BIT7 402 | 403 | 404 | #x2604 405 | 0 406 | 1 407 | Digital Output 4 408 | BOOL 409 | 410 | 411 | #x2007 412 | 0 413 | 7 414 | 415 | BIT7 416 | 417 | 418 | #x2703 419 | 0 420 | 32 421 | User MOSI 422 | UINT32 423 | 424 | 425 | 426 | #x1a00 427 | Tx PDO Mapping 428 | 429 | #x6041 430 | 0 431 | 16 432 | Statusword 433 | UINT16 434 | 435 | 436 | #x6061 437 | 0 438 | 8 439 | Op Mode Display 440 | UINT8 441 | 442 | 443 | #x6064 444 | 0 445 | 32 446 | Position Value 447 | UINT32 448 | 449 | 450 | #x606c 451 | 0 452 | 32 453 | Velocity Value 454 | UINT32 455 | 456 | 457 | #x6077 458 | 0 459 | 16 460 | Torque Value 461 | UINT16 462 | 463 | 464 | #x230a 465 | 0 466 | 32 467 | Secondary position value 468 | UINT32 469 | 470 | 471 | #x230b 472 | 0 473 | 32 474 | Secondary velocity value 475 | UINT32 476 | 477 | 478 | #x2401 479 | 0 480 | 16 481 | Analog input 1 482 | UINT16 483 | 484 | 485 | #x2402 486 | 0 487 | 16 488 | Analog input 2 489 | UINT16 490 | 491 | 492 | #x2403 493 | 0 494 | 16 495 | Analog input 3 496 | UINT16 497 | 498 | 499 | #x2404 500 | 0 501 | 16 502 | Analog input 4 503 | UINT16 504 | 505 | 506 | #x2702 507 | 0 508 | 32 509 | Tuning status 510 | UINT32 511 | 512 | 513 | #x2501 514 | 0 515 | 1 516 | Digital Input 1 517 | BOOL 518 | 519 | 520 | #x2007 521 | 0 522 | 7 523 | 524 | BIT7 525 | 526 | 527 | #x2502 528 | 0 529 | 1 530 | Digital Input 2 531 | BOOL 532 | 533 | 534 | #x2007 535 | 0 536 | 7 537 | 538 | BIT7 539 | 540 | 541 | #x2503 542 | 0 543 | 1 544 | Digital Input 3 545 | BOOL 546 | 547 | 548 | #x2007 549 | 0 550 | 7 551 | 552 | BIT7 553 | 554 | 555 | #x2504 556 | 0 557 | 1 558 | Digital Input 4 559 | BOOL 560 | 561 | 562 | #x2007 563 | 0 564 | 7 565 | 566 | BIT7 567 | 568 | 569 | #x2704 570 | 0 571 | 32 572 | User MISO 573 | UINT32 574 | 575 | 576 | #x20f0 577 | 0 578 | 32 579 | Timestamp 580 | UINT32 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 8914 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | #x1a0c 601 | Output 12 602 | 603 | #x2000 604 | 1 605 | 4 606 | Padding 607 | BIT4 608 | 609 | 610 | #x3101 611 | 1 612 | 1 613 | Output 614 | BOOL 615 | 616 | 617 | #x3102 618 | 1 619 | 1 620 | Output 621 | BOOL 622 | 623 | 624 | #x3103 625 | 1 626 | 1 627 | Output 628 | BOOL 629 | 630 | 631 | #x3104 632 | 1 633 | 1 634 | Output 635 | BOOL 636 | 637 | 638 | #x3105 639 | 1 640 | 1 641 | Output 642 | BOOL 643 | 644 | 645 | #x3106 646 | 1 647 | 1 648 | Output 649 | BOOL 650 | 651 | 652 | #x3107 653 | 1 654 | 1 655 | Output 656 | BOOL 657 | 658 | 659 | #x3108 660 | 1 661 | 1 662 | Output 663 | BOOL 664 | 665 | 666 | #x3109 667 | 1 668 | 1 669 | Output 670 | BOOL 671 | 672 | 673 | #x310a 674 | 1 675 | 1 676 | Output 677 | BOOL 678 | 679 | 680 | #x310b 681 | 1 682 | 1 683 | Output 684 | BOOL 685 | 686 | 687 | #x310c 688 | 1 689 | 1 690 | Output 691 | BOOL 692 | 693 | 694 | 695 | #x160b 696 | Input 12 697 | 698 | #x3001 699 | 1 700 | 1 701 | Input 702 | BOOL 703 | 704 | 705 | #x3002 706 | 1 707 | 1 708 | Input 709 | BOOL 710 | 711 | 712 | #x3003 713 | 1 714 | 1 715 | Input 716 | BOOL 717 | 718 | 719 | #x3004 720 | 1 721 | 1 722 | Input 723 | BOOL 724 | 725 | 726 | #x3005 727 | 1 728 | 1 729 | Input 730 | BOOL 731 | 732 | 733 | #x3006 734 | 1 735 | 1 736 | Input 737 | BOOL 738 | 739 | 740 | #x3007 741 | 1 742 | 1 743 | Input 744 | BOOL 745 | 746 | 747 | #x3008 748 | 1 749 | 1 750 | Input 751 | BOOL 752 | 753 | 754 | #x3009 755 | 1 756 | 1 757 | Input 758 | BOOL 759 | 760 | 761 | #x300a 762 | 1 763 | 1 764 | Input 765 | BOOL 766 | 767 | 768 | #x300b 769 | 1 770 | 1 771 | Input 772 | BOOL 773 | 774 | 775 | #x300c 776 | 1 777 | 1 778 | Input 779 | BOOL 780 | 781 | 782 | 783 | 784 | 785 | 786 | -------------------------------------------------------------------------------- /ethercat_master/src/control/DigitalIO.cpp: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #include "DigitalIO.h" 41 | #include 42 | 43 | using namespace mcx; 44 | 45 | void DigitalIO::create_(const char *name, parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) { 46 | 47 | } 48 | 49 | bool DigitalIO::initPhase1_() { 50 | 51 | digitalInputs_.digital_inputs.resize(12, 0); 52 | digitalOutputs_.digital_outputs.resize(12, 0); 53 | 54 | std::string inputParam = "digitalInput"; 55 | std::string outputParam = "digitalOutput"; 56 | 57 | for (int i = 0; i < 12; i++) { 58 | std::string inputParamName = inputParam + std::to_string(i+1); 59 | std::string outputParamName = outputParam + std::to_string(i+1); 60 | addParameter(inputParamName.c_str(), mcx::parameter_server::ParameterType::INPUT, &digitalInputs_.digital_inputs[i]); 61 | addParameter(outputParamName.c_str(), mcx::parameter_server::ParameterType::OUTPUT, &digitalOutputs_.digital_outputs[i]); 62 | } 63 | 64 | return true; 65 | } 66 | 67 | bool DigitalIO::initPhase2_() { 68 | return true; 69 | } 70 | 71 | bool DigitalIO::startOp_() { 72 | return true; 73 | } 74 | 75 | bool DigitalIO::stopOp_() { 76 | return true; 77 | } 78 | 79 | bool DigitalIO::iterateOp_(const container::TaskTime &system_time, container::UserTime *user_time) { 80 | return true; 81 | } -------------------------------------------------------------------------------- /ethercat_master/src/control/DigitalIO.h: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #ifndef DIGITALIO_H 41 | #define DIGITALIO_H 42 | 43 | #include 44 | #include "motorcortex_msgs/DigitalInputs.h" 45 | #include "motorcortex_msgs/DigitalOutputs.h" 46 | 47 | class DigitalIO : public mcx::container::Module { 48 | public: 49 | 50 | DigitalIO() = default; 51 | 52 | ~DigitalIO() override = default; 53 | 54 | const motorcortex_msgs::DigitalInputs &getDIOFeedback() const { 55 | return digitalInputs_; 56 | } 57 | 58 | void setDigitalOutputs(const motorcortex_msgs::DigitalOutputs& digitalOutputsCommand) { 59 | digitalOutputs_ = digitalOutputsCommand; 60 | } 61 | 62 | private: 63 | void create_(const char *name, mcx::parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) override; 64 | 65 | bool initPhase1_() override; 66 | 67 | bool initPhase2_() override; 68 | 69 | bool startOp_() override; 70 | 71 | bool stopOp_() override; 72 | 73 | bool iterateOp_(const mcx::container::TaskTime &system_time, mcx::container::UserTime *user_time) override; 74 | 75 | motorcortex_msgs::DigitalInputs digitalInputs_; 76 | motorcortex_msgs::DigitalOutputs digitalOutputs_; 77 | }; 78 | 79 | #endif /* DIGITALIO_H */ 80 | -------------------------------------------------------------------------------- /ethercat_master/src/control/Drive.cpp: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #include "Drive.h" 41 | 42 | using namespace mcx; 43 | 44 | 45 | void Drive::create_(const char *name, parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) { 46 | 47 | createSubmodule(&drive_sdo_read_, "SDORead"); 48 | createSubmodule(&drive_sdo_write_, "SDOWrite"); 49 | 50 | } 51 | 52 | bool Drive::initPhase1_() { 53 | 54 | using namespace mcx::parameter_server; 55 | 56 | driveFeedback_.analog_inputs.resize(4, 0); 57 | driveFeedback_.digital_inputs.resize(4, 0); 58 | driveCommand_.digital_outputs.resize(4, 0); 59 | 60 | addParameter("driveErrorCode", ParameterType::INPUT, &driveFeedback_.drive_error_code); 61 | addParameter("slaveTimestamp", ParameterType::INPUT, &driveFeedback_.slave_timestamp); 62 | addParameter("positionValue", ParameterType::INPUT, &driveFeedback_.position_value); 63 | addParameter("velocityValue", ParameterType::INPUT, &driveFeedback_.velocity_value); 64 | addParameter("torqueValue", ParameterType::INPUT, &driveFeedback_.torque_value); 65 | addParameter("secondaryPositionValue", ParameterType::INPUT, &driveFeedback_.secondary_position_value); 66 | addParameter("secondaryVelocityValue", ParameterType::INPUT, &driveFeedback_.secondary_velocity_value); 67 | addParameter("analogInput1", ParameterType::INPUT, &driveFeedback_.analog_inputs[0]); 68 | addParameter("analogInput2", ParameterType::INPUT, &driveFeedback_.analog_inputs[1]); 69 | addParameter("analogInput3", ParameterType::INPUT, &driveFeedback_.analog_inputs[2]); 70 | addParameter("analogInput4", ParameterType::INPUT, &driveFeedback_.analog_inputs[3]); 71 | addParameter("digitalInput1", ParameterType::INPUT, &driveFeedback_.digital_inputs[0]); 72 | addParameter("digitalInput2", ParameterType::INPUT, &driveFeedback_.digital_inputs[1]); 73 | addParameter("digitalInput3", ParameterType::INPUT, &driveFeedback_.digital_inputs[2]); 74 | addParameter("digitalInput4", ParameterType::INPUT, &driveFeedback_.digital_inputs[3]); 75 | 76 | addParameter("targetPosition", ParameterType::OUTPUT, &driveCommand_.target_position); 77 | addParameter("targetVelocity", ParameterType::OUTPUT, &driveCommand_.target_velocity); 78 | addParameter("targetTorque", ParameterType::OUTPUT, &driveCommand_.target_torque); 79 | addParameter("torqueOffset", ParameterType::OUTPUT, &driveCommand_.torque_offset); 80 | addParameter("digitalOutput1", ParameterType::OUTPUT, &driveCommand_.digital_outputs[0]); 81 | addParameter("digitalOutput2", ParameterType::OUTPUT, &driveCommand_.digital_outputs[1]); 82 | addParameter("digitalOutput3", ParameterType::OUTPUT, &driveCommand_.digital_outputs[2]); 83 | addParameter("digitalOutput4", ParameterType::OUTPUT, &driveCommand_.digital_outputs[3]); 84 | 85 | return true; 86 | } 87 | 88 | bool Drive::initPhase2_() { 89 | return true; 90 | } 91 | 92 | bool Drive::startOp_() { 93 | return true; 94 | } 95 | 96 | bool Drive::stopOp_() { 97 | return true; 98 | } 99 | 100 | bool Drive::iterateOp_(const container::TaskTime &system_time, container::UserTime *user_time) { 101 | 102 | drive_sdo_write_.iterate(system_time, user_time); 103 | drive_sdo_read_.iterate(system_time, user_time); 104 | 105 | return true; 106 | } -------------------------------------------------------------------------------- /ethercat_master/src/control/Drive.h: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #ifndef DRIVE_H 41 | #define DRIVE_H 42 | 43 | #include 44 | #include "DriveSdo.h" 45 | #include "motorcortex_msgs/DriveIn.h" 46 | #include "motorcortex_msgs/DriveOut.h" 47 | 48 | 49 | class Drive : public mcx::container::Module { 50 | public: 51 | 52 | Drive() = default; 53 | ~Drive() override = default; 54 | 55 | 56 | const motorcortex_msgs::DriveIn &getDriveFeedback() const { 57 | return driveFeedback_; 58 | } 59 | 60 | int getOpMode() const { 61 | return driveCommand_.opmode; 62 | } 63 | 64 | int getControlWord() const { 65 | return driveCommand_.controlword; 66 | } 67 | 68 | void setStatusWord(int statusword) { 69 | driveFeedback_.statusword = statusword; 70 | } 71 | 72 | void setDriveCommand(const motorcortex_msgs::DriveOut& driveCommand) { 73 | driveCommand_ = driveCommand; 74 | } 75 | 76 | void setSDOCfg(const DriveSdo::SDOCfg& sdoCfg) { 77 | drive_sdo_write_.setSDOCfg(sdoCfg); 78 | } 79 | 80 | const DriveSdo::SDOCfg &getSDOCfg() const { 81 | return drive_sdo_read_.getSDOCfg(); 82 | } 83 | 84 | void requestSDOUpdate(bool req) { 85 | request_update_ = req; 86 | } 87 | 88 | void saveAllCfg(const uint32_t& save_cfg) { 89 | drive_sdo_write_.saveAllCfg(save_cfg); 90 | } 91 | 92 | void restoreAllCfg(const uint32_t& restore_cfg) { 93 | drive_sdo_write_.restoreAllCfg(restore_cfg); 94 | } 95 | 96 | private: 97 | void create_(const char *name, mcx::parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) override; 98 | 99 | bool initPhase1_() override; 100 | 101 | bool initPhase2_() override; 102 | 103 | bool startOp_() override; 104 | 105 | bool stopOp_() override; 106 | 107 | bool iterateOp_(const mcx::container::TaskTime &system_time, mcx::container::UserTime *user_time) override; 108 | 109 | motorcortex_msgs::DriveIn driveFeedback_; 110 | 111 | motorcortex_msgs::DriveOut driveCommand_; 112 | 113 | bool request_update_{}; 114 | 115 | DriveSdo drive_sdo_read_; 116 | DriveSdo drive_sdo_write_; 117 | 118 | }; 119 | 120 | #endif /* DRIVE_H */ 121 | -------------------------------------------------------------------------------- /ethercat_master/src/control/DriveSdo.cpp: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #include "DriveSdo.h" 41 | #include 42 | 43 | using namespace mcx; 44 | 45 | void DriveSdo::create_(const char *name, parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) { 46 | 47 | } 48 | 49 | bool DriveSdo::initPhase1_() { 50 | 51 | using namespace mcx::parameter_server; 52 | 53 | //SDOs 54 | auto param = addParameter("torqueControllerKp", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.controller_Kp); 55 | handles_.push_back(param); 56 | param = addParameter("torqueControllerKi", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.controller_Ki); 57 | handles_.push_back(param); 58 | param = addParameter("torqueControllerKd", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.controller_Kd); 59 | handles_.push_back(param); 60 | param = addParameter("fieldWeakeningEnable", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.field_weakening_enable); 61 | handles_.push_back(param); 62 | param = addParameter("fieldWeakeningPercentage", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.field_weakening_percentage); 63 | handles_.push_back(param); 64 | param = addParameter("fieldWeakeningStartingSpeed", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.field_weakening_starting_speed); 65 | handles_.push_back(param); 66 | param = addParameter("fieldWeakeningEndingSpeed", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.field_weakening_ending_speed); 67 | handles_.push_back(param); 68 | param = addParameter("commutationAngleMeasurementDelay", ParameterType::PARAMETER, &sdoCfg_.torqueControllerCfg.commutation_angle_measurement_delay); 69 | handles_.push_back(param); 70 | 71 | param = addParameter("velocityControllerKp", ParameterType::PARAMETER, &sdoCfg_.velocityControllerCfg.controller_Kp); 72 | handles_.push_back(param); 73 | param = addParameter("velocityControllerKi", ParameterType::PARAMETER, &sdoCfg_.velocityControllerCfg.controller_Ki); 74 | handles_.push_back(param); 75 | param = addParameter("velocityControllerKd", ParameterType::PARAMETER, &sdoCfg_.velocityControllerCfg.controller_Kd); 76 | handles_.push_back(param); 77 | param = addParameter("velocityControllerIntegralLimit", ParameterType::PARAMETER, &sdoCfg_.velocityControllerCfg.controller_integral_limit); 78 | handles_.push_back(param); 79 | 80 | param = addParameter("positionControllerPositionLoopKp", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.position_loop_Kp); 81 | handles_.push_back(param); 82 | param = addParameter("positionControllerPositionLoopKi", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.position_loop_Ki); 83 | handles_.push_back(param); 84 | param = addParameter("positionControllerPositionLoopKd", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.position_loop_Kd); 85 | handles_.push_back(param); 86 | param = addParameter("positionControllerPositionLoopIntegralLimit", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.position_loop_integral_limit); 87 | handles_.push_back(param); 88 | param = addParameter("positionControllerVelocityLoopKp", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.velocity_loop_Kp); 89 | handles_.push_back(param); 90 | param = addParameter("positionControllerVelocityLoopKi", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.velocity_loop_Ki); 91 | handles_.push_back(param); 92 | param = addParameter("positionControllerVelocityLoopKd", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.velocity_loop_Kd); 93 | handles_.push_back(param); 94 | param = addParameter("positionControllerVelocityLoopIntegralLimit", ParameterType::PARAMETER, &sdoCfg_.positionControllerCfg.velocity_loop_integral_limit); 95 | handles_.push_back(param); 96 | 97 | for (auto& handle : handles_) { 98 | handle.updateOutput(false); 99 | } 100 | 101 | param = addParameter("saveAllParameters", ParameterType::PARAMETER, &save_cfg_); 102 | save_params_handler_ = param; 103 | save_params_handler_.updateOutput(false); 104 | 105 | param = addParameter("restoreAllParameters", ParameterType::PARAMETER, &restore_cfg_); 106 | restore_params_handler_ = param; 107 | restore_params_handler_.updateOutput(false); 108 | 109 | return true; 110 | } 111 | 112 | bool DriveSdo::initPhase2_() { 113 | return true; 114 | } 115 | 116 | bool DriveSdo::startOp_() { 117 | return true; 118 | } 119 | 120 | bool DriveSdo::stopOp_() { 121 | return true; 122 | } 123 | 124 | bool DriveSdo::iterateOp_(const container::TaskTime &system_time, container::UserTime *user_time) { 125 | return true; 126 | } -------------------------------------------------------------------------------- /ethercat_master/src/control/DriveSdo.h: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #ifndef DRIVESDO_H 41 | #define DRIVESDO_H 42 | 43 | #include 44 | #include "motorcortex_msgs/TorqueControllerCfg.h" 45 | #include "motorcortex_msgs/VelocityControllerCfg.h" 46 | #include "motorcortex_msgs/PositionControllerCfg.h" 47 | 48 | class DriveSdo : public mcx::container::Module { 49 | public: 50 | 51 | DriveSdo() = default; 52 | 53 | ~DriveSdo() override = default; 54 | 55 | struct SDOCfg { 56 | motorcortex_msgs::TorqueControllerCfg torqueControllerCfg; 57 | motorcortex_msgs::VelocityControllerCfg velocityControllerCfg; 58 | motorcortex_msgs::PositionControllerCfg positionControllerCfg; 59 | }; 60 | 61 | void setSDOCfg(const SDOCfg& sdoCfg) { 62 | sdoCfg_ = sdoCfg; 63 | for (auto& handle : handles_) { 64 | handle.updateOutputOnce(); 65 | } 66 | //LOG_INFO("kp: %f", sdoCfg_.velocityControllerCfg.controller_Kp); 67 | //LOG_INFO("All params updated"); 68 | } 69 | 70 | const SDOCfg &getSDOCfg() const { 71 | return sdoCfg_; 72 | } 73 | 74 | void saveAllCfg(const uint32_t& save_cfg) { 75 | save_cfg_ = save_cfg; 76 | save_params_handler_.updateOutputOnce(); 77 | } 78 | 79 | void restoreAllCfg(const uint32_t& restore_cfg) { 80 | restore_cfg_ = restore_cfg; 81 | restore_params_handler_.updateOutputOnce(); 82 | } 83 | 84 | private: 85 | void create_(const char *name, mcx::parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) override; 86 | 87 | bool initPhase1_() override; 88 | 89 | bool initPhase2_() override; 90 | 91 | bool startOp_() override; 92 | 93 | bool stopOp_() override; 94 | 95 | bool iterateOp_(const mcx::container::TaskTime &system_time, mcx::container::UserTime *user_time) override; 96 | 97 | SDOCfg sdoCfg_; 98 | std::vector handles_; 99 | mcx::parameter_server::ParamHandle save_params_handler_; 100 | mcx::parameter_server::ParamHandle restore_params_handler_; 101 | uint32_t save_cfg_{}; 102 | uint32_t restore_cfg_{}; 103 | 104 | }; 105 | 106 | #endif /* DRIVESDO_H */ 107 | -------------------------------------------------------------------------------- /ethercat_master/src/control/MainControlLoop.cpp: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #include "MainControlLoop.h" 41 | #include 42 | 43 | using namespace mcx; 44 | 45 | MainControlLoop::MainControlLoop(unsigned int number_of_drives, unsigned int number_of_ios) : number_of_drives_{ 46 | number_of_drives}, number_of_ios_{number_of_ios} { 47 | 48 | opmode_ = new int[number_of_drives]{}; 49 | controlword_ = new int[number_of_drives]{}; 50 | statusword_ = new int[number_of_drives]{}; 51 | drives_ = new Drive[number_of_drives]; 52 | if (number_of_ios_ > 0) { 53 | dio_devices_ = new DigitalIO[number_of_ios]; 54 | } 55 | 56 | } 57 | 58 | MainControlLoop::~MainControlLoop() { 59 | 60 | delete[] opmode_; 61 | delete[] controlword_; 62 | delete[] statusword_; 63 | delete[] drives_; 64 | delete[] dio_devices_; 65 | } 66 | 67 | void MainControlLoop::create_(const char *name, parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) { 68 | 69 | sub_drives_ = nh_.subscribe("/drive_control", 1, 70 | &MainControlLoop::drivesControlCallback, this); 71 | if (number_of_ios_ > 0) { 72 | sub_dios_ = nh_.subscribe("/digital_outputs", 1, 73 | &MainControlLoop::diosControlCallback, this); 74 | } 75 | 76 | drive_feedback_pub_ = nh_.advertise("/drive_feedback", 1); 77 | if (number_of_ios_ > 0) { 78 | digital_inputs_pub_ = nh_.advertise("/digital_inputs", 1); 79 | } 80 | 81 | service_get_sdo_ = nh_.advertiseService("get_sdo_config", &MainControlLoop::getSDOSrv, this); 82 | service_set_sdo_ = nh_.advertiseService("set_sdo_config", &MainControlLoop::setSDOSrv, this); 83 | service_save_cfg_ = nh_.advertiseService("save_sdo_config", &MainControlLoop::saveCfgParamsSrv, this); 84 | service_restore_cfg_ = nh_.advertiseService("restore_default_sdo_config", &MainControlLoop::restoreCfgParamsSrv, this); 85 | 86 | std::string axisName = "axis"; 87 | for (unsigned int i = 0; i < number_of_drives_; i++) { 88 | std::string indexedName = axisName + std::to_string(i + 1); 89 | createSubmodule(&drives_[i], indexedName.c_str()); 90 | } 91 | 92 | std::string deviceName = "device"; 93 | for (unsigned int i = 0; i < number_of_ios_; i++) { 94 | std::string indexedName = deviceName + std::to_string(i + 1); 95 | createSubmodule(&dio_devices_[i], indexedName.c_str()); 96 | } 97 | 98 | } 99 | 100 | bool MainControlLoop::initPhase1_() { 101 | // user input 102 | addParameter("controlword", mcx::parameter_server::ParameterType::OUTPUT, controlword_, number_of_drives_); 103 | addParameter("opmode", mcx::parameter_server::ParameterType::OUTPUT, opmode_, number_of_drives_); 104 | addParameter("statusword", mcx::parameter_server::ParameterType::INPUT, statusword_, number_of_drives_); 105 | 106 | addParameter("read_sdo", mcx::parameter_server::ParameterType::OUTPUT, &read_sdo_); 107 | addParameter("read_sdo_update_sec", mcx::parameter_server::ParameterType::PARAMETER, &read_sdo_time_max_sec_); 108 | 109 | return true; 110 | } 111 | 112 | bool MainControlLoop::initPhase2_() { 113 | return true; 114 | } 115 | 116 | bool MainControlLoop::startOp_() { 117 | return true; 118 | } 119 | 120 | bool MainControlLoop::stopOp_() { 121 | return true; 122 | } 123 | 124 | bool MainControlLoop::iterateOp_(const container::TaskTime &system_time, container::UserTime *user_time) { 125 | 126 | if (read_sdo_time_sec_ >= read_sdo_time_max_sec_) { 127 | read_sdo_time_sec_ = 0; 128 | read_sdo_ = true; 129 | } else { 130 | read_sdo_ = false; 131 | read_sdo_time_sec_ += getDtSec(); 132 | } 133 | 134 | ros::spinOnce(); 135 | 136 | motorcortex_msgs::DriveInList driveFeedbackList; 137 | 138 | for (unsigned int i = 0; i < number_of_drives_; i++) { 139 | drives_[i].iterate(system_time, user_time); 140 | controlword_[i] = drives_[i].getControlWord(); 141 | opmode_[i] = drives_[i].getOpMode(); 142 | drives_[i].setStatusWord(statusword_[i]); 143 | driveFeedbackList.drives_feedback.push_back(drives_[i].getDriveFeedback()); 144 | } 145 | 146 | drive_feedback_pub_.publish(driveFeedbackList); 147 | 148 | if (number_of_ios_ > 0) { 149 | motorcortex_msgs::DigitalInputsList digitalInputsList; 150 | 151 | for (unsigned int i = 0; i < number_of_ios_; i++) { 152 | dio_devices_[i].iterate(system_time, user_time); 153 | digitalInputsList.devices_feedback.push_back(dio_devices_[i].getDIOFeedback()); 154 | } 155 | 156 | digital_inputs_pub_.publish(digitalInputsList); 157 | } 158 | 159 | return true; 160 | } 161 | 162 | void MainControlLoop::drivesControlCallback(const motorcortex_msgs::DriveOutList::ConstPtr &drives_command_msg) { 163 | unsigned int max_counter = std::min(number_of_drives_, drives_command_msg->drive_command.size()); 164 | for (unsigned int i = 0; i < max_counter; i++) { 165 | drives_[i].setDriveCommand(drives_command_msg->drive_command[i]); 166 | } 167 | } 168 | 169 | void MainControlLoop::diosControlCallback(const motorcortex_msgs::DigitalOutputsList::ConstPtr &dios_command_msg) { 170 | unsigned int max_counter = std::min(number_of_ios_, dios_command_msg->devices_command.size()); 171 | for (unsigned int i = 0; i < max_counter; i++) { 172 | dio_devices_[i].setDigitalOutputs(dios_command_msg->devices_command[i]); 173 | } 174 | } 175 | 176 | bool MainControlLoop::getSDOSrv(motorcortex_msgs::GetSDOCfg::Request &req, 177 | motorcortex_msgs::GetSDOCfg::Response &res) { 178 | unsigned int max_counter = std::min(number_of_drives_, req.read_cfg.size()); 179 | for (unsigned int i = 0; i < max_counter; i++) { 180 | //ToDO: request an update 181 | drives_[i].requestSDOUpdate(req.read_cfg[i]); 182 | 183 | //ToDo: receive the updated data 184 | DriveSdo::SDOCfg SDOCfg = drives_[i].getSDOCfg(); 185 | res.torque_controller_cfg.push_back(SDOCfg.torqueControllerCfg); 186 | res.velocity_controller_cfg.push_back(SDOCfg.velocityControllerCfg); 187 | res.position_controller_cfg.push_back(SDOCfg.positionControllerCfg); 188 | } 189 | 190 | return true; 191 | } 192 | 193 | bool MainControlLoop::setSDOSrv(motorcortex_msgs::SetSDOCfg::Request &req, 194 | motorcortex_msgs::SetSDOCfg::Response &res) { 195 | for (unsigned int i = 0; i < number_of_drives_; i++) { 196 | DriveSdo::SDOCfg sdoCfg; 197 | if (i < req.torque_controller_cfg.size()) { 198 | sdoCfg.torqueControllerCfg = req.torque_controller_cfg[i]; 199 | } 200 | if (i < req.velocity_controller_cfg.size()) { 201 | sdoCfg.velocityControllerCfg = req.velocity_controller_cfg[i]; 202 | } 203 | if (i < req.velocity_controller_cfg.size()) { 204 | sdoCfg.positionControllerCfg = req.position_controller_cfg[i]; 205 | } 206 | //ToDo: set the configuration 207 | drives_[i].setSDOCfg(sdoCfg); 208 | 209 | //ToDo: notify of success or failure 210 | res.success.push_back(true); 211 | } 212 | 213 | return true; 214 | } 215 | 216 | bool MainControlLoop::saveCfgParamsSrv(motorcortex_msgs::SaveCfgParams::Request &req, 217 | motorcortex_msgs::SaveCfgParams::Response &res) { 218 | 219 | unsigned int max_counter = std::min(number_of_drives_, req.save.size()); 220 | LOG_INFO("save called %i", max_counter); 221 | for (unsigned int i = 0; i < max_counter; i++) { 222 | LOG_INFO("save: %s", req.save[i] ? "true" : "false"); 223 | if (req.save[i]) { 224 | drives_[i].saveAllCfg(0x65766173);//"save" 225 | } 226 | 227 | //ToDo: notify of success or failure 228 | res.success.push_back(true); 229 | } 230 | return true; 231 | } 232 | 233 | bool MainControlLoop::restoreCfgParamsSrv(motorcortex_msgs::RestoreCfgParams::Request &req, 234 | motorcortex_msgs::RestoreCfgParams::Response &res) { 235 | unsigned int max_counter = std::min(number_of_drives_, req.reset_to_default.size()); 236 | for (unsigned int i = 0; i < max_counter; i++) { 237 | LOG_INFO("reset to default: %s", req.reset_to_default[i] ? "true" : "false"); 238 | if (req.reset_to_default[i]) { 239 | drives_[i].restoreAllCfg(0x64616f6c);//"load" 240 | } 241 | 242 | //ToDo: notify of success or failure 243 | res.success.push_back(true); 244 | } 245 | 246 | return true; 247 | } -------------------------------------------------------------------------------- /ethercat_master/src/control/MainControlLoop.h: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #ifndef MAINCONTROLLOOP_H 41 | #define MAINCONTROLLOOP_H 42 | 43 | #include 44 | #include "ros/ros.h" 45 | #include "std_msgs/String.h" 46 | #include "Drive.h" 47 | #include "DigitalIO.h" 48 | #include "motorcortex_msgs/DriveInList.h" 49 | #include "motorcortex_msgs/DriveOutList.h" 50 | #include "motorcortex_msgs/DigitalInputsList.h" 51 | #include "motorcortex_msgs/DigitalOutputsList.h" 52 | #include "motorcortex_msgs/GetSDOCfg.h" 53 | #include "motorcortex_msgs/SetSDOCfg.h" 54 | #include "motorcortex_msgs/SaveCfgParams.h" 55 | #include "motorcortex_msgs/RestoreCfgParams.h" 56 | 57 | class MainControlLoop : public mcx::container::Module { 58 | 59 | public: 60 | 61 | MainControlLoop(unsigned int number_of_drives, unsigned int number_of_ios); 62 | 63 | ~MainControlLoop() override; 64 | 65 | void drivesControlCallback(const motorcortex_msgs::DriveOutList::ConstPtr &drives_command_msg); 66 | 67 | void diosControlCallback(const motorcortex_msgs::DigitalOutputsList::ConstPtr &dios_command_msg); 68 | 69 | bool getSDOSrv(motorcortex_msgs::GetSDOCfg::Request &req, motorcortex_msgs::GetSDOCfg::Response &res); 70 | 71 | bool setSDOSrv(motorcortex_msgs::SetSDOCfg::Request &req, motorcortex_msgs::SetSDOCfg::Response &res); 72 | 73 | bool saveCfgParamsSrv(motorcortex_msgs::SaveCfgParams::Request &req, motorcortex_msgs::SaveCfgParams::Response &res); 74 | 75 | bool restoreCfgParamsSrv(motorcortex_msgs::RestoreCfgParams::Request &req, motorcortex_msgs::RestoreCfgParams::Response &res); 76 | 77 | private: 78 | void create_(const char *name, mcx::parameter_server::Parameter *parameter_server, uint64_t dt_micro_s) override; 79 | 80 | bool initPhase1_() override; 81 | 82 | bool initPhase2_() override; 83 | 84 | bool startOp_() override; 85 | 86 | bool stopOp_() override; 87 | 88 | bool iterateOp_(const mcx::container::TaskTime &system_time, mcx::container::UserTime *user_time) override; 89 | 90 | ros::NodeHandle nh_; 91 | ros::Subscriber sub_drives_; 92 | ros::Subscriber sub_dios_; 93 | ros::Publisher drive_feedback_pub_; 94 | ros::Publisher digital_inputs_pub_; 95 | ros::ServiceServer service_get_sdo_; 96 | ros::ServiceServer service_set_sdo_; 97 | ros::ServiceServer service_save_cfg_; 98 | ros::ServiceServer service_restore_cfg_; 99 | 100 | 101 | size_t number_of_drives_; 102 | size_t number_of_ios_; 103 | int* opmode_; 104 | int* controlword_; 105 | int* statusword_; 106 | Drive* drives_; 107 | DigitalIO* dio_devices_; 108 | bool read_sdo_; 109 | double read_sdo_time_max_sec_{0.5}; 110 | double read_sdo_time_sec_{}; 111 | 112 | 113 | }; 114 | 115 | #endif /* MAINCONTROLLOOP_H */ -------------------------------------------------------------------------------- /ethercat_master/src/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2018, Vectioneer B.V. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Vectioneer nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Alexey Zakharov 37 | * Author: Viatcheslav Tretyakov 38 | *********************************************************************/ 39 | 40 | #include "MainControlLoop.h" 41 | 42 | #include 43 | 44 | #include 45 | #include "ros/ros.h" 46 | 47 | using namespace mcx; 48 | 49 | #define NUM_OF_DRIVES 2 50 | #define NUM_OD_DIOS 0 51 | 52 | void link(parameter_server::Parameter *ps) { 53 | 54 | 55 | ps->link("root/Control/controlword", "root/cia402/driveCommand"); 56 | ps->link("root/Control/opmode", "root/cia402/driveMode"); 57 | ps->link("root/cia402/driveState", "root/Control/statusword"); 58 | 59 | ps->link("root/Control/read_sdo", "root/EtherCAT/Domain1/read_sdo"); 60 | 61 | } 62 | 63 | void run(const utils::CommandLineArgs &cmd_args) { 64 | 65 | // sets cycle time of the controls 66 | const int rt_dt_micro_s = 1000; 67 | 68 | // sets configuration paths 69 | std::string path_control_dir = cmd_args.config_path + "/control"; 70 | std::string path_control = path_control_dir + "/control.xml"; 71 | std::string path_ethercat = cmd_args.config_path + "/io"; 72 | std::string path_log = cmd_args.log_path; 73 | 74 | parameter_server::Parameter param_server; 75 | 76 | // creates root of the parameter tree 77 | param_server.create("root", nullptr); 78 | 79 | // creates log output to a file 80 | mcx::log::Module logger(path_log); 81 | logger.create("logger", ¶m_server, rt_dt_micro_s); 82 | // create and configure log output task 83 | mcx::container::Task logger_task("Logger_task", ¶m_server); 84 | logger_task.add(&logger); 85 | logger_task.configure(); 86 | 87 | // prints system configuration 88 | utils::printSystemConfig(cmd_args, "test_master"); 89 | 90 | // creates main control loop 91 | MainControlLoop main_control_loop(NUM_OF_DRIVES, NUM_OD_DIOS); 92 | main_control_loop.create("Control", ¶m_server, rt_dt_micro_s); 93 | 94 | // creates cia402 driver 95 | mcx::drive::Module cia402(cmd_args.system_mode, mcx::drive::DriveType::CiA402, NUM_OF_DRIVES); 96 | cia402.create("cia402", ¶m_server, rt_dt_micro_s); 97 | 98 | // creates and configure control task 99 | container::Task controls_task("Control_task", ¶m_server); 100 | // adds main control loop module to the control task 101 | controls_task.add(&main_control_loop); 102 | // adds cia402 drier to the control task 103 | controls_task.add(&cia402); 104 | controls_task.configure(); 105 | 106 | // creates EtherCAT master 107 | auto master = ecat::createMaster(cmd_args.system_mode); 108 | // loads a list of the domains from {cmd_args.config_path}/io/pdo.xml 109 | ecat::Domain domain("Domain1", path_ethercat, "pdo.xml"); 110 | // adds domains to the master 111 | ecat::Module ethercat(master, domain); 112 | ethercat.create("EtherCAT", ¶m_server, rt_dt_micro_s); 113 | 114 | // creates and configure EtherCAT task 115 | container::Task ethercat_task("EtherCAT_task", ¶m_server); 116 | ethercat_task.add(ðercat); 117 | ethercat_task.configure(); 118 | 119 | // creates req/rep server 120 | comm::RequestReply reqrep; 121 | // creates publisher module 122 | comm::Publisher publisher(reqrep, cmd_args.conn_data); 123 | publisher.create("ParamPub", ¶m_server, rt_dt_micro_s); 124 | // creates and configure publisher task 125 | container::Task comm_task("Comm_task", ¶m_server); 126 | comm_task.add(&publisher); 127 | comm_task.configure(); 128 | 129 | // when all modules are configured req/rep caches the parameter tree 130 | reqrep.configure((¶m_server), path_control_dir); 131 | 132 | // loads configuration from control.xml 133 | parameter_server::load(path_control, ¶m_server); 134 | 135 | 136 | // linking modules 137 | link(¶m_server); 138 | 139 | // starts logger and communication with non-realtime scheduler 140 | logger_task.start(rt_dt_micro_s, container::TaskSched::NORMAL); 141 | comm_task.start(rt_dt_micro_s, container::TaskSched::NORMAL); 142 | // starts control and ethercat with realtime scheduler, 143 | // attaches to isolated CPUs 0 and 1, sets priorities 144 | controls_task.start(rt_dt_micro_s, container::TaskSched::REALTIME, {0}, 80); 145 | ethercat_task.start(rt_dt_micro_s, container::TaskSched::REALTIME, {1}, 80); 146 | 147 | // starts req/rep server 148 | bool is_connected = reqrep.start(cmd_args.conn_data); 149 | ASSERT(is_connected, "Failed to start Req/Rep server"); 150 | 151 | // running until terminate signal is received 152 | while (utils::running() && ros::ok()) { 153 | reqrep.iterate(); 154 | } 155 | 156 | // stops all the tasks 157 | reqrep.stop(); 158 | comm_task.stop(); 159 | ethercat_task.stop(); 160 | controls_task.stop(); 161 | logger_task.stop(); 162 | 163 | // clears all allocated resources 164 | param_server.destroy(); 165 | 166 | } 167 | 168 | int main(int argc, char **argv) { 169 | 170 | // ros node initialization 171 | ros::init(argc, argv, "ethercat_master_server"); 172 | std::string ros_package_path = ros::package::getPath("ethercat_master"); 173 | 174 | using namespace utils; 175 | 176 | // default settings 177 | utils::CommandLineArgs command_line_args; 178 | command_line_args.config_path = ros_package_path + "/src/config"; 179 | command_line_args.log_path = "/var/log/motorcortex"; 180 | command_line_args.system_type = 0; 181 | command_line_args.system_mode = SystemMode::PRODUCTION; 182 | command_line_args.error_level = 3; 183 | command_line_args.app_version = std::string("0.1"); 184 | command_line_args.lib_version = std::string(utils::version()); 185 | command_line_args.conn_data = {.direction = 186 | comm::ConnectionDir::BIND_TO_LOCAL, 187 | .transport = "ws", .address = "*", .req_port = "5558", .pub_port 188 | = "5557"}; 189 | // parse settings from command line 190 | parseCmdLine(argc, argv, &command_line_args); 191 | 192 | // starts low latency, isolates CPU 0 and 1 193 | utils::startRealTime({0, 1}); 194 | // runs awesome controls 195 | run(command_line_args); 196 | 197 | // stops low latency, removes CPU isolation 198 | utils::stopRealTime(); 199 | 200 | return 0; 201 | } -------------------------------------------------------------------------------- /motion_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(motion_control) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | # message_generation 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | #add_message_files( 51 | # FILES 52 | # MotorcortexIn.msg 53 | # MotorcortexInList.msg 54 | # MotorcortexOut.msg 55 | # MotorcortexOutList.msg 56 | #) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | #generate_messages( 74 | # DEPENDENCIES 75 | # std_msgs 76 | #) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES motion_control 110 | CATKIN_DEPENDS rospy std_msgs 111 | # CATKIN_DEPENDS message_runtime 112 | # DEPENDS system_lib 113 | 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | # include 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/motion_control.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/motion_control_node.cpp) 141 | 142 | ## Rename C++ executable without prefix 143 | ## The above recommended prefix causes long target names, the following renames the 144 | ## target back to the shorter version for ease of user use 145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 147 | 148 | ## Add cmake target dependencies of the executable 149 | ## same as for the library above 150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | 157 | ############# 158 | ## Install ## 159 | ############# 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # install(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables and/or libraries for installation 172 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 173 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark cpp header files for installation 179 | # install(DIRECTORY include/${PROJECT_NAME}/ 180 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 181 | # FILES_MATCHING PATTERN "*.h" 182 | # PATTERN ".svn" EXCLUDE 183 | # ) 184 | 185 | ## Mark other files for installation (e.g. launch and bag files, etc.) 186 | # install(FILES 187 | # # myfile1 188 | # # myfile2 189 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | # ) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_motion_control.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) 204 | -------------------------------------------------------------------------------- /motion_control/launch/motion_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /motion_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | motion_control 4 | 0.1.0 5 | ROS EtherCAT package based on IgH EtherCAT Master for Linux and Motorcortex Core Library 6 | 7 | Viatcheslav Tretyakov 8 | Alexey Zakharov 9 | 10 | Synapticon GmbH 11 | 12 | BSD 13 | 14 | catkin 15 | rospy 16 | std_msgs 17 | rospy 18 | std_msgs 19 | rospy 20 | 21 | 22 | -------------------------------------------------------------------------------- /motion_control/scripts/dio.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from motorcortex_msgs.msg import DigitalOutputs 4 | 5 | class DIO(object): 6 | 7 | def __init__(self, id): 8 | self.id = id 9 | self.digitalOutputs = [False] * 12 10 | 11 | def update(self, feedbackMsg): 12 | self.digitalInputs = feedbackMsg.digital_inputs 13 | 14 | def getId(self): 15 | return self.id 16 | 17 | def getDigitalInputs(self): 18 | return self.digitalInputs 19 | 20 | def setDigitalOutputs(self, digital_outputs): 21 | self.digitalOutputs = digital_outputs 22 | 23 | def encode(self): 24 | ctrl_msg = DigitalOutputs() 25 | ctrl_msg.digital_outputs = self.digitalOutputs 26 | 27 | return ctrl_msg -------------------------------------------------------------------------------- /motion_control/scripts/drive.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from motorcortex_msgs.msg import DriveOut 4 | from enum import Enum 5 | 6 | class DriveCommand(Enum): 7 | DRIVE_CMD_OFF = 0x0 8 | DRIVE_CMD_DISENGAGE = 0x1 9 | DRIVE_CMD_ENGAGE = 0x2 10 | DRIVE_CMD_FAULT_ACK = 0x3 11 | DRIVE_CMD_QUICK_STOP = 0x4 12 | 13 | class DriveState(Enum): 14 | DRIVE_STATUS_OFF = 0x0 15 | DRIVE_STATUS_DISENGAGED = 0x1 16 | DRIVE_STATUS_ENGAGED = 0x2 17 | DRIVE_STATUS_FAULT = 0x3 18 | DRIVE_STATUS_QUICK_STOP_ACTIVE = 0x4 19 | 20 | class OpModesCiA402(Enum): 21 | CSP = 8 22 | CSV = 9 23 | CST = 10 24 | NONE = 0 25 | 26 | class Drive(object): 27 | enabled = False 28 | actualPosition = 0 29 | targetPosition = 0 30 | status = 0 31 | errorCode = 0 32 | command = DriveCommand.DRIVE_CMD_OFF.value 33 | mode = OpModesCiA402.NONE.value 34 | 35 | def __init__(self, id): 36 | self.id = id 37 | self.digitalOutputs = [False,False,False,False] 38 | self.targetPosition = None 39 | self.targetVelocity = 0 40 | self.targetTorque = 0 41 | self.torqueOffset = 0 42 | 43 | 44 | def update(self, statusMsg): 45 | self.status = statusMsg.statusword 46 | self.errorCode = statusMsg.drive_error_code 47 | self.slaveTimestamp = statusMsg.slave_timestamp 48 | self.actualPosition = statusMsg.position_value 49 | self.actualVelocity = statusMsg.velocity_value 50 | self.actualTorque = statusMsg.torque_value 51 | self.actualSecondaryPosition = statusMsg.secondary_position_value 52 | self.actualSecondaryVelocity = statusMsg.secondary_velocity_value 53 | self.analogInputs = statusMsg.analog_inputs 54 | self.digitalInputs = statusMsg.digital_inputs 55 | 56 | def getId(self): 57 | return self.id 58 | 59 | def isEnabled(self): 60 | return self.status == DriveState.DRIVE_STATUS_ENGAGED.value 61 | 62 | def setPosition(self, position): 63 | self.targetPosition = position 64 | 65 | def setVelocity(self, velocity): 66 | self.targetVelocity = velocity 67 | 68 | def setTorque(self, torque): 69 | self.targetTorque = torque 70 | 71 | def setTorqueOffset(self, torque_offset): 72 | self.torqueOffset = torque_offset 73 | 74 | def setDigitalOutputs(self, digital_outputs): 75 | self.digitalOutputs = digital_outputs 76 | 77 | def getTimestamp(self): 78 | return self.slaveTimestamp 79 | 80 | def getPosition(self): 81 | return self.actualPosition 82 | 83 | def getVelocity(self): 84 | return self.actualVelocity 85 | 86 | def getTorque(self): 87 | return self.actualTorque 88 | 89 | def getSecondaryPosition(self): 90 | return self.actualSecondaryPosition 91 | 92 | def getSecondaryVelocity(self): 93 | return self.actualSecondaryVelocity 94 | 95 | def getAnalogInputs(self): 96 | return self.analogInputs 97 | 98 | def getDigitalInputs(self): 99 | return self.digitalInputs 100 | 101 | def setMode(self, mode): 102 | self.mode = mode 103 | 104 | def switchOn(self): 105 | self.command = DriveCommand.DRIVE_CMD_ENGAGE.value 106 | 107 | def switchOff(self): 108 | self.command = DriveCommand.DRIVE_CMD_OFF.value 109 | 110 | def resetError(self): 111 | self.command = DriveCommand.DRIVE_CMD_FAULT_ACK.value 112 | 113 | def hasError(self): 114 | return self.status == DriveState.DRIVE_STATUS_FAULT.value 115 | 116 | def encode(self): 117 | ctrl_msg = DriveOut() 118 | ctrl_msg.controlword = self.command 119 | ctrl_msg.opmode = self.mode 120 | ctrl_msg.target_position = self.targetPosition 121 | ctrl_msg.target_velocity = self.targetVelocity 122 | ctrl_msg.target_torque = self.targetTorque 123 | ctrl_msg.torque_offset = self.torqueOffset 124 | ctrl_msg.digital_outputs = self.digitalOutputs 125 | 126 | return ctrl_msg 127 | -------------------------------------------------------------------------------- /motion_control/scripts/test_SDO_get_n_set.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from motorcortex_msgs.srv import * 5 | 6 | def read_cfg_client(request_drives): 7 | rospy.wait_for_service('get_sdo_config') 8 | try: 9 | read_cfg = rospy.ServiceProxy('get_sdo_config', GetSDOCfg) 10 | cfgs = read_cfg(request_drives) 11 | return cfgs.torque_controller_cfg, cfgs.velocity_controller_cfg, cfgs.position_controller_cfg 12 | except rospy.ServiceException as e: 13 | print "Service call failed: %s"%e 14 | 15 | 16 | def write_cfg_client(torqueControllerCfgs, velocityControllerCfgs, positionControllerCfgs): 17 | rospy.wait_for_service('set_sdo_config') 18 | try: 19 | write_cfg = rospy.ServiceProxy('set_sdo_config', SetSDOCfg) 20 | successes = write_cfg(torqueControllerCfgs, velocityControllerCfgs, positionControllerCfgs) 21 | return successes 22 | except rospy.ServiceException as e: 23 | print "Service call failed: %s"%e 24 | 25 | 26 | if __name__ == "__main__": 27 | 28 | rospy.init_node('SDO_updater', anonymous=True) 29 | rate = rospy.Rate(2) # 2hz 30 | 31 | while not rospy.is_shutdown(): 32 | 33 | torqueControllerCfgs = [] 34 | velocityControllerCfgs = [] 35 | positionControllerCfgs = [] 36 | 37 | request_drives = [True, True] 38 | torques, velocities, positions = read_cfg_client(request_drives) 39 | for torque, velocity, position in zip(torques, velocities, positions): 40 | 41 | print ("******************* Torque *********************") 42 | print (torque) 43 | print ("------------------ Velocity --------------------") 44 | print (velocity) 45 | print("------------------- Position --------------------") 46 | print (position) 47 | print("********************************************") 48 | 49 | #change some parameters for a test purpose 50 | velocity.controller_Kp = velocity.controller_Kp + 0.000001 51 | 52 | torqueControllerCfgs.append(torque) 53 | velocityControllerCfgs.append(velocity) 54 | positionControllerCfgs.append(position) 55 | 56 | successes = write_cfg_client(torqueControllerCfgs, velocityControllerCfgs, positionControllerCfgs) 57 | 58 | print(successes) 59 | 60 | rate.sleep() -------------------------------------------------------------------------------- /motion_control/scripts/test_ctrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import print_function, division 4 | 5 | import rospy 6 | from drive import Drive, OpModesCiA402 7 | from dio import DIO 8 | 9 | from motorcortex_msgs.msg import DriveOutList, DriveInList, DigitalInputsList, DigitalOutputsList 10 | 11 | # list your slave devices here 12 | drives = [Drive(0), Drive(1)] 13 | dios = [] # for example, [DIO(0)] 14 | 15 | 16 | def drives_feedback_callback(drive_feedback): 17 | for drive in drives: 18 | drive.update(drive_feedback.drives_feedback[drive.getId()]) 19 | 20 | def digital_inputs_callback(digital_inputs): 21 | if not dios: 22 | rospy.logerr("The list of DIOs is empty. Please unsubscribe or list the devices") 23 | else: 24 | for dio in dios: 25 | dio.update(digital_inputs.devices_feedback[dio.getId()]) 26 | 27 | # publish control commands 28 | drives_pub = rospy.Publisher('/drive_control', DriveOutList, queue_size = 1) 29 | if dios: 30 | dios_pub = rospy.Publisher('/digital_outputs', DigitalOutputsList, queue_size = 1) 31 | 32 | # subscribe to feedback topics 33 | rospy.Subscriber("/drive_feedback", DriveInList, drives_feedback_callback) 34 | if dios: 35 | rospy.Subscriber("/digital_inputs", DigitalInputsList, digital_inputs_callback) 36 | 37 | 38 | def safe_off(): 39 | drivesControlMsg = DriveOutList() 40 | for drive in drives: 41 | drive.switchOff() 42 | drivesControlMsg.drive_command.append(drive.encode()) 43 | 44 | digitalOutputsControlMsg = DigitalOutputsList() 45 | if dios: 46 | for dio in dios: 47 | digital_outputs = [False] * 12 48 | dio.setDigitalOutputs(digital_outputs) 49 | digitalOutputsControlMsg.devices_command.append(dio.encode()) 50 | 51 | drives_pub.publish(drivesControlMsg) 52 | if dios: 53 | dios_pub.publish(digitalOutputsControlMsg) 54 | 55 | def controller(): 56 | 57 | rospy.init_node('drive_control', anonymous=True) 58 | rospy.on_shutdown(safe_off) 59 | r = rospy.Rate(100)#Hz 60 | 61 | # set your test parameters here 62 | opMode = OpModesCiA402.CSV.value 63 | positionIncrement = 1000 64 | referenceVelocity = 500 65 | referenceTorque = 50 66 | 67 | counter = 0 68 | 69 | # switch on here 70 | while not rospy.is_shutdown(): 71 | # control here 72 | drivesControlMsg = DriveOutList() 73 | for drive in drives: 74 | if drive.hasError(): 75 | print("drive %d has error"%drive.getId()) 76 | drive.resetError() 77 | else: 78 | drive.setMode(opMode) 79 | drive.switchOn() 80 | 81 | if drive.isEnabled(): 82 | if opMode == OpModesCiA402.CSP.value: 83 | drive.setPosition(drive.getPosition() + positionIncrement) 84 | elif opMode == OpModesCiA402.CSV.value: 85 | drive.setVelocity(referenceVelocity) 86 | elif opMode == OpModesCiA402.CST.value: 87 | drive.setTorque(referenceTorque) 88 | else: 89 | drive.setPosition(drive.getPosition()) 90 | drive.setVelocity(0) 91 | drive.setTorque(0) 92 | 93 | drivesControlMsg.drive_command.append(drive.encode()) 94 | 95 | digitalOutputsControlMsg = DigitalOutputsList() 96 | for dio in dios: 97 | # blink 98 | if counter < 100: 99 | output_state = True 100 | else: 101 | output_state = False 102 | 103 | digital_outputs = [output_state] * 12 104 | dio.setDigitalOutputs(digital_outputs) 105 | digitalOutputsControlMsg.devices_command.append(dio.encode()) 106 | 107 | # send here 108 | drives_pub.publish(drivesControlMsg) 109 | if dios: 110 | dios_pub.publish(digitalOutputsControlMsg) 111 | 112 | counter += 1 113 | if counter > 200: 114 | counter = 0 115 | 116 | r.sleep() 117 | 118 | 119 | 120 | if __name__ == '__main__': 121 | try: 122 | controller() 123 | except rospy.ROSInterruptException: pass -------------------------------------------------------------------------------- /motion_control/scripts/test_get_SDO.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from motorcortex_msgs.srv import * 5 | 6 | def read_cfg_client(request_drives): 7 | rospy.wait_for_service('get_sdo_config') 8 | try: 9 | read_cfg = rospy.ServiceProxy('get_sdo_config', GetSDOCfg) 10 | cfgs = read_cfg(request_drives) 11 | return cfgs.torque_controller_cfg, cfgs.velocity_controller_cfg, cfgs.position_controller_cfg 12 | except rospy.ServiceException, e: 13 | print "Service call failed: %s"%e 14 | 15 | 16 | if __name__ == "__main__": 17 | request_drives = [True, True] 18 | torques, velocities, positions = read_cfg_client(request_drives) 19 | for torque, velocity, position in zip(torques, velocities, positions): 20 | print ("********************************************") 21 | print (torque) 22 | print ("--------------------------------------------") 23 | print (velocity) 24 | print("--------------------------------------------") 25 | print (position) 26 | print("********************************************") -------------------------------------------------------------------------------- /motion_control/scripts/test_restore_params.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from motorcortex_msgs.srv import * 5 | 6 | def reset_cfg_client(restore_on_slaves): 7 | rospy.wait_for_service('restore_default_sdo_config') 8 | try: 9 | restore_cfg = rospy.ServiceProxy('restore_default_sdo_config', RestoreCfgParams) 10 | successes = restore_cfg(restore_on_slaves) 11 | return successes 12 | except rospy.ServiceException as e: 13 | print "Store service call failed: %s"%e 14 | 15 | 16 | if __name__ == "__main__": 17 | restore = [True, True] 18 | 19 | successes = reset_cfg_client(restore) 20 | 21 | print (successes) 22 | -------------------------------------------------------------------------------- /motion_control/scripts/test_save_params.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from motorcortex_msgs.srv import * 5 | 6 | def save_cfg_client(save_on_slaves): 7 | rospy.wait_for_service('save_sdo_config') 8 | try: 9 | save_cfg = rospy.ServiceProxy('save_sdo_config', SaveCfgParams) 10 | successes = save_cfg(save_on_slaves) 11 | return successes 12 | except rospy.ServiceException as e: 13 | print "Store service call failed: %s"%e 14 | 15 | 16 | if __name__ == "__main__": 17 | 18 | save = [True, True] 19 | 20 | successes = save_cfg_client(save) 21 | 22 | print (successes) 23 | -------------------------------------------------------------------------------- /motion_control/scripts/test_set_SDO.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from motorcortex_msgs.srv import * 5 | from motorcortex_msgs.msg import TorqueControllerCfg, VelocityControllerCfg, PositionControllerCfg 6 | 7 | def write_cfg_client(torqueControllerCfgs, velocityControllerCfgs, positionControllerCfgs): 8 | rospy.wait_for_service('set_sdo_config') 9 | try: 10 | write_cfg = rospy.ServiceProxy('set_sdo_config', SetSDOCfg) 11 | successes = write_cfg(torqueControllerCfgs, velocityControllerCfgs, positionControllerCfgs) 12 | return successes 13 | except rospy.ServiceException, e: 14 | print "Service call failed: %s"%e 15 | 16 | 17 | if __name__ == "__main__": 18 | torqueControllerCfgs = [TorqueControllerCfg(), TorqueControllerCfg()] 19 | velocityControllerCfgs = [VelocityControllerCfg(), VelocityControllerCfg()] 20 | positionControllerCfgs = [PositionControllerCfg(), PositionControllerCfg()] 21 | 22 | successes = write_cfg_client(torqueControllerCfgs, velocityControllerCfgs, positionControllerCfgs) 23 | 24 | print (successes) 25 | -------------------------------------------------------------------------------- /motorcortex_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(motorcortex_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | ) 7 | 8 | ## Generate messages in the 'msg' folder 9 | add_message_files( 10 | FILES 11 | DriveIn.msg 12 | DriveInList.msg 13 | DriveOut.msg 14 | DriveOutList.msg 15 | DigitalInputs.msg 16 | DigitalInputsList.msg 17 | DigitalOutputs.msg 18 | DigitalOutputsList.msg 19 | TorqueControllerCfg.msg 20 | VelocityControllerCfg.msg 21 | PositionControllerCfg.msg 22 | ) 23 | 24 | add_service_files( 25 | FILES 26 | GetSDOCfg.srv 27 | SetSDOCfg.srv 28 | SaveCfgParams.srv 29 | RestoreCfgParams.srv 30 | ) 31 | 32 | ## Generate added messages and services with any dependencies listed here 33 | generate_messages() 34 | 35 | catkin_package( 36 | CATKIN_DEPENDS message_runtime 37 | ) 38 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DigitalInputs.msg: -------------------------------------------------------------------------------- 1 | bool[] digital_inputs 2 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DigitalInputsList.msg: -------------------------------------------------------------------------------- 1 | DigitalInputs[] devices_feedback 2 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DigitalOutputs.msg: -------------------------------------------------------------------------------- 1 | bool[] digital_outputs 2 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DigitalOutputsList.msg: -------------------------------------------------------------------------------- 1 | DigitalOutputs[] devices_command 2 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DriveIn.msg: -------------------------------------------------------------------------------- 1 | int32 statusword 2 | int32 drive_error_code 3 | uint32 slave_timestamp 4 | int32 position_value 5 | int32 velocity_value 6 | int16 torque_value 7 | int32 secondary_position_value 8 | int32 secondary_velocity_value 9 | uint16[] analog_inputs 10 | bool[] digital_inputs -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DriveInList.msg: -------------------------------------------------------------------------------- 1 | DriveIn[] drives_feedback 2 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DriveOut.msg: -------------------------------------------------------------------------------- 1 | int32 controlword 2 | int32 opmode 3 | int32 target_position 4 | int32 target_velocity 5 | int16 target_torque 6 | int16 torque_offset 7 | bool[] digital_outputs -------------------------------------------------------------------------------- /motorcortex_msgs/msg/DriveOutList.msg: -------------------------------------------------------------------------------- 1 | DriveOut[] drive_command 2 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/PositionControllerCfg.msg: -------------------------------------------------------------------------------- 1 | float32 position_loop_Kp 2 | float32 position_loop_Ki 3 | float32 position_loop_Kd 4 | uint32 position_loop_integral_limit 5 | float32 velocity_loop_Kp 6 | float32 velocity_loop_Ki 7 | float32 velocity_loop_Kd 8 | uint32 velocity_loop_integral_limit 9 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/TorqueControllerCfg.msg: -------------------------------------------------------------------------------- 1 | float32 controller_Kp 2 | float32 controller_Ki 3 | float32 controller_Kd 4 | uint32 field_weakening_enable 5 | uint32 field_weakening_percentage 6 | uint32 field_weakening_starting_speed 7 | uint32 field_weakening_ending_speed 8 | uint32 commutation_angle_measurement_delay 9 | -------------------------------------------------------------------------------- /motorcortex_msgs/msg/VelocityControllerCfg.msg: -------------------------------------------------------------------------------- 1 | float32 controller_Kp 2 | float32 controller_Ki 3 | float32 controller_Kd 4 | uint32 controller_integral_limit 5 | -------------------------------------------------------------------------------- /motorcortex_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | motorcortex_msgs 4 | 0.1.0 5 | 6 | generic Motorcortex messages for communicating with EtherCAT slave devices 7 | 8 | 9 | Viatcheslav Tretyakov 10 | Alexey Zakharov 11 | 12 | Synapticon GmbH 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | message_generation 19 | message_runtime 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /motorcortex_msgs/srv/GetSDOCfg.srv: -------------------------------------------------------------------------------- 1 | bool[] read_cfg 2 | --- 3 | TorqueControllerCfg[] torque_controller_cfg 4 | VelocityControllerCfg[] velocity_controller_cfg 5 | PositionControllerCfg[] position_controller_cfg 6 | -------------------------------------------------------------------------------- /motorcortex_msgs/srv/RestoreCfgParams.srv: -------------------------------------------------------------------------------- 1 | bool[] reset_to_default 2 | --- 3 | bool[] success 4 | -------------------------------------------------------------------------------- /motorcortex_msgs/srv/SaveCfgParams.srv: -------------------------------------------------------------------------------- 1 | bool[] save 2 | --- 3 | bool[] success 4 | -------------------------------------------------------------------------------- /motorcortex_msgs/srv/SetSDOCfg.srv: -------------------------------------------------------------------------------- 1 | TorqueControllerCfg[] torque_controller_cfg 2 | VelocityControllerCfg[] velocity_controller_cfg 3 | PositionControllerCfg[] position_controller_cfg 4 | --- 5 | bool[] success 6 | -------------------------------------------------------------------------------- /ros_ethercat_igh/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_ethercat_igh) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ros_ethercat_igh/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_ethercat_igh 4 | 0.1.0 5 | 6 | ROS EtherCAT IgH meta-package 7 | 8 | 9 | Viatcheslav Tretyakov 10 | Alexey Zakharov 11 | 12 | Synapticon GmbH 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | ethercat_master 19 | motion_control 20 | motorcortex_msgs 21 | 22 | 23 | 24 | 25 | --------------------------------------------------------------------------------