├── .gitignore ├── .version ├── CMakeLists.txt ├── LICENSE ├── README.md ├── Uranus.cmake ├── demo ├── axis_homing.cpp └── axis_move.cpp ├── fb ├── FbPLCOpenBase.cpp ├── FbPLCOpenBase.hpp ├── FbSingleAxis.cpp ├── FbSingleAxis.hpp ├── FunctionBlock.cpp ├── FunctionBlock.hpp └── PLCTypes.hpp ├── misc ├── Event.hpp ├── ExeclQueue.cpp ├── ExeclQueue.hpp ├── LinkList.cpp ├── LinkList.hpp └── Queue.hpp └── motion ├── Global.hpp ├── Scheduler.cpp ├── Scheduler.hpp ├── Servo.cpp ├── Servo.hpp ├── axis ├── Axis.cpp ├── Axis.hpp ├── AxisBase.cpp ├── AxisBase.hpp ├── AxisHoming.cpp ├── AxisHoming.hpp ├── AxisMotion.cpp ├── AxisMotion.hpp ├── AxisMotionBase.cpp ├── AxisMotionBase.hpp ├── AxisMove.cpp ├── AxisMove.hpp ├── AxisStatus.cpp └── AxisStatus.hpp └── utils ├── MathUtils.hpp ├── ProfilePlanner.cpp ├── ProfilePlanner.hpp ├── ProfilesPlanner.cpp └── ProfilesPlanner.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /.version: -------------------------------------------------------------------------------- 1 | 0.0.1 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 3 | # 4 | # Licensed to the Apache Software Foundation (ASF) under one 5 | # or more contributor license agreements. See the NOTICE file 6 | # distributed with this work for additional information 7 | # regarding copyright ownership. The ASF licenses this file 8 | # to you under the Apache License, Version 2.0 (the 9 | # "License"); you may not use this file except in compliance 10 | # with the License. You may obtain a copy of the License at 11 | # 12 | # http://www.apache.org/licenses/LICENSE-2.0 13 | # 14 | # Unless required by applicable law or agreed to in writing, 15 | # software distributed under the License is distributed on an 16 | # "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 17 | # KIND, either express or implied. See the License for the 18 | # specific language governing permissions and limitations 19 | # under the License. 20 | # 21 | 22 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8) 23 | 24 | PROJECT(Uranus) 25 | 26 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O2 -fPIC -ggdb3") 27 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CMAKE_C_FLAGS}") 28 | 29 | INCLUDE(Uranus.cmake) 30 | 31 | ADD_LIBRARY(${PROJECT_NAME} SHARED ${URANUS_SOURCE}) 32 | 33 | FILE(STRINGS ".version" URANUS_VER) 34 | SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES VERSION ${URANUS_VER} SOVERSION 0) 35 | 36 | ADD_EXECUTABLE(axis_move demo/axis_move.cpp) 37 | TARGET_LINK_LIBRARIES(axis_move ${PROJECT_NAME}) 38 | 39 | ADD_EXECUTABLE(axis_homing demo/axis_homing.cpp) 40 | TARGET_LINK_LIBRARIES(axis_homing ${PROJECT_NAME}) 41 | 42 | INSTALL(TARGETS Uranus 43 | LIBRARY DESTINATION lib 44 | ) 45 | 46 | INSTALL(FILES 47 | fb/FunctionBlock.hpp 48 | fb/FbPLCOpenBase.hpp 49 | fb/FbSingleAxis.hpp 50 | fb/PLCTypes.hpp 51 | motion/Servo.hpp 52 | motion/Global.hpp 53 | motion/Scheduler.hpp 54 | DESTINATION include/Uranus 55 | ) 56 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PLCOpen Motion - Uranus 2 | 3 | Uranus is a C++ library that implements the part 1 & 2 of PLCopen motion control standard. 4 | 5 | # Currently included function blocks 6 | Function Block | Description 7 | ------------------------- | ----------------------------------- 8 | MC_Power | Controls the power stage (On or Off). 9 | MC_Home | Commands the axis to perform the «search home» sequence. 10 | MC_Stop | Commands a controlled motion stop and transfers the axis to the state ‘Stopping’. 11 | MC_Halt | Commands a controlled motion stop and transfers the axis to the state ‘StandStill’. 12 | MC_MoveAbsolute | Commands a controlled motion to a specified absolute position. 13 | MC_MoveRelative | Commands a controlled motion of a specified distance relative to the set position at the time of the execution. 14 | MC_MoveAdditive | Commands a controlled motion of a specified relative distance additional to the most recent commanded position. 15 | MC_MoveVelocity | Commands a never ending controlled motion at a specified velocity. 16 | MC_ReadStatus | Returns in detail the status of the state diagram of the selected axis. 17 | MC_ReadMotionState | Returns in detail the status of the axis with respect to the motion currently in progress. 18 | MC_ReadAxisError | Reads information concerning an axis, like modes, inputs directly related to the axis, and certain status information. 19 | MC_EmergencyStop | Commands the axis to stop immediately and transfers the axis to the state ‘ErrorStop’. 20 | MC_Reset | Makes the transition from the state ‘ErrorStop’ to ‘Standstill’ or ‘Disabled’ by resetting all internal axis-related errors. 21 | MC_ReadActualPosition | Returns the actual position. 22 | MC_ReadCommandPosition | Returns the command position. 23 | MC_ReadActualVelocity | Returns the actual velocity. 24 | MC_ReadCommandVelocity | Returns the command velocity. 25 | 26 | # Build & install commands 27 | 28 | mkdir build 29 | cd build 30 | cmake .. 31 | make 32 | sudo make install 33 | -------------------------------------------------------------------------------- /Uranus.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 3 | # 4 | # Licensed to the Apache Software Foundation (ASF) under one 5 | # or more contributor license agreements. See the NOTICE file 6 | # distributed with this work for additional information 7 | # regarding copyright ownership. The ASF licenses this file 8 | # to you under the Apache License, Version 2.0 (the 9 | # "License"); you may not use this file except in compliance 10 | # with the License. You may obtain a copy of the License at 11 | # 12 | # http://www.apache.org/licenses/LICENSE-2.0 13 | # 14 | # Unless required by applicable law or agreed to in writing, 15 | # software distributed under the License is distributed on an 16 | # "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 17 | # KIND, either express or implied. See the License for the 18 | # specific language governing permissions and limitations 19 | # under the License. 20 | # 21 | 22 | set(CMAKE_CXX_STANDARD 11) 23 | 24 | INCLUDE_DIRECTORIES( 25 | include 26 | motion 27 | motion/axis 28 | motion/utils 29 | fb 30 | misc 31 | ) 32 | 33 | AUX_SOURCE_DIRECTORY(motion URANUS_SOURCE) 34 | AUX_SOURCE_DIRECTORY(motion/axis URANUS_SOURCE) 35 | AUX_SOURCE_DIRECTORY(motion/utils URANUS_SOURCE) 36 | AUX_SOURCE_DIRECTORY(fb URANUS_SOURCE) 37 | AUX_SOURCE_DIRECTORY(misc URANUS_SOURCE) 38 | -------------------------------------------------------------------------------- /demo/axis_homing.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * axis_homing.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | * 示例代码，使用MC_Power、MC_Home使能轴并完成回零 24 | * MC_ReadActualPosition与ReadActualVelocity用于读取当前位置与速度 25 | * MyAxis继承Servo类仿真实现回零点在某位置被触发与回弹的电平变化 26 | * 27 | */ 28 | 29 | #include "Scheduler.hpp" 30 | #include "FbSingleAxis.hpp" 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | using namespace Uranus; 37 | using namespace std; 38 | 39 | class MyServo : public Servo 40 | { 41 | public: 42 | uint8_t homingSwitch = false; //定义“物理”回零点 43 | int32_t homingSwitchPosition = 50 * 8192; //定义回零点的“编码器位置” 44 | 45 | protected: 46 | virtual void runCycle(double freq) override; 47 | }; 48 | 49 | void MyServo::runCycle(double freq) 50 | { 51 | Servo::runCycle(freq); 52 | //当前“编码器位置”超出homingSwitchPosition点，则回零点置为true,反之置为false 53 | homingSwitch = (pos() >= homingSwitchPosition); 54 | } 55 | 56 | int main(void) 57 | { 58 | cout.precision(8); 59 | 60 | //调度器初始化 61 | Scheduler sched; 62 | double frequency = 100; //定义外部周期调度的频率 63 | int32_t axisId = 1; //id可随意定义，各轴不重复即可 64 | sched.setFrequency(frequency); 65 | MyServo* servo = new MyServo(); 66 | Axis* axis = sched.newAxis(axisId, servo); 67 | //设定axis的回零开关地址与回零参数 68 | AxisConfig config; 69 | config.mHomingInfo.mHomingSig = &servo->homingSwitch; 70 | config.mHomingInfo.mHomingMode = MC_HOMINGMODE_MODE7; 71 | config.mHomingInfo.mHomingVelSearch = 20; 72 | config.mHomingInfo.mHomingVelRegression = 1; 73 | config.mHomingInfo.mHomingAcc = 50; 74 | sched.setAxisConfig(axis, config); 75 | 76 | //功能块初始化 77 | FbPower power; 78 | power.mAxis = axis; 79 | power.mEnable = true; 80 | power.mEnablePositive = true; 81 | power.mEnableNegative = true; 82 | 83 | FbHome home; 84 | home.mAxis = axis; 85 | home.mPosition = 200; //零点位置指定为位置200 86 | 87 | FbReadActualPosition readPos; 88 | readPos.mAxis = axis; 89 | readPos.mEnable = true; 90 | 91 | FbReadCommandVelocity readVel; 92 | readVel.mAxis = axis; 93 | readVel.mEnable = true; 94 | 95 | double t = 0; 96 | //“实时”周期任务 97 | while(1) { 98 | //调度器周期处理 99 | sched.runCycle(); 100 | 101 | //功能块调用 102 | power.call(); 103 | home.call(); 104 | readPos.call(); 105 | readVel.call(); 106 | 107 | //显示当前时间，轴的位置与速度 108 | cout << fixed << t << " " 109 | << readPos.mPosition << " " 110 | << readVel.mVelocity << " " 111 | << endl; 112 | 113 | bool isPowerOn = power.mStatus && power.mValid; 114 | if(isPowerOn && !home.mExecute) { //使能成功后开始回零 115 | cout << "axis poweron, homing start" << endl; 116 | home.mExecute = isPowerOn; 117 | } 118 | 119 | if(home.mDone) { //提示回零完成并退出 120 | cout << "homing complete" << endl; 121 | break; 122 | } 123 | 124 | usleep(1000000 / frequency); //演示用，sleep代替实时定时器 125 | t += 1 / frequency; 126 | } 127 | 128 | return 0; 129 | } 130 | -------------------------------------------------------------------------------- /demo/axis_move.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * axis_move.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | * 示例代码，使用MC_Power、MC_MoveAbsolute使能轴并完成两段移动 24 | * MC_ReadActualPosition与ReadActualVelocity用于读取当前位置与速度 25 | * 26 | */ 27 | 28 | #include "Scheduler.hpp" 29 | #include "FbSingleAxis.hpp" 30 | #include 31 | #include 32 | #include 33 | 34 | using namespace Uranus; 35 | using namespace std; 36 | 37 | int main(void) 38 | { 39 | cout.precision(8); 40 | 41 | //调度器初始化 42 | Scheduler sched; 43 | double frequency = 100; //定义外部周期调度的频率 44 | int32_t axisId = 1; //id可随意定义，各轴不重复即可 45 | sched.setFrequency(frequency); 46 | Axis* axis = sched.newAxis(axisId, new Servo()); 47 | //new完成后也可通过 axis = sched.axis(axisId); 获取axis实例 48 | 49 | //功能块初始化 50 | FbPower power; 51 | power.mAxis = axis; 52 | power.mEnable = true; 53 | power.mEnablePositive = true; 54 | power.mEnableNegative = true; 55 | 56 | FbMoveAbsolute moveAbs1; 57 | moveAbs1.mAxis = axis; 58 | moveAbs1.mPosition = 500; 59 | moveAbs1.mVelocity = 400; 60 | moveAbs1.mAcceleration = 500; 61 | moveAbs1.mDeceleration = 500; 62 | 63 | FbMoveAbsolute moveAbs2; 64 | moveAbs2.mAxis = axis; 65 | moveAbs2.mPosition = 1000; 66 | moveAbs2.mVelocity = 200; 67 | moveAbs2.mAcceleration = 300; 68 | moveAbs2.mDeceleration = 300; 69 | moveAbs2.mBufferMode = MC_BUFFERMODE_BUFFERED; 70 | 71 | FbReadActualPosition readPos; 72 | readPos.mAxis = axis; 73 | readPos.mEnable = true; 74 | 75 | FbReadCommandVelocity readVel; 76 | readVel.mAxis = axis; 77 | readVel.mEnable = true; 78 | 79 | double t = 0; 80 | bool moveAbs1AlreadyDone = false; 81 | //“实时”周期任务 82 | while(1) { 83 | //调度器周期处理 84 | sched.runCycle(); 85 | 86 | //功能块调用 87 | power.call(); 88 | moveAbs1.call(); 89 | moveAbs2.call(); 90 | readPos.call(); 91 | readVel.call(); 92 | 93 | //显示当前时间，轴的位置与速度 94 | cout << "time:" << fixed << t 95 | << ",\tposition:" << readPos.mPosition 96 | << ",\tvelocity:" << readVel.mVelocity 97 | << endl; 98 | 99 | bool isPowerOn = power.mStatus && power.mValid; 100 | if(isPowerOn && !moveAbs1.mExecute) { //使能成功后move1开始移动 101 | cout << "axis poweron, moveAbs1 start" << endl; 102 | moveAbs1.mExecute = isPowerOn; 103 | } 104 | 105 | if(moveAbs1.mDone && !moveAbs1AlreadyDone) { //提示move1完成 106 | cout << "moveAbs1 complete, moveAbs2 start" << endl; 107 | moveAbs1AlreadyDone = true; 108 | } 109 | 110 | moveAbs2.mExecute = moveAbs1.mBusy; 111 | 112 | if(moveAbs2.mDone) { //提示move2完成并退出 113 | cout << "moveAbs2 complete" << endl; 114 | break; 115 | } 116 | 117 | usleep(1000000 / frequency); //演示用，sleep代替实时定时器 118 | t += 1 / frequency; 119 | } 120 | 121 | return 0; 122 | } 123 | -------------------------------------------------------------------------------- /fb/FbPLCOpenBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FbPLCOpenBase.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "FbPLCOpenBase.hpp" 26 | 27 | namespace Uranus { 28 | 29 | void FbBaseType::onOperationError(MC_ErrorCode errorCode, int32_t customId) 30 | { 31 | mError = true; 32 | mErrorID = errorCode; 33 | } 34 | 35 | void FbBaseType::clearError(void) 36 | { 37 | mError = false; 38 | mErrorID = MC_ERRORCODE_GOOD; 39 | } 40 | 41 | //////////////////////////////////////////////////////////// 42 | 43 | void FbEnableType::call(void) 44 | { 45 | MC_ErrorCode err = mEnable? onEnableTrue(): onEnableFalse(); 46 | if(err) 47 | onOperationError(err, 0); 48 | else 49 | clearError(); 50 | } 51 | 52 | //////////////////////////////////////////////////////////// 53 | 54 | void FbComExecuteType::call(void) 55 | { 56 | if(mExecute) { 57 | if(mDone || mError) 58 | return; 59 | 60 | bool isDone = false; 61 | MC_ErrorCode err = onExecTriggered(isDone); 62 | if(err) { 63 | onOperationError(err, 0); 64 | return; 65 | } 66 | 67 | if(isDone) { 68 | mDone = true; 69 | mBusy = false; 70 | } else { 71 | mDone = false; 72 | mBusy = true; 73 | } 74 | } else { 75 | mDone = false; 76 | mBusy = false; 77 | } 78 | 79 | clearError(); 80 | } 81 | 82 | void FbComExecuteType::onOperationError(MC_ErrorCode errorCode, int32_t customId) 83 | { 84 | mDone = mBusy = false; 85 | FbBaseType::onOperationError(errorCode, customId); 86 | } 87 | 88 | //////////////////////////////////////////////////////////// 89 | 90 | void FbSeqExecuteType::call(void) 91 | { 92 | if(mExecute && !mExecuteTrigger) { //上升沿 93 | MC_ErrorCode err = onExecPosedge(); 94 | if(err) { 95 | onOperationError(err, 0); 96 | } else { 97 | mDone = mActive = mCommandAborted = false; 98 | mBusy = true; 99 | mOkFlag = false; 100 | clearError(); 101 | } 102 | } else if(mExecute && mExecuteTrigger) { //高值 103 | 104 | } else if(!mExecute && mExecuteTrigger) { //下降沿 105 | if((mDone || mCommandAborted || mError) && !mBusy) { 106 | mDone = mCommandAborted = mBusy = mActive = false; 107 | clearError(); 108 | } 109 | mOkFlag = false; 110 | onExecNegedge(); 111 | } else { //低值 112 | if(!mBusy && !mOkFlag) { 113 | mDone = mCommandAborted = mBusy = mActive = false; 114 | clearError(); 115 | } 116 | mOkFlag = false; 117 | } 118 | 119 | mExecuteTrigger = mExecute; 120 | } 121 | 122 | void FbSeqExecuteType::onOperationActive(int32_t customId) 123 | { 124 | mDone = mCommandAborted = false; 125 | mBusy = mActive = true; 126 | clearError(); 127 | } 128 | 129 | void FbSeqExecuteType::onOperationAborted(int32_t customId) 130 | { 131 | mDone = mBusy = mActive = false; 132 | mCommandAborted = true; 133 | mOkFlag = !mExecute; 134 | clearError(); 135 | } 136 | 137 | void FbSeqExecuteType::onOperationDone(int32_t customId) 138 | { 139 | mCommandAborted = mBusy = mActive = false; 140 | mDone = true; 141 | mOkFlag = !mExecute; 142 | clearError(); 143 | } 144 | 145 | void FbSeqExecuteType::onOperationError(MC_ErrorCode errorCode, int32_t customId) 146 | { 147 | mDone = mBusy = mActive = mCommandAborted = false; 148 | mOkFlag = !mExecute; 149 | FbBaseType::onOperationError(errorCode, customId); 150 | } 151 | 152 | //////////////////////////////////////////////////////////// 153 | 154 | MC_ErrorCode FbReadInfoType::onEnableTrue(void) 155 | { 156 | bool isDone = false; 157 | MC_ErrorCode err = onEnable(isDone); 158 | if(err) return err; 159 | mValid = isDone; 160 | mBusy = !isDone; 161 | return MC_ERRORCODE_GOOD; 162 | } 163 | 164 | MC_ErrorCode FbReadInfoType::onEnableFalse(void) 165 | { 166 | mValid = false; 167 | mBusy = false; 168 | onDisable(); 169 | return MC_ERRORCODE_GOOD; 170 | } 171 | 172 | void FbReadInfoType::onOperationError(MC_ErrorCode errorCode, int32_t customId) 173 | { 174 | mValid = false; 175 | mBusy = false; 176 | onDisable(); 177 | FbBaseType::onOperationError(errorCode, customId); 178 | } 179 | 180 | //////////////////////////////////////////////////////////// 181 | 182 | MC_ErrorCode FbExecAxisType::onExecPosedge(void) 183 | { 184 | return mAxis? onAxisExecPosedge(): MC_ERRORCODE_AXISNOTEXIST; 185 | } 186 | 187 | //////////////////////////////////////////////////////////// 188 | 189 | void FbExecAxisBufferContType::onOperationDone(int32_t customId) 190 | { 191 | mCommandAborted = false; 192 | mBusy = mActive = mDone = true; 193 | clearError(); 194 | } 195 | 196 | //////////////////////////////////////////////////////////// 197 | 198 | MC_ErrorCode FbReadInfoAxisType::onEnable(bool& isDone) 199 | { 200 | return mAxis? onAxisEnable(isDone): MC_ERRORCODE_AXISNOTEXIST; 201 | } 202 | 203 | //////////////////////////////////////////////////////////// 204 | 205 | MC_ErrorCode FbWriteInfoAxisType::onExecTriggered(bool& isDone) 206 | { 207 | return mAxis? onAxisTriggered(isDone): MC_ERRORCODE_AXISNOTEXIST; 208 | } 209 | 210 | //////////////////////////////////////////////////////////// 211 | 212 | MC_ErrorCode FbExecAxisBufferContSyncType::onAxisExecPosedge(void) 213 | { 214 | return mMaster? onMasterSlaveExecPosedge(): MC_ERRORCODE_AXISNOTEXIST; 215 | } 216 | 217 | //////////////////////////////////////////////////////////// 218 | 219 | 220 | } 221 | -------------------------------------------------------------------------------- /fb/FbPLCOpenBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FbPLCOpenBase.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_FBPLCOPENBASE_HPP_ 26 | #define _URANUS_FBPLCOPENBASE_HPP_ 27 | 28 | #include "FunctionBlock.hpp" 29 | #include "PLCTypes.hpp" 30 | 31 | namespace Uranus { 32 | 33 | #pragma pack(push) 34 | #pragma pack(4) 35 | 36 | #define FB_INPUT 37 | #define FB_OUTPUT 38 | 39 | class FbBaseType : virtual public FunctionBlock 40 | { 41 | public: 42 | FB_OUTPUT BOOL mError = false; 43 | FB_OUTPUT MC_ERRORCODE mErrorID = MC_ERRORCODE_GOOD; 44 | 45 | public: 46 | void onOperationError(MC_ErrorCode errorCode, int32_t customId); 47 | 48 | void clearError(void); 49 | 50 | public: 51 | virtual void call(void) = 0; 52 | }; 53 | 54 | class FbEnableType : virtual public FbBaseType 55 | { 56 | public: 57 | FB_INPUT BOOL mEnable = false; 58 | 59 | public: 60 | void call(void); 61 | 62 | public: 63 | virtual MC_ErrorCode onEnableTrue(void) = 0; 64 | virtual MC_ErrorCode onEnableFalse(void) = 0; 65 | }; 66 | 67 | class FbComExecuteType : virtual public FbBaseType 68 | { 69 | public: 70 | FB_INPUT BOOL mExecute = false; 71 | 72 | FB_OUTPUT BOOL mDone = false; 73 | FB_OUTPUT BOOL mBusy = false; 74 | 75 | public: 76 | void call(void); 77 | 78 | void onOperationError(MC_ErrorCode errorCode, int32_t customId); 79 | 80 | public: 81 | virtual MC_ErrorCode onExecTriggered(bool& isDone) = 0; 82 | }; 83 | 84 | class FbSeqExecuteType : virtual public FbBaseType 85 | { 86 | public: 87 | FB_INPUT BOOL mExecute = false; 88 | 89 | FB_OUTPUT BOOL mDone = false; 90 | FB_OUTPUT BOOL mBusy = false; 91 | FB_OUTPUT BOOL mActive = false; 92 | FB_OUTPUT BOOL mCommandAborted = false; 93 | 94 | public: 95 | void call(void); 96 | 97 | void onOperationActive(int32_t customId); 98 | 99 | void onOperationAborted(int32_t customId); 100 | 101 | void onOperationDone(int32_t customId); 102 | 103 | void onOperationError(MC_ErrorCode errorCode, int32_t customId); 104 | 105 | public: 106 | virtual MC_ErrorCode onExecPosedge(void) = 0; 107 | virtual void onExecNegedge(void){} 108 | 109 | private: 110 | bool mExecuteTrigger = false; 111 | bool mOkFlag = false; 112 | }; 113 | 114 | class FbReadInfoType : virtual public FbEnableType 115 | { 116 | public: 117 | FB_OUTPUT BOOL mValid = false; 118 | FB_OUTPUT BOOL mBusy = false; 119 | 120 | public: 121 | MC_ErrorCode onEnableTrue(void); 122 | MC_ErrorCode onEnableFalse(void); 123 | 124 | void onOperationError(MC_ErrorCode errorCode, int32_t customId); 125 | 126 | public: 127 | virtual MC_ErrorCode onEnable(bool& isDone) = 0; 128 | virtual void onDisable(void) = 0; 129 | }; 130 | 131 | class FbWriteInfoAxisType : virtual public FbComExecuteType 132 | { 133 | public: 134 | FB_INPUT AXIS_REF mAxis = nullptr; 135 | 136 | public: 137 | MC_ErrorCode onExecTriggered(bool& isDone); 138 | 139 | public: 140 | virtual MC_ErrorCode onAxisTriggered(bool& isDone) = 0; 141 | }; 142 | 143 | class FbReadInfoAxisType : virtual public FbReadInfoType 144 | { 145 | public: 146 | FB_INPUT AXIS_REF mAxis = nullptr; 147 | 148 | public: 149 | MC_ErrorCode onEnable(bool& isDone); 150 | 151 | public: 152 | virtual MC_ErrorCode onAxisEnable(bool& isDone) = 0; 153 | }; 154 | 155 | class FbCoordSystemType 156 | { 157 | public: 158 | FB_INPUT MC_COORD_SYSTEM mCoordSystem = MC_COORDSYSTEM_MCS; 159 | }; 160 | 161 | class FbBufferModeType : virtual public FbSeqExecuteType 162 | { 163 | public: 164 | FB_INPUT MC_BUFFER_MODE mBufferMode = MC_BUFFERMODE_ABORTING; 165 | }; 166 | 167 | class FbTranslModeType : virtual public FbSeqExecuteType 168 | { 169 | public: 170 | FB_INPUT MC_TRANSITION_MODE mTransitionMode = MC_TRANSITIONMODE_NONE; 171 | FB_INPUT LREAL mTransitionParameter[URANUS_TRANSITIONPARAMETER_NUM] = {0}; 172 | }; 173 | 174 | class FbExecAxisType : virtual public FbSeqExecuteType 175 | { 176 | public: 177 | FB_INPUT AXIS_REF mAxis = nullptr; 178 | 179 | public: 180 | MC_ErrorCode onExecPosedge(void); 181 | 182 | public: 183 | virtual MC_ErrorCode onAxisExecPosedge(void) = 0; 184 | }; 185 | 186 | class FbExecAxisBufferType : 187 | virtual public FbExecAxisType, virtual public FbBufferModeType 188 | { 189 | }; 190 | 191 | class FbExecAxisBufferContType : virtual public FbExecAxisBufferType 192 | { 193 | public: 194 | void onOperationDone(int32_t customId); 195 | }; 196 | 197 | class FbExecAxisBufferContSyncType : virtual public FbExecAxisBufferContType 198 | { 199 | public: 200 | FB_INPUT AXIS_REF mMaster = nullptr; 201 | FB_INPUT AXIS_REF& mSlave = mAxis; 202 | FB_OUTPUT BOOL mStartSync = false; 203 | FB_OUTPUT BOOL& mInGear = mDone; 204 | FB_OUTPUT BOOL& mInSync = mDone; 205 | 206 | public: 207 | MC_ErrorCode onAxisExecPosedge(void); 208 | 209 | public: 210 | virtual MC_ErrorCode onMasterSlaveExecPosedge(void) = 0; 211 | }; 212 | 213 | #pragma pack(pop) 214 | 215 | } 216 | 217 | #endif /** _URANUS_FBPLCOPENBASE_HPP_ **/ 218 | 219 | -------------------------------------------------------------------------------- /fb/FbSingleAxis.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FbSingleAxis.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "FbSingleAxis.hpp" 26 | #include "Axis.hpp" 27 | 28 | #include 29 | 30 | namespace Uranus { 31 | 32 | void FbPower::call(void) 33 | { 34 | if(!mAxis) { 35 | onOperationError(MC_ERRORCODE_AXISNOTEXIST, 0); 36 | return; 37 | } 38 | 39 | MC_ErrorCode err; 40 | bool isDone; 41 | err = mAxis->setPower(mEnable, mEnablePositive, mEnableNegative, isDone); 42 | if(err) { 43 | onOperationError(err, 0); 44 | return; 45 | } 46 | 47 | mStatus = isDone? mEnable: false; 48 | mValid = isDone; 49 | clearError(); 50 | } 51 | 52 | void FbPower::onOperationError(MC_ErrorCode errorCode, int32_t customId) 53 | { 54 | mStatus = mValid = false; 55 | FbBaseType::onOperationError(errorCode, customId); 56 | } 57 | 58 | //////////////////////////////////////////////////////////// 59 | 60 | MC_ErrorCode FbHome::onAxisExecPosedge(void) 61 | { 62 | return mAxis->addHoming(this, mPosition, mBufferMode); 63 | } 64 | 65 | //////////////////////////////////////////////////////////// 66 | 67 | MC_ErrorCode FbStop::onAxisExecPosedge(void) 68 | { 69 | return mAxis->addStop(this, mDeceleration, mJerk); 70 | } 71 | 72 | void FbStop::onExecNegedge(void) 73 | { 74 | mAxis->cancelStopLater(); 75 | } 76 | 77 | //////////////////////////////////////////////////////////// 78 | 79 | MC_ErrorCode FbHalt::onAxisExecPosedge(void) 80 | { 81 | return mAxis->addHalt(this, mDeceleration, mJerk, mBufferMode); 82 | } 83 | 84 | //////////////////////////////////////////////////////////// 85 | 86 | MC_ErrorCode FbMoveAbsolute::onAxisExecPosedge(void) 87 | { 88 | return mAxis->addMovePos( 89 | this, 90 | mPosition, 91 | mVelocity, 92 | mAcceleration, 93 | mDeceleration, 94 | mJerk, 95 | MC_SHIFTINGMODE_ABSOLUTE, 96 | mDirection, 97 | mBufferMode); 98 | } 99 | 100 | //////////////////////////////////////////////////////////// 101 | 102 | MC_ErrorCode FbMoveRelative::onAxisExecPosedge(void) 103 | { 104 | return mAxis->addMovePos( 105 | this, 106 | mDistance, 107 | mVelocity, 108 | mAcceleration, 109 | mDeceleration, 110 | mJerk, 111 | MC_SHIFTINGMODE_RELATIVE, 112 | MC_DIRECTION_CURRENT, 113 | mBufferMode); 114 | } 115 | 116 | //////////////////////////////////////////////////////////// 117 | 118 | MC_ErrorCode FbMoveAdditive::onAxisExecPosedge(void) 119 | { 120 | return mAxis->addMovePos( 121 | this, 122 | mDistance, 123 | mVelocity, 124 | mAcceleration, 125 | mDeceleration, 126 | mJerk, 127 | MC_SHIFTINGMODE_ADDITIVE, 128 | MC_DIRECTION_CURRENT, 129 | mBufferMode); 130 | } 131 | 132 | //////////////////////////////////////////////////////////// 133 | 134 | MC_ErrorCode FbMoveVelocity::onAxisExecPosedge(void) 135 | { 136 | return mAxis->addMoveVel( 137 | this, 138 | mVelocity, 139 | mAcceleration, 140 | mDeceleration, 141 | mJerk, 142 | mBufferMode); 143 | } 144 | 145 | //////////////////////////////////////////////////////////// 146 | 147 | MC_ErrorCode FbReadStatus::onAxisEnable(bool& isDone) 148 | { 149 | onDisable(); 150 | 151 | switch(mAxis->status()) { 152 | case MC_AXISSTATUS_DISABLED: 153 | mDisabled = true; 154 | break; 155 | case MC_AXISSTATUS_STANDSTILL: 156 | mStandstill = true; 157 | break; 158 | case MC_AXISSTATUS_HOMING: 159 | mHoming = true; 160 | break; 161 | case MC_AXISSTATUS_DISCRETEMOTION: 162 | mDiscreteMotion = true; 163 | break; 164 | case MC_AXISSTATUS_CONTINUOUSMOTION: 165 | mContinuousMotion = true; 166 | break; 167 | case MC_AXISSTATUS_SYNCHRONIZEDMOTION: 168 | mSynchronizedMotion = true; 169 | break; 170 | case MC_AXISSTATUS_STOPPING: 171 | mStopping = true; 172 | break; 173 | case MC_AXISSTATUS_ERRORSTOP: 174 | mErrorStop = true; 175 | break; 176 | } 177 | 178 | isDone = true; 179 | 180 | return MC_ERRORCODE_GOOD; 181 | } 182 | 183 | void FbReadStatus::onDisable(void) 184 | { 185 | memset(&mErrorStop, 0, 186 | &mSynchronizedMotion - &mErrorStop + sizeof(mSynchronizedMotion)); 187 | } 188 | 189 | //////////////////////////////////////////////////////////// 190 | 191 | MC_ErrorCode FbReadMotionState::onAxisEnable(bool& isDone) 192 | { 193 | double acc, vel; 194 | switch(mSource) { 195 | case MC_SOURCE_SETVALUE: 196 | acc = mAxis->cmdAcceleration(); 197 | vel = mAxis->cmdVelocity(); 198 | break; 199 | 200 | case MC_SOURCE_ACTUALVALUE: 201 | acc = mAxis->actAcceleration(); 202 | vel = mAxis->actVelocity(); 203 | break; 204 | 205 | default: 206 | return MC_ERRORCODE_SOURCEILLEGAL; 207 | } 208 | 209 | onDisable(); 210 | 211 | if(vel > 0) { 212 | if(acc < 0) 213 | mDecelerating = true; 214 | else if(acc > 0) 215 | mAccelerating = true; 216 | else 217 | mConstantVelocity = true; 218 | 219 | mDirectionPositive = true; 220 | } else if(vel < 0) { 221 | if(acc > 0) 222 | mDecelerating = true; 223 | else if(acc < 0) 224 | mAccelerating = true; 225 | else 226 | mConstantVelocity = true; 227 | 228 | mDirectionNegative = true; 229 | } else { 230 | if(acc) 231 | mAccelerating = true; 232 | } 233 | 234 | isDone = true; 235 | 236 | return MC_ERRORCODE_GOOD; 237 | } 238 | 239 | void FbReadMotionState::onDisable(void) 240 | { 241 | memset(&mConstantVelocity, 0, 242 | &mDirectionNegative - &mConstantVelocity + sizeof(mDirectionNegative)); 243 | } 244 | 245 | //////////////////////////////////////////////////////////// 246 | 247 | void FbReadAxisError::call(void) 248 | { 249 | mEnable? onEnableTrue(): onEnableFalse(); 250 | } 251 | 252 | MC_ErrorCode FbReadAxisError::onEnableTrue(void) 253 | { 254 | if(!mAxis) 255 | return MC_ERRORCODE_AXISNOTEXIST; 256 | 257 | mValid = true; 258 | mError = false; 259 | mErrorID = mAxis->errorCode(); 260 | mAxisErrorID = mAxis->devErrorCode(); 261 | return MC_ERRORCODE_GOOD; 262 | } 263 | 264 | MC_ErrorCode FbReadAxisError::onEnableFalse(void) 265 | { 266 | mValid = false; 267 | mErrorID = MC_ERRORCODE_GOOD; 268 | mAxisErrorID = 0; 269 | return MC_ERRORCODE_GOOD; 270 | } 271 | 272 | //////////////////////////////////////////////////////////// 273 | 274 | MC_ErrorCode FbReset::onAxisTriggered(bool& isDone) 275 | { 276 | return mAxis->resetError(isDone); 277 | } 278 | 279 | //////////////////////////////////////////////////////////// 280 | 281 | MC_ErrorCode FbReadActualPosition::onAxisEnable(bool& isDone) 282 | { 283 | mPosition = mAxis->actPosition(); 284 | isDone = true; 285 | return MC_ERRORCODE_GOOD; 286 | } 287 | 288 | void FbReadActualPosition::onDisable(void) 289 | { 290 | mPosition = 0; 291 | } 292 | 293 | //////////////////////////////////////////////////////////// 294 | 295 | MC_ErrorCode FbReadCommandPosition::onAxisEnable(bool& isDone) 296 | { 297 | mPosition = mAxis->cmdPosition(); 298 | isDone = true; 299 | return MC_ERRORCODE_GOOD; 300 | } 301 | 302 | //////////////////////////////////////////////////////////// 303 | 304 | MC_ErrorCode FbReadActualVelocity::onAxisEnable(bool& isDone) 305 | { 306 | mVelocity = mAxis->actVelocity(); 307 | isDone = true; 308 | return MC_ERRORCODE_GOOD; 309 | } 310 | 311 | void FbReadActualVelocity::onDisable(void) 312 | { 313 | mVelocity = 0; 314 | } 315 | 316 | //////////////////////////////////////////////////////////// 317 | 318 | MC_ErrorCode FbReadCommandVelocity::onAxisEnable(bool& isDone) 319 | { 320 | mVelocity = mAxis->cmdVelocity(); 321 | isDone = true; 322 | return MC_ERRORCODE_GOOD; 323 | } 324 | 325 | //////////////////////////////////////////////////////////// 326 | 327 | MC_ErrorCode FbEmergencyStop::onAxisTriggered(bool& isDone) 328 | { 329 | mAxis->emergStop(MC_ERRORCODE_SOFTWAREEMGS); 330 | isDone = true; 331 | return MC_ERRORCODE_GOOD; 332 | } 333 | 334 | } 335 | 336 | -------------------------------------------------------------------------------- /fb/FbSingleAxis.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FbSingleAxis.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_FBSINGLEAXIS_HPP_ 26 | #define _URANUS_FBSINGLEAXIS_HPP_ 27 | 28 | #include "FbPLCOpenBase.hpp" 29 | 30 | namespace Uranus { 31 | 32 | #pragma pack(push) 33 | #pragma pack(4) 34 | 35 | class FbPower : public FbBaseType 36 | { 37 | public: 38 | FB_INPUT AXIS_REF mAxis; 39 | 40 | FB_INPUT BOOL mEnable = false; 41 | FB_INPUT BOOL mEnablePositive = false; 42 | FB_INPUT BOOL mEnableNegative = false; 43 | 44 | FB_OUTPUT BOOL mStatus = false; 45 | FB_OUTPUT BOOL mValid = false; 46 | 47 | public: 48 | void call(void); 49 | 50 | void onOperationError(MC_ErrorCode errorCode, int32_t customId); 51 | }; 52 | 53 | class FbHome : public FbExecAxisBufferType 54 | { 55 | public: 56 | FB_INPUT LREAL mPosition = 0; 57 | 58 | public: 59 | MC_ErrorCode onAxisExecPosedge(void); 60 | }; 61 | 62 | class FbStop : public FbExecAxisType 63 | { 64 | public: 65 | FB_INPUT LREAL mDeceleration = 0; 66 | FB_INPUT LREAL mJerk = 0; 67 | 68 | public: 69 | MC_ErrorCode onAxisExecPosedge(void); 70 | void onExecNegedge(void); 71 | }; 72 | 73 | class FbHalt : public FbExecAxisBufferType 74 | { 75 | public: 76 | FB_INPUT LREAL mDeceleration = 0; 77 | FB_INPUT LREAL mJerk = 0; 78 | 79 | public: 80 | MC_ErrorCode onAxisExecPosedge(void); 81 | }; 82 | 83 | class FbMoveAbsolute : public FbExecAxisBufferType 84 | { 85 | public: 86 | FB_INPUT LREAL mPosition = 0; 87 | FB_INPUT LREAL mVelocity = 0; 88 | FB_INPUT LREAL mAcceleration = 0; 89 | FB_INPUT LREAL mDeceleration = 0; 90 | FB_INPUT LREAL mJerk = 0; 91 | FB_INPUT MC_DIRECTION mDirection = MC_DIRECTION_CURRENT; 92 | 93 | public: 94 | MC_ErrorCode onAxisExecPosedge(void); 95 | }; 96 | 97 | class FbMoveRelative : public FbExecAxisBufferType 98 | { 99 | public: 100 | FB_INPUT LREAL mDistance = 0; 101 | FB_INPUT LREAL mVelocity = 0; 102 | FB_INPUT LREAL mAcceleration = 0; 103 | FB_INPUT LREAL mDeceleration = 0; 104 | FB_INPUT LREAL mJerk = 0; 105 | 106 | public: 107 | MC_ErrorCode onAxisExecPosedge(void); 108 | }; 109 | 110 | class FbMoveAdditive : public FbExecAxisBufferType 111 | { 112 | public: 113 | FB_INPUT LREAL mDistance = 0; 114 | FB_INPUT LREAL mVelocity = 0; 115 | FB_INPUT LREAL mAcceleration = 0; 116 | FB_INPUT LREAL mDeceleration = 0; 117 | FB_INPUT LREAL mJerk = 0; 118 | 119 | public: 120 | MC_ErrorCode onAxisExecPosedge(void); 121 | }; 122 | 123 | class FbMoveVelocity : public FbExecAxisBufferContType 124 | { 125 | public: 126 | FB_INPUT LREAL mVelocity = 0; 127 | FB_INPUT LREAL mAcceleration = 0; 128 | FB_INPUT LREAL mDeceleration = 0; 129 | FB_INPUT LREAL mJerk = 0; 130 | 131 | FB_OUTPUT BOOL& mInVelocity = mDone; 132 | 133 | public: 134 | MC_ErrorCode onAxisExecPosedge(void); 135 | }; 136 | 137 | class FbReadStatus : public FbReadInfoAxisType 138 | { 139 | public: 140 | FB_OUTPUT BOOL mErrorStop = false; 141 | FB_OUTPUT BOOL mDisabled = false; 142 | FB_OUTPUT BOOL mStopping = false; 143 | FB_OUTPUT BOOL mHoming = false; 144 | FB_OUTPUT BOOL mStandstill = false; 145 | FB_OUTPUT BOOL mDiscreteMotion = false; 146 | FB_OUTPUT BOOL mContinuousMotion = false; 147 | FB_OUTPUT BOOL mSynchronizedMotion = false; 148 | 149 | public: 150 | MC_ErrorCode onAxisEnable(bool& isDone); 151 | void onDisable(void); 152 | }; 153 | 154 | class FbReadMotionState : public FbReadInfoAxisType 155 | { 156 | public: 157 | FB_INPUT MC_SOURCE mSource = MC_SOURCE_SETVALUE; 158 | 159 | FB_OUTPUT BOOL mConstantVelocity = false; 160 | FB_OUTPUT BOOL mAccelerating = false; 161 | FB_OUTPUT BOOL mDecelerating = false; 162 | FB_OUTPUT BOOL mDirectionPositive = false; 163 | FB_OUTPUT BOOL mDirectionNegative = false; 164 | 165 | public: 166 | MC_ErrorCode onAxisEnable(bool& isDone); 167 | void onDisable(void); 168 | }; 169 | 170 | class FbReadAxisError : public FbEnableType 171 | { 172 | public: 173 | FB_INPUT AXIS_REF mAxis = nullptr; 174 | FB_OUTPUT BOOL mValid = false; 175 | FB_OUTPUT BOOL mBusy = false; 176 | FB_OUTPUT MC_SERVOERRORCODE mAxisErrorID = 0; 177 | 178 | public: 179 | void call(void); 180 | MC_ErrorCode onEnableTrue(void); 181 | MC_ErrorCode onEnableFalse(void); 182 | }; 183 | 184 | class FbReset : public FbWriteInfoAxisType 185 | { 186 | public: 187 | MC_ErrorCode onAxisTriggered(bool& isDone); 188 | }; 189 | 190 | typedef enum { 191 | MC_PARAMETER_COMMANDEDPOSITION = 1, 192 | MC_PARAMETER_SWLIMITPOS = 2, 193 | MC_PARAMETER_SWLIMITNEG = 3, 194 | MC_PARAMETER_ENABLELIMITPOS = 4, 195 | MC_PARAMETER_ENABLELIMITNEG = 5, 196 | MC_PARAMETER_ENABLEPOSLAGMONITORING = 6, 197 | MC_PARAMETER_MAXPOSITIONLAG = 7, 198 | MC_PARAMETER_MAXVELOCITYSYSTEM = 8, 199 | MC_PARAMETER_MAXVELOCITYAPPL = 9, 200 | MC_PARAMETER_ACTUALVELOCITY = 10, 201 | MC_PARAMETER_COMMANDEDVELOCITY = 11, 202 | MC_PARAMETER_MAXACCELERATIONSYSTEM = 12, 203 | MC_PARAMETER_MAXACCELERATIONAPPL = 13, 204 | MC_PARAMETER_MAXDECELERATIONSYSTEM = 14, 205 | MC_PARAMETER_MAXDECELERATIONAPPL = 15, 206 | MC_PARAMETER_MAXJERKSYSTEM = 16, 207 | MC_PARAMETER_MAXJERKAPPL = 17, 208 | }MC_Parameter; 209 | 210 | class FbReadActualPosition : public FbReadInfoAxisType 211 | { 212 | public: 213 | FB_OUTPUT LREAL mPosition = 0; 214 | 215 | public: 216 | MC_ErrorCode onAxisEnable(bool& isDone); 217 | void onDisable(void); 218 | }; 219 | 220 | class FbReadCommandPosition : public FbReadActualPosition 221 | { 222 | public: 223 | MC_ErrorCode onAxisEnable(bool& isDone); 224 | }; 225 | 226 | class FbReadActualVelocity : public FbReadInfoAxisType 227 | { 228 | public: 229 | FB_OUTPUT LREAL mVelocity = 0; 230 | 231 | public: 232 | MC_ErrorCode onAxisEnable(bool& isDone); 233 | void onDisable(void); 234 | }; 235 | 236 | class FbReadCommandVelocity : public FbReadActualVelocity 237 | { 238 | public: 239 | MC_ErrorCode onAxisEnable(bool& isDone); 240 | }; 241 | 242 | class FbEmergencyStop : public FbWriteInfoAxisType 243 | { 244 | public: 245 | MC_ErrorCode onAxisTriggered(bool& isDone); 246 | }; 247 | 248 | #pragma pack(pop) 249 | 250 | } 251 | 252 | #endif /** _URANUS_FBSINGLEAXIS_HPP_ **/ 253 | 254 | -------------------------------------------------------------------------------- /fb/FunctionBlock.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FunctionBlock.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "FunctionBlock.hpp" 26 | #include "Scheduler.hpp" 27 | 28 | namespace Uranus { 29 | 30 | } 31 | -------------------------------------------------------------------------------- /fb/FunctionBlock.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FunctionBlock.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_FUNCTIONBLOCK_HPP_ 26 | #define _URANUS_FUNCTIONBLOCK_HPP_ 27 | 28 | #include "Global.hpp" 29 | 30 | namespace Uranus { 31 | 32 | #pragma pack(push) 33 | #pragma pack(4) 34 | 35 | class FunctionBlock 36 | { 37 | public: 38 | virtual ~FunctionBlock() = default; 39 | 40 | virtual void onOperationActive(int32_t customId){} 41 | 42 | virtual void onOperationAborted(int32_t customId){} 43 | 44 | virtual void onOperationDone(int32_t customId){} 45 | 46 | virtual void onOperationError(MC_ErrorCode errorCode, int32_t customId){} 47 | }; 48 | 49 | #pragma pack(pop) 50 | 51 | } 52 | 53 | #endif /** _URANUS_FUNCTIONBLOCK_HPP_ **/ 54 | 55 | -------------------------------------------------------------------------------- /fb/PLCTypes.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PLCTypes.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_PLCTYPES_HPP_ 26 | #define _URANUS_PLCTYPES_HPP_ 27 | 28 | #include 29 | #include 30 | #include "Global.hpp" 31 | 32 | namespace Uranus { 33 | 34 | #pragma pack(push) 35 | #pragma pack(4) 36 | 37 | typedef bool BOOL; 38 | typedef uint8_t BYTE; 39 | typedef uint8_t USINT; 40 | typedef int8_t SINT; 41 | typedef uint16_t WORD; 42 | typedef uint16_t UINT; 43 | typedef int16_t INT; 44 | typedef uint16_t DWORD; 45 | typedef uint32_t UDINT; 46 | typedef int32_t DINT; 47 | typedef uint64_t LWORD; 48 | typedef uint64_t ULINT; 49 | typedef int64_t LINT; 50 | typedef float REAL; 51 | typedef double LREAL; 52 | typedef char* STRING; 53 | 54 | typedef MC_BufferMode MC_BUFFER_MODE; 55 | typedef MC_TransitionMode MC_TRANSITION_MODE; 56 | typedef MC_CoordSystem MC_COORD_SYSTEM; 57 | typedef MC_CircMode MC_CIRC_MODE; 58 | typedef MC_CircPath MC_CIRC_PATHCHOICE; 59 | typedef MC_Direction MC_DIRECTION; 60 | typedef MC_Source MC_SOURCE; 61 | typedef MC_ErrorCode MC_ERRORCODE; 62 | typedef MC_ServoErrorCode MC_SERVOERRORCODE; 63 | 64 | class Axis; 65 | typedef Axis* AXIS_REF; 66 | 67 | class AxesGroup; 68 | typedef AxesGroup* AXES_GROUP_REF; 69 | 70 | class CamTable; 71 | typedef std::shared_ptr MC_CAM_REF; 72 | 73 | #pragma pack(pop) 74 | 75 | } 76 | 77 | 78 | #endif /** _URANUS_PLCTYPES_HPP_ **/ 79 | -------------------------------------------------------------------------------- /misc/Event.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Event.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_EVENT_HPP_ 26 | #define _URANUS_EVENT_HPP_ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace Uranus { 33 | 34 | #ifdef URANUS_DEBUGMSG 35 | #define URANUS_MSG(...) printf(__VA_ARGS__) 36 | #else 37 | #define URANUS_MSG(...) 38 | #endif 39 | 40 | #define URANUS_DEFINE_EVENT(Event, ...) std::list Event; 41 | #define URANUS_ADD_HANDLER(Event, FuncPtr) Event.push_back(FuncPtr); 42 | #define URANUS_CALL_EVENT(Event, ...) for(auto& f : Event) (*f)(__VA_ARGS__); 43 | 44 | } 45 | 46 | #endif /** _URANUS_EVENT_HPP_ **/ 47 | -------------------------------------------------------------------------------- /misc/ExeclQueue.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ExeclQueue.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "ExeclQueue.hpp" 26 | #include "Queue.hpp" 27 | 28 | #include 29 | 30 | namespace Uranus { 31 | 32 | #define URANUS_AXISEXECLLISTSIZE 6 33 | #define URANUS_AXISEXECLNODESIZE 1024 34 | 35 | struct ExeclNodeContainer 36 | { 37 | ExeclNode* node = nullptr; 38 | uint8_t data[URANUS_AXISEXECLNODESIZE]; 39 | }; 40 | 41 | ExeclNode::ExeclNode() 42 | { 43 | } 44 | 45 | ExeclNode::~ExeclNode() 46 | { 47 | } 48 | 49 | class ExeclQueue::ExeclQueueImpl 50 | { 51 | public: 52 | ExeclQueue* mThis_ = nullptr;; 53 | Queue mQueue; 54 | ExeclNode* mHoldNode = nullptr; 55 | 56 | public: 57 | void processFrontNode(void); 58 | }; 59 | 60 | void ExeclQueue::ExeclQueueImpl::processFrontNode(void) 61 | { 62 | MC_ErrorCode err; 63 | ExeclNodeExecStat stat = EXECLNODEEXECSTAT_BUSY; 64 | ExeclNodeContainer* container = mQueue.front(); 65 | if(!container) 66 | return; 67 | 68 | ExeclNode* node = container->node; 69 | if(!node->mIsActived) { //第一次Active 70 | err = node->onActive(mThis_); 71 | if(err) goto ERROR; 72 | node->mIsActived = true; 73 | } 74 | 75 | err = node->onExecuting(mThis_, stat); 76 | if(err) goto ERROR; 77 | 78 | switch(stat) { 79 | case EXECLNODEEXECSTAT_DONE: 80 | case EXECLNODEEXECSTAT_FASTDONE: { 81 | bool isHold = false; 82 | node->onDone(mThis_, isHold); 83 | if(isHold) {//提交holdNode序列 84 | mHoldNode = node; 85 | } else { 86 | node->~ExeclNode(); 87 | } 88 | 89 | mQueue.pop_front(); 90 | break; 91 | } 92 | default: 93 | ; 94 | } 95 | 96 | if(stat == EXECLNODEEXECSTAT_FASTDONE) 97 | processFrontNode(); 98 | 99 | return; 100 | 101 | ERROR: 102 | mThis_->setAllNodesError(err); 103 | } 104 | 105 | ExeclQueue::ExeclQueue() 106 | { 107 | mImpl_ = new ExeclQueueImpl(); 108 | mImpl_->mThis_ = this; 109 | } 110 | 111 | ExeclQueue::~ExeclQueue() 112 | { 113 | delete mImpl_; 114 | } 115 | 116 | void ExeclQueue::processExeclNode(void) 117 | { 118 | MC_ErrorCode err; 119 | ExeclNodeExecStat stat = EXECLNODEEXECSTAT_BUSY; 120 | 121 | if(mImpl_->mHoldNode) { 122 | if(!mImpl_->mQueue.empty()) { //打断holdNode 123 | mImpl_->mHoldNode->onAborted(this); 124 | mImpl_->mHoldNode->~ExeclNode(); 125 | mImpl_->mHoldNode = nullptr; 126 | } else { 127 | err = mImpl_->mHoldNode->onExecuting(this, stat); 128 | if(err) { 129 | setAllNodesError(err); 130 | return; 131 | } 132 | } 133 | } 134 | 135 | mImpl_->processFrontNode(); 136 | 137 | return; 138 | } 139 | 140 | MC_ErrorCode ExeclQueue::pushAndNewData( 141 | const std::function& constructor, 142 | bool abortFlag) 143 | { 144 | if(abortFlag) 145 | setAllNodesAborted(); 146 | 147 | if(mImpl_->mQueue.push_back()) { 148 | ExeclNodeContainer* container = mImpl_->mQueue.back(); 149 | container->node = constructor(container->data); 150 | container->node->mContainer = container; 151 | return MC_ERRORCODE_GOOD; 152 | } else { 153 | return MC_ERRORCODE_QUEUEFULL; 154 | } 155 | } 156 | 157 | ExeclNode* ExeclQueue::front(void) const 158 | { 159 | ExeclNodeContainer* container = mImpl_->mQueue.front(); 160 | return container? container->node: nullptr; 161 | 162 | } 163 | 164 | ExeclNode* ExeclQueue::back(void) const 165 | { 166 | ExeclNodeContainer* container = mImpl_->mQueue.back(); 167 | return container? container->node: nullptr; 168 | } 169 | 170 | ExeclNode* ExeclQueue::next(ExeclNode* node) const 171 | { 172 | ExeclNodeContainer* container = node->mContainer; 173 | ExeclNodeContainer* containerNext = mImpl_->mQueue.next(container); 174 | 175 | return containerNext? containerNext->node: nullptr; 176 | } 177 | 178 | ExeclNode* ExeclQueue::prev(ExeclNode* node) const 179 | { 180 | ExeclNodeContainer* container = node->mContainer; 181 | ExeclNodeContainer* containerPrev = mImpl_->mQueue.prev(container); 182 | 183 | return containerPrev? containerPrev->node: nullptr; 184 | } 185 | 186 | bool ExeclQueue::busy(void) const 187 | { 188 | return !(mImpl_->mQueue.empty() && !mImpl_->mHoldNode); 189 | } 190 | 191 | size_t ExeclQueue::operationRemains(void) const 192 | { 193 | return mImpl_->mQueue.used(); 194 | } 195 | 196 | void ExeclQueue::setAllNodesAborted(void) 197 | { 198 | if(mImpl_->mHoldNode) { 199 | mImpl_->mHoldNode->onAborted(this); 200 | mImpl_->mHoldNode->~ExeclNode(); 201 | mImpl_->mHoldNode = nullptr; 202 | } 203 | 204 | ExeclNodeContainer* container; 205 | while((container = mImpl_->mQueue.front())) { 206 | ExeclNode* node = container->node; 207 | node->onAborted(this); 208 | node->~ExeclNode(); 209 | mImpl_->mQueue.pop_front(); 210 | } 211 | 212 | URANUS_CALL_EVENT(onAllNodesAborted, this); 213 | } 214 | 215 | void ExeclQueue::setAllNodesError(MC_ErrorCode errorCodeToSet) 216 | { 217 | if(mImpl_->mHoldNode) { 218 | mImpl_->mHoldNode->onError(this, errorCodeToSet); 219 | mImpl_->mHoldNode->~ExeclNode(); 220 | mImpl_->mHoldNode = nullptr; 221 | } 222 | 223 | ExeclNodeContainer* container; 224 | while((container = mImpl_->mQueue.front())) { 225 | ExeclNode* node = container->node; 226 | node->onError(this, errorCodeToSet); 227 | node->~ExeclNode(); 228 | mImpl_->mQueue.pop_front(); 229 | } 230 | 231 | URANUS_CALL_EVENT(onAllNodesError, this, errorCodeToSet); 232 | } 233 | 234 | } 235 | -------------------------------------------------------------------------------- /misc/ExeclQueue.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ExeclQueue.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_EXECLQUEUE_HPP_ 26 | #define _URANUS_EXECLQUEUE_HPP_ 27 | 28 | #include "Global.hpp" 29 | #include "Event.hpp" 30 | #include 31 | 32 | namespace Uranus { 33 | 34 | typedef enum { 35 | EXECLNODEEXECSTAT_BUSY = 0, 36 | EXECLNODEEXECSTAT_DONE = 1, 37 | EXECLNODEEXECSTAT_FASTDONE = 2, 38 | }ExeclNodeExecStat; 39 | 40 | struct ExeclNodeContainer; 41 | class ExeclQueue; 42 | class ExeclNode 43 | { 44 | public: 45 | ExeclNode(); 46 | virtual ~ExeclNode(); 47 | 48 | protected: 49 | virtual MC_ErrorCode onActive(ExeclQueue* queue) = 0; 50 | virtual MC_ErrorCode onExecuting(ExeclQueue* queue, ExeclNodeExecStat& stat) = 0; 51 | virtual void onAborted(ExeclQueue* queue) = 0; 52 | virtual void onDone(ExeclQueue* queue, bool& isHold) = 0; 53 | virtual void onError(ExeclQueue* queue, MC_ErrorCode errorCode) = 0; 54 | 55 | private: 56 | bool mIsActived = false; 57 | ExeclNodeContainer* mContainer = nullptr; 58 | friend class ExeclQueue; 59 | }; 60 | 61 | class ExeclQueue 62 | { 63 | public: 64 | ExeclQueue(); 65 | virtual ~ExeclQueue(); 66 | 67 | void processExeclNode(void); 68 | MC_ErrorCode pushAndNewData( 69 | const std::function& constructor, 70 | bool abortFlag); 71 | 72 | ExeclNode* front(void) const; 73 | ExeclNode* back(void) const; 74 | ExeclNode* next(ExeclNode* node) const; 75 | ExeclNode* prev(ExeclNode* node) const; 76 | bool busy(void) const; 77 | size_t operationRemains(void) const; 78 | void setAllNodesAborted(void); 79 | void setAllNodesError(MC_ErrorCode errorCodeToSet); 80 | 81 | protected: 82 | URANUS_DEFINE_EVENT(onAllNodesAborted, ExeclQueue*); 83 | URANUS_DEFINE_EVENT(onAllNodesError, ExeclQueue*, MC_ErrorCode); 84 | 85 | private: 86 | class ExeclQueueImpl; 87 | ExeclQueueImpl* mImpl_; 88 | }; 89 | 90 | } 91 | 92 | #endif /** _URANUS_EXECLQUEUE_HPP_ **/ 93 | -------------------------------------------------------------------------------- /misc/LinkList.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LinkList.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "LinkList.hpp" 26 | 27 | namespace Uranus { 28 | 29 | LinkNode::LinkNode() 30 | { 31 | mPrev = mNext = 0; 32 | } 33 | 34 | LinkNode::~LinkNode() 35 | { 36 | takeOut(); 37 | } 38 | 39 | void LinkNode::takeOut(void) 40 | { 41 | if(mNext) mNext->mPrev = mPrev; 42 | if(mPrev) mPrev->mNext = mNext; 43 | mNext = mPrev = 0; 44 | } 45 | 46 | void LinkNode::insertBack(LinkNode* one) 47 | { 48 | takeOut(); 49 | 50 | mNext = one->mNext; 51 | mPrev = one; 52 | 53 | if(mNext) mNext->mPrev = this; 54 | one->mNext = this; 55 | } 56 | 57 | void LinkNode::insertFront(LinkNode* one) 58 | { 59 | takeOut(); 60 | 61 | mPrev = one->mPrev; 62 | mNext = one; 63 | 64 | if(mPrev) mPrev->mNext = this; 65 | one->mPrev = this; 66 | } 67 | 68 | LinkNode* LinkNode::next(void) const 69 | { 70 | return mNext; 71 | } 72 | 73 | LinkNode* LinkNode::prev(void) const 74 | { 75 | return mPrev; 76 | } 77 | 78 | } 79 | -------------------------------------------------------------------------------- /misc/LinkList.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LinkList.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_LINKLIST_HPP_ 26 | #define _URANUS_LINKLIST_HPP_ 27 | 28 | namespace Uranus { 29 | 30 | class LinkNode 31 | { 32 | private: 33 | LinkNode* mPrev; 34 | LinkNode* mNext; 35 | 36 | public: 37 | LinkNode(); 38 | virtual ~LinkNode(); 39 | 40 | void takeOut(void); 41 | void insertBack(LinkNode* one); 42 | void insertFront(LinkNode* one); 43 | LinkNode* next(void) const; 44 | LinkNode* prev(void) const; 45 | }; 46 | 47 | } 48 | 49 | #endif /** _URANUS_LINKLIST_HPP_ **/ 50 | -------------------------------------------------------------------------------- /misc/Queue.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Queue.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_QUEUE_HPP_ 26 | #define _URANUS_QUEUE_HPP_ 27 | 28 | template 29 | class Queue 30 | { 31 | private: 32 | size_t mHead = 0; 33 | size_t mTail = 0; 34 | size_t mUsed = 0; 35 | size_t mSize; 36 | T queue[Size+1]; 37 | 38 | public: 39 | Queue(); 40 | ~Queue(); 41 | 42 | T* front(void) const; 43 | T* back(void) const; 44 | 45 | T* next(T*) const; 46 | T* prev(T*) const; 47 | 48 | bool pop_front(void); 49 | bool pop_back(void); 50 | 51 | bool push_back(void); 52 | 53 | void clear(void); 54 | bool empty(void) const; 55 | size_t used(void) const; 56 | }; 57 | 58 | #define INC(num, limit) \ 59 | if(num >= limit - 1) num = 0; \ 60 | else ++num; 61 | 62 | #define DEC(num, limit) \ 63 | if(num <= 0) num = limit - 1; \ 64 | else --num; 65 | 66 | template 67 | inline Queue::Queue() 68 | { 69 | mSize = Size + 1; 70 | } 71 | 72 | template 73 | inline Queue::~Queue() 74 | { 75 | } 76 | 77 | template 78 | inline T* Queue::front(void) const 79 | { 80 | if(mHead == mTail) 81 | return 0; 82 | 83 | T* tmp = (T*)queue + mHead; 84 | return tmp; 85 | } 86 | 87 | template 88 | inline T* Queue::back(void) const 89 | { 90 | if(mHead == mTail) 91 | return 0; 92 | 93 | size_t pos = mTail; 94 | DEC(pos, mSize); 95 | T* tmp = (T*)queue + pos; 96 | return tmp; 97 | } 98 | 99 | template 100 | inline T* Queue::next(T* one) const 101 | { 102 | size_t cur = one - queue; 103 | INC(cur, mSize); 104 | 105 | if((mHead < mTail && cur >= mHead && cur < mTail) || 106 | (mTail < mHead && (cur >= mHead || cur < mTail))) 107 | return (T*)queue + cur; 108 | else 109 | return 0; 110 | } 111 | 112 | template 113 | inline T* Queue::prev(T* one) const 114 | { 115 | size_t cur = one - queue; 116 | DEC(cur, mSize); 117 | 118 | if((mHead < mTail && cur >= mHead && cur < mTail) || 119 | (mTail < mHead && (cur >= mHead || cur < mTail))) 120 | return (T*)queue + cur; 121 | else 122 | return 0; 123 | } 124 | 125 | template 126 | inline bool Queue::pop_front(void) 127 | { 128 | if(mHead == mTail) 129 | return false; 130 | 131 | INC(mHead, mSize); 132 | --mUsed; 133 | return true; 134 | } 135 | 136 | template 137 | bool Queue::pop_back(void) 138 | { 139 | if(mHead == mTail) 140 | return false; 141 | 142 | DEC(mTail, mSize); 143 | --mUsed; 144 | return true; 145 | } 146 | 147 | template 148 | inline bool Queue::push_back(void) 149 | { 150 | if((mTail == (mHead - 1)) || ((mTail - mSize) == (mHead - 1))) 151 | return false; 152 | 153 | INC(mTail, mSize); 154 | ++mUsed; 155 | return true; 156 | } 157 | 158 | template 159 | inline void Queue::clear(void) 160 | { 161 | mHead = mTail; 162 | } 163 | 164 | template 165 | inline bool Queue::empty(void) const 166 | { 167 | bool result = (mTail == mHead); 168 | return result; 169 | } 170 | 171 | template 172 | size_t Queue::used(void) const 173 | { 174 | return mUsed; 175 | } 176 | 177 | #undef INC 178 | #undef DEC 179 | 180 | #endif /** _URANUS_QUEUE_HPP_ **/ 181 | -------------------------------------------------------------------------------- /motion/Global.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Global.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_MOTION_GLOBAL_HPP_ 26 | #define _URANUS_MOTION_GLOBAL_HPP_ 27 | 28 | #include 29 | #include 30 | 31 | namespace Uranus { 32 | 33 | #pragma pack(push) 34 | #pragma pack(4) 35 | 36 | #define URANUS_AXESGROUP_IDENT_NUM 8 37 | #define URANUS_CARTESIAN_DIMENSION3 3 38 | #define URANUS_CARTESIAN_DIMENSION6 6 39 | #define URANUS_TRANSITIONPARAMETER_NUM 4 40 | 41 | typedef uint32_t MC_ServoErrorCode; 42 | 43 | typedef enum { 44 | MC_SERVOCONTROLMODE_POSITION = 0, 45 | MC_SERVOCONTROLMODE_VELOCITY = 1, 46 | MC_SERVOCONTROLMODE_TORQUE = 2, 47 | }MC_ServoControlMode; 48 | 49 | typedef enum 50 | { 51 | MC_ERRORCODE_GOOD = 0x0, //成功 52 | 53 | MC_ERRORCODE_QUEUEFULL = 0x1, //轴队列已满 54 | MC_ERRORCODE_AXISENCODEROVERFLOW = 0x2, //轴编码器溢出 55 | MC_ERRORCODE_AXISPOWEROFF = 0x3, //轴未使能 56 | MC_ERRORCODE_AXISPOWERON = 0x4, //轴已功能 57 | MC_ERRORCODE_FREQUENCYILLEGAL = 0x5, //频率不合法 58 | MC_ERRORCODE_AXISNOTEXIST = 0x8, //轴ID号不存在 59 | MC_ERRORCODE_AXISBUSY = 0xA, //轴正忙，有功能块正在控制轴运动 60 | MC_ERRORCODE_FAILEDTOBUFFER = 0xF, //不支持以buffer形式添加 61 | MC_ERRORCODE_BLENDINGMODEILLEGAL = 0x10, //BufferMode值非法 62 | MC_ERRORCODE_PARAMETERNOTSUPPORT = 0x14, //不支持该参数号 63 | MC_ERRORCODE_OVERRIDEILLEGAL = 0x17, //OVERRIDE值非法 64 | MC_ERRORCODE_SHIFTINGMODEILLEGAL = 0x19, //移动模式非法 65 | MC_ERRORCODE_SOURCEILLEGAL = 0x1A, //获取源非法 66 | MC_ERRORCODE_CONTROLMODEILLEGAL = 0x23, //控制模式设置错误 67 | 68 | MC_ERRORCODE_POSILLEGAL = 0x100, //位置不合法 69 | MC_ERRORCODE_ACCILLEGAL = 0x101, //加/减速度不合法 70 | MC_ERRORCODE_VELILLEGAL = 0x102, //速度不合法 71 | MC_ERRORCODE_AXISHARDWARE = 0x103, //硬件错误 72 | MC_ERRORCODE_VELLIMITTOOLOW = 0x104, //由于配置文件限制，无法到达跟随的速度（电子齿轮，凸轮中） 73 | MC_ERRORCODE_ENDVELCANNOTREACH = 0x105, //实际终速度过高无法到达预设速度 74 | 75 | MC_ERRORCODE_CMDPPOSOVERLIMIT = 0x106, //指令位置超出配置文件正向限制 76 | MC_ERRORCODE_CMDNPOSOVERLIMIT = 0x107, //指令位置超出配置文件负向限制 77 | MC_ERRORCODE_FORBIDDENPPOSMOVE = 0x108, //禁止正向移动 78 | MC_ERRORCODE_FORBIDDENNPOSMOVE = 0x109, //禁止负向移动 79 | 80 | MC_ERRORCODE_POSLAGOVERLIMIT = 0x10A, //轴跟随误差超限 81 | MC_ERRORCODE_CMDVELOVERLIMIT = 0x10B, //轴指令速度超出限制 82 | MC_ERRORCODE_CMDACCOVERLIMIT = 0x10C, //轴指令加速度超出限制 83 | MC_ERRORCODE_POSINFINITY = 0x10E, //轴设定位置不合法 84 | 85 | MC_ERRORCODE_SOFTWAREEMGS = 0x1EE, //用户急停 86 | MC_ERRORCODE_SYSTEMEMGS = 0x1EF, //系统急停 87 | MC_ERRORCODE_COMMUNICATION = 0x1F0, //硬件通信异常 88 | 89 | /** 配置错误**/ 90 | MC_ERRORCODE_CFGAXISIDILLEGAL = 0x201, 91 | MC_ERRORCODE_CFGUNITRATIOOUTOFRANGE = 0x202, 92 | MC_ERRORCODE_CFGCONTROLMODEILLEGAL = 0x203, 93 | MC_ERRORCODE_CFGVELLIMITILLEGAL = 0x204, 94 | MC_ERRORCODE_CFGACCLIMITILLEGAL = 0x205, 95 | MC_ERRORCODE_CFGPOSLAGILLEGAL = 0x206, 96 | MC_ERRORCODE_CFGPKPILLEGAL = 0x207, 97 | MC_ERRORCODE_CFGFEEDFORWORDILLEGAL = 0x208, 98 | MC_ERRORCODE_CFGMODULOILLEGAL = 0x209, 99 | 100 | MC_ERRORCODE_HOMINGVELILLEGAL = 0x210, 101 | MC_ERRORCODE_HOMINGACCILLEGAL = 0x211, 102 | MC_ERRORCODE_HOMINGSIGILLEGAL = 0x212, 103 | MC_ERRORCODE_HOMINGMODEILLEGAL = 0x214, 104 | MC_ERRORCODE_HOMEPOSITIONILLEGAL = 0x215, 105 | 106 | MC_ERRORCODE_AXISDISABLED = 0x500, 107 | MC_ERRORCODE_AXISSTANDSTILL = 0x501, 108 | MC_ERRORCODE_AXISHOMING = 0x502, 109 | MC_ERRORCODE_AXISDISCRETEMOTION = 0x503, 110 | MC_ERRORCODE_AXISCONTINUOUSMOTION = 0x504, 111 | MC_ERRORCODE_AXISSYNCHRONIZEDMOTION = 0x505, 112 | MC_ERRORCODE_AXISSTOPPING = 0x506, 113 | MC_ERRORCODE_AXISERRORSTOP = 0x507, 114 | }MC_ErrorCode; 115 | 116 | typedef enum 117 | { 118 | MC_AXISSTATUS_DISABLED = 0, 119 | MC_AXISSTATUS_STANDSTILL = 1, 120 | MC_AXISSTATUS_HOMING = 2, 121 | MC_AXISSTATUS_DISCRETEMOTION = 3, 122 | MC_AXISSTATUS_CONTINUOUSMOTION = 4, 123 | MC_AXISSTATUS_SYNCHRONIZEDMOTION = 5, 124 | MC_AXISSTATUS_STOPPING = 6, 125 | MC_AXISSTATUS_ERRORSTOP = 7, 126 | }MC_AxisStatus; 127 | 128 | typedef enum 129 | { 130 | MC_GROUPSTATUS_DISABLED = 0, 131 | MC_GROUPSTATUS_STANDBY = 1, 132 | MC_GROUPSTATUS_HOMING = 2, 133 | MC_GROUPSTATUS_MOVING = 3, 134 | MC_GROUPSTATUS_STOPPING = 4, 135 | MC_GROUPSTATUS_ERRORSTOP = 5, 136 | }MC_GroupStatus; 137 | 138 | typedef enum 139 | { 140 | MC_MOTIONSTATE_INPOSITION = 0, 141 | MC_MOTIONSTATE_CONSTANTVELOCITY = 1, 142 | MC_MOTIONSTATE_ACCELERATING = 2, 143 | MC_MOTIONSTATE_DECELERATING = 3, 144 | }MC_MotionState; 145 | 146 | typedef enum 147 | { 148 | MC_BUFFERMODE_ABORTING = 0, 149 | MC_BUFFERMODE_BUFFERED = 1, 150 | MC_BUFFERMODE_BLENDINGLOW = 2, 151 | MC_BUFFERMODE_BLENDINGPREVIOUS = 3, 152 | MC_BUFFERMODE_BLENDINGNEXT = 4, 153 | MC_BUFFERMODE_BLENDINGHIGH = 5, 154 | MC_BUFFERMODE_BLENDINGCNC = 128, 155 | }MC_BufferMode; 156 | 157 | typedef enum 158 | { 159 | MC_DIRECTION_POSITIVE = 1, 160 | MC_DIRECTION_SHORTESTWAY = 2, 161 | MC_DIRECTION_NEGATIVE = 3, 162 | MC_DIRECTION_CURRENT = 4, 163 | }MC_Direction; 164 | 165 | typedef enum 166 | { 167 | MC_SOURCE_SETVALUE = 0, 168 | MC_SOURCE_ACTUALVALUE = 1, 169 | }MC_Source; 170 | 171 | typedef enum 172 | { 173 | MC_SHIFTINGMODE_ABSOLUTE = 0, 174 | MC_SHIFTINGMODE_RELATIVE = 1, 175 | MC_SHIFTINGMODE_ADDITIVE = 2, 176 | }MC_ShiftingMode; 177 | 178 | typedef enum 179 | { 180 | MC_HOMINGMODE_DIRECT = 1000, //直接以当前位置作为零点 181 | 182 | MC_HOMINGMODE_MODE5 = 1005, //负向移动寻找回零开关，触发后正向移动离开回零开关，最终停留在刚离开回零开关处，回零开关为上升沿触发 183 | MC_HOMINGMODE_MODE6 = 1006, //负向移动寻找回零开关，触发后正向移动离开回零开关，最终停留在刚离开回零开关处，回零开关为下降沿触发 184 | MC_HOMINGMODE_MODE7 = 1007, //正向移动寻找回零开关，触发后负向移动离开回零开关，最终停留在刚离开回零开关处，回零开关为上升沿触发 185 | MC_HOMINGMODE_MODE8 = 1008, //正向移动寻找回零开关，触发后负向移动离开回零开关，最终停留在刚离开回零开关处，回零开关为下降沿触发 186 | }MC_HomingMode; 187 | 188 | typedef enum 189 | { 190 | MC_TOUCHPROBESTATUS_NOTEXIST = 0, 191 | MC_TOUCHPROBESTATUS_RESETTING = 1, 192 | MC_TOUCHPROBESTATUS_RESETED = 2, 193 | MC_TOUCHPROBESTATUS_TIGGERING = 3, 194 | MC_TOUCHPROBESTATUS_TIGGERED = 4, 195 | }MC_TouchProbeStatus; 196 | 197 | typedef enum 198 | { 199 | MC_CONTROLMODE_POSOPENLOOP = 0, 200 | MC_CONTROLMODE_VELCLOSELOOP = 1, 201 | MC_CONTROLMODE_VELOPENLOOP = 2, 202 | }MC_ControlMode; 203 | 204 | typedef enum 205 | { 206 | MC_COORDSYSTEM_ACS = 0, 207 | MC_COORDSYSTEM_MCS = 1, 208 | MC_COORDSYSTEM_PCS = 2, 209 | }MC_CoordSystem; 210 | 211 | typedef enum 212 | { 213 | MC_CIRCMODE_BORDER = 0, 214 | MC_CIRCMODE_CENTER = 1, 215 | MC_CIRCMODE_RADIUS = 2, 216 | MC_CIRCMODE_VECTOR = 1000, 217 | }MC_CircMode; 218 | 219 | typedef enum 220 | { 221 | MC_CIRCPATH_CLOCKWISE = 0, 222 | MC_CIRCPATH_COUNTERCLOCKWISE = 1, 223 | }MC_CircPath; 224 | 225 | typedef enum 226 | { 227 | MC_TRANSITIONMODE_NONE = 0, 228 | MC_TRANSITIONMODE_STARTVELOCITY = 1, //不支持 229 | MC_TRANSITIONMODE_CONSTANTVELOCITY = 2, //不支持 230 | MC_TRANSITIONMODE_CORNERDISTANCE = 3, 231 | MC_TRANSITIONMODE_MAXCORNERDEVIATION = 4, 232 | }MC_TransitionMode; 233 | 234 | typedef enum 235 | { 236 | MC_LOGLEVEL_ERROR = 0, 237 | MC_LOGLEVEL_WARN = 1, 238 | MC_LOGLEVEL_INFO = 2, 239 | MC_LOGLEVEL_DEBUG = 3, 240 | }MC_LogLevel; 241 | 242 | ////////////////////////////////////////////////////////////// 243 | 244 | struct AxisMetricInfo 245 | { 246 | double mDevUnitRatio = 8192; //设备编码器单位比率 247 | double mModulo = 0; //模数值 248 | }; 249 | 250 | struct AxisRangeLimitInfo 251 | { 252 | bool mSwLimitPositive = false; //正向限位启用标志位 253 | bool mSwLimitNegative = false; //负向限位启用标志位 254 | double mLimitPositive = 0; //正向限位位置 255 | double mLimitNegative = 0; //负向限位位置 256 | }; 257 | 258 | struct AxisMotionLimitInfo 259 | { 260 | double mVelLimit = 1000; //速度限制 261 | double mAccLimit = 5000; //加速度限制 262 | double mPosLagLimit = 150; //跟随误差限制 263 | }; 264 | 265 | struct AxisControlInfo 266 | { 267 | MC_ControlMode mControlMode = MC_CONTROLMODE_POSOPENLOOP; //控制模式 268 | double mPKp = 10; //闭环位置Kp 269 | double mFF = 0; //位置前馈 270 | }; 271 | 272 | struct AxisHomingInfo 273 | { 274 | uint8_t* mHomingSig = 0; //回零信号地址 275 | uint8_t mHomingSigBitOffset = 0; //回零信号偏移 276 | MC_HomingMode mHomingMode = MC_HOMINGMODE_DIRECT; //回零模式 277 | double mHomingVelSearch = 0; //寻找回零信号速度 278 | double mHomingVelRegression = 0; //返回零位速度 279 | double mHomingAcc = 0; //回零加速度 280 | double mHomingJerk = 0; //回零加加速 281 | }; 282 | 283 | struct AxisConfig 284 | { 285 | AxisMetricInfo mMetricInfo; 286 | AxisRangeLimitInfo mRangeLimitInfo; 287 | AxisMotionLimitInfo mMotionLimitInfo; 288 | AxisControlInfo mControlInfo; 289 | AxisHomingInfo mHomingInfo; 290 | }; 291 | 292 | ////////////////////////////////////////////////////////////// 293 | /* 294 | struct GroupMotionLimitInfo 295 | { 296 | double mLinVelLimit = 1000; //线速度限制 297 | double mLinAccLimit = 5000; //线加速度限制 298 | double mAngVelLimit = 3600; //角速度限制 299 | double mAngAccLimit = 18000; //角加速度限制 300 | };*/ 301 | 302 | struct GroupConfig 303 | { 304 | // GroupMotionLimitInfo mMotionLimitInfo; 305 | }; 306 | 307 | #pragma pack(pop) 308 | 309 | } 310 | 311 | #endif /** _URANUS_MOTION_GLOBAL_HPP_ **/ 312 | -------------------------------------------------------------------------------- /motion/Scheduler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Scheduler.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "Scheduler.hpp" 26 | #include "Axis.hpp" 27 | 28 | namespace Uranus { 29 | 30 | class Scheduler::SchedulerImpl 31 | { 32 | public: 33 | Axis mAxisHead; 34 | double mFreq = 1000.0; 35 | uint32_t mTick = 0; 36 | }; 37 | 38 | Scheduler::Scheduler() 39 | { 40 | mImpl_ = new SchedulerImpl(); 41 | } 42 | 43 | Scheduler::~Scheduler() 44 | { 45 | delete mImpl_; 46 | } 47 | 48 | void Scheduler::runCycle(void) 49 | { 50 | Axis* axis = axisListFirst(); 51 | while(axis) { 52 | axis->runCycle(); 53 | axis = axisListNext(axis); 54 | } 55 | 56 | ++mImpl_->mTick; 57 | } 58 | 59 | MC_ErrorCode Scheduler::setFrequency(double frequency) 60 | { 61 | if(frequency <= 0) 62 | return MC_ERRORCODE_FREQUENCYILLEGAL; 63 | 64 | if(axisListFirst()) 65 | return MC_ERRORCODE_AXISBUSY; 66 | 67 | mImpl_->mFreq = frequency; 68 | 69 | return MC_ERRORCODE_GOOD; 70 | } 71 | 72 | double Scheduler::frequency(void) const 73 | { 74 | return mImpl_->mFreq; 75 | } 76 | 77 | uint32_t Scheduler::tick(void) const 78 | { 79 | return mImpl_->mTick; 80 | } 81 | 82 | Axis* Scheduler::newAxis(int32_t axisId, Servo* servo) 83 | { 84 | if(axis(axisId)) 85 | return nullptr; 86 | 87 | Axis* newAxis = new Axis(); 88 | if(!servo) 89 | servo = new Servo(); 90 | 91 | newAxis->setServo(servo); 92 | newAxis->mSched = this; 93 | newAxis->mAxisId = axisId; 94 | newAxis->insertBack(&mImpl_->mAxisHead); 95 | 96 | return newAxis; 97 | } 98 | 99 | Axis* Scheduler::axis(int32_t axisId) const 100 | { 101 | Axis* axis = axisListFirst(); 102 | while(axis) { 103 | if(axis->mAxisId == axisId) 104 | return axis; 105 | axis = axisListNext(axis); 106 | } 107 | 108 | return nullptr; 109 | } 110 | 111 | MC_ErrorCode Scheduler::setAxisConfig( 112 | Axis* axis, const AxisConfig& config) 113 | { 114 | MC_ErrorCode err; 115 | err = axis->setControlInfo(config.mControlInfo); 116 | if(err) return err; 117 | err = axis->setMetricInfo(config.mMetricInfo); 118 | if(err) return err; 119 | err = axis->setMotionLimitInfo(config.mMotionLimitInfo); 120 | if(err) return err; 121 | err = axis->setRangeLimitInfo(config.mRangeLimitInfo); 122 | if(err) return err; 123 | err = axis->setHomingInfo(config.mHomingInfo); 124 | return err; 125 | } 126 | 127 | MC_ErrorCode Scheduler::setAxisHomePosition(Axis* axis, double homePos) 128 | { 129 | return axis->setHomePosition(homePos); 130 | } 131 | 132 | Axis* Scheduler::axisListFirst(void) const 133 | { 134 | return dynamic_cast(mImpl_->mAxisHead.LinkNode::next()); 135 | } 136 | 137 | Axis* Scheduler::axisListNext(const Axis* one) const 138 | { 139 | return dynamic_cast(one->LinkNode::next()); 140 | } 141 | 142 | void Scheduler::release(void) 143 | { 144 | LinkNode* node; 145 | while((node = axisListFirst())) { 146 | node->takeOut(); 147 | delete node; 148 | } 149 | } 150 | 151 | } 152 | -------------------------------------------------------------------------------- /motion/Scheduler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Scheduler.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_SCHEDULER_HPP_ 26 | #define _URANUS_SCHEDULER_HPP_ 27 | 28 | #include "Global.hpp" 29 | #include "Servo.hpp" 30 | 31 | namespace Uranus { 32 | 33 | #pragma pack(push) 34 | #pragma pack(4) 35 | 36 | class Axis; 37 | class Scheduler 38 | { 39 | public: 40 | Scheduler(); 41 | 42 | virtual ~Scheduler(); 43 | 44 | //执行一次插补 45 | void runCycle(void); 46 | 47 | //设定runCycle的频率 48 | MC_ErrorCode setFrequency(double frequency); 49 | 50 | //获取频率 51 | double frequency(void) const; 52 | 53 | //当前tick，每次runCycle后自增 54 | uint32_t tick(void) const; 55 | 56 | /* 57 | * 新建轴 58 | * axisId:轴Id，不重复 59 | * servo:伺服驱动器实例 60 | * 返回:轴实例 61 | */ 62 | Axis* newAxis(int32_t axisId, Servo* servo); 63 | 64 | //通过Id获取轴 65 | Axis* axis(int32_t axisId) const; 66 | 67 | //设定轴配置 68 | MC_ErrorCode setAxisConfig(Axis* axis, const AxisConfig& config); 69 | 70 | //直接设定轴零点配置 71 | MC_ErrorCode setAxisHomePosition(Axis* axis, double homePos); 72 | 73 | //获取第一个轴 74 | Axis* axisListFirst(void) const; 75 | 76 | //获取下一个轴 77 | Axis* axisListNext(const Axis* one) const; 78 | 79 | //释放所有创建的轴 80 | void release(void); 81 | 82 | protected: 83 | virtual void vprintLog(MC_LogLevel level, const char* fmt, va_list ap) { } 84 | 85 | private: 86 | class SchedulerImpl; 87 | SchedulerImpl* mImpl_; 88 | friend class Axis; 89 | }; 90 | 91 | #pragma pack(pop) 92 | 93 | } 94 | 95 | #endif /** _URANUS_SCHEDULER_HPP_ **/ 96 | 97 | -------------------------------------------------------------------------------- /motion/Servo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Servo.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "Servo.hpp" 26 | #include "Scheduler.hpp" 27 | 28 | namespace Uranus { 29 | 30 | class Servo::ServoImpl 31 | { 32 | public: 33 | int32_t mSubmitPos = 0; 34 | int32_t mPos = 0; 35 | double mVel = 0; 36 | double mAcc = 0; 37 | }; 38 | 39 | Servo::Servo() 40 | { 41 | mImpl_ = new ServoImpl(); 42 | } 43 | 44 | Servo::~Servo() 45 | { 46 | delete mImpl_; 47 | } 48 | 49 | MC_ServoErrorCode Servo::setPower(bool powerStatus, bool& isDone) 50 | { 51 | isDone = true; 52 | return 0; 53 | } 54 | 55 | MC_ServoErrorCode Servo::setPos(int32_t pos) 56 | { 57 | mImpl_->mSubmitPos = pos; 58 | return 0; 59 | } 60 | 61 | MC_ServoErrorCode Servo::setVel(int32_t vel) 62 | { 63 | return 0xFFFFFFFF; 64 | } 65 | 66 | MC_ServoErrorCode Servo::setTorque(double torque) 67 | { 68 | return 0xFFFFFFFF; 69 | } 70 | 71 | int32_t Servo::pos(void) 72 | { 73 | return mImpl_->mPos; 74 | } 75 | 76 | int32_t Servo::vel(void) 77 | { 78 | return mImpl_->mVel; 79 | } 80 | 81 | int32_t Servo::acc(void) 82 | { 83 | return mImpl_->mAcc; 84 | } 85 | 86 | double Servo::torque(void) 87 | { 88 | return 0; 89 | } 90 | 91 | bool Servo::readVal(int index, double& value) 92 | { 93 | return false; 94 | } 95 | 96 | bool Servo::writeVal(int index, double value) 97 | { 98 | return false; 99 | } 100 | 101 | MC_ServoErrorCode Servo::resetError(bool& isDone) 102 | { 103 | isDone = true; 104 | return 0; 105 | } 106 | 107 | void Servo::runCycle(double freq) 108 | { 109 | int32_t posDiff = mImpl_->mSubmitPos - mImpl_->mPos; 110 | double curVel = posDiff * freq; 111 | mImpl_->mAcc = (curVel - mImpl_->mVel) * freq; 112 | mImpl_->mVel = curVel; 113 | mImpl_->mPos = mImpl_->mSubmitPos; 114 | } 115 | 116 | void Servo::emergStop(void) 117 | { 118 | mImpl_->mPos = mImpl_->mSubmitPos; 119 | mImpl_->mVel = mImpl_->mAcc = 0; 120 | } 121 | 122 | } 123 | -------------------------------------------------------------------------------- /motion/Servo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Servo.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_SERVO_HPP_ 26 | #define _URANUS_SERVO_HPP_ 27 | 28 | #include "Global.hpp" 29 | #include 30 | 31 | namespace Uranus { 32 | 33 | #pragma pack(push) 34 | #pragma pack(4) 35 | 36 | class Servo 37 | { 38 | public: 39 | Servo(); 40 | virtual ~Servo(); 41 | 42 | virtual MC_ServoErrorCode setPower(bool powerStatus, bool& isDone); 43 | virtual MC_ServoErrorCode setPos(int32_t pos); 44 | virtual MC_ServoErrorCode setVel(int32_t vel); 45 | virtual MC_ServoErrorCode setTorque(double torque); 46 | virtual int32_t pos(void); 47 | virtual int32_t vel(void); 48 | virtual int32_t acc(void); 49 | virtual double torque(void); 50 | virtual bool readVal(int index, double& value); 51 | virtual bool writeVal(int index, double value); 52 | virtual MC_ServoErrorCode resetError(bool& isDone); 53 | virtual void runCycle(double freq); 54 | virtual void emergStop(void); 55 | 56 | private: 57 | class ServoImpl; 58 | ServoImpl* mImpl_; 59 | }; 60 | 61 | #pragma pack(pop) 62 | 63 | } 64 | 65 | #endif /** _URANUS_SERVO_HPP_ **/ 66 | -------------------------------------------------------------------------------- /motion/axis/Axis.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Axis.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "Axis.hpp" 26 | #include "Scheduler.hpp" 27 | #include 28 | #include 29 | 30 | namespace Uranus { 31 | 32 | Axis::Axis() 33 | { 34 | } 35 | 36 | Axis::~Axis() 37 | { 38 | } 39 | 40 | double Axis::frequency(void) 41 | { 42 | return mSched->frequency(); 43 | } 44 | 45 | uint32_t Axis::tick(void) 46 | { 47 | return mSched->tick(); 48 | } 49 | 50 | void Axis::vprintLog(MC_LogLevel level, const char* fmt, va_list ap) 51 | { 52 | size_t size = strlen(fmt) + 32; 53 | char fmtAxis[size]; 54 | snprintf(fmtAxis, size, "Axis %d: %s", axisId(), fmt); 55 | mSched->vprintLog(level, fmtAxis, ap); 56 | } 57 | 58 | int32_t Axis::axisId(void) 59 | { 60 | return mAxisId; 61 | } 62 | 63 | } 64 | -------------------------------------------------------------------------------- /motion/axis/Axis.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Axis.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_AXIS_HPP_ 26 | #define _URANUS_AXIS_HPP_ 27 | 28 | #include "AxisMotion.hpp" 29 | 30 | namespace Uranus { 31 | 32 | class Scheduler; 33 | class Axis : public AxisMotion 34 | { 35 | public: 36 | Axis(); 37 | virtual ~Axis(); 38 | 39 | int32_t axisId(void); 40 | 41 | private: 42 | double frequency(void) override final; 43 | uint32_t tick(void) override final; 44 | void vprintLog(MC_LogLevel level, const char* fmt, va_list ap) override final; 45 | 46 | private: 47 | Scheduler* mSched = nullptr; 48 | int32_t mAxisId = 0; 49 | friend class Scheduler; 50 | }; 51 | 52 | } 53 | 54 | #endif /** _URANUS_AXIS_HPP_ **/ 55 | -------------------------------------------------------------------------------- /motion/axis/AxisBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisBase.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "AxisBase.hpp" 26 | #include "Servo.hpp" 27 | #include "MathUtils.hpp" 28 | #include "Event.hpp" 29 | 30 | #include 31 | 32 | namespace Uranus { 33 | 34 | #define URANUS_AXISNAMESIZE 64 35 | 36 | class AxisBase::AxisBaseImpl 37 | { 38 | public: 39 | AxisBase* mThis_; 40 | 41 | Servo* mServo = nullptr; 42 | 43 | char mAxisName[URANUS_AXISNAMESIZE] = "Axis"; 44 | AxisMetricInfo mMetric; 45 | AxisRangeLimitInfo mRangeLimit; 46 | AxisMotionLimitInfo mMotionLimit; 47 | AxisControlInfo mControl; 48 | double mHomePos = 0; 49 | 50 | MC_ErrorCode mErrorCode = MC_ERRORCODE_GOOD; 51 | MC_ServoErrorCode mDevErrorCode = 0; 52 | bool mNeedReset = false; 53 | 54 | bool mPowerStatus = false; 55 | bool mPowerStatusValid = false; 56 | 57 | double mSubmitCmdPos = 0; 58 | double mCmdPos = 0; 59 | double mCmdVel = 0; 60 | double mCmdAcc = 0; 61 | 62 | double mCalVel = 0; 63 | 64 | bool mEnablePositive = false; 65 | bool mEnableNegative = false; 66 | 67 | double mEncoderOverflowOffset = 0; 68 | 69 | public: 70 | void processPositionLoop(void); 71 | void servoStatusMaintains(void); 72 | void updateCmdPosToDev(void); 73 | 74 | int32_t toDevRaw(double x) const; 75 | double toSystemLogic(double x) const; 76 | 77 | double moduloToLinear(double basePos, double targetPos, MC_Direction dir) const; 78 | double linearToModulo(double pos) const; 79 | double packHomeOffset(double pos) const; 80 | double stripHomeOffset(double basePos, double pos) const; 81 | }; 82 | 83 | void AxisBase::AxisBaseImpl::processPositionLoop(void) 84 | { 85 | if(mThis_->errorCode()) 86 | return; 87 | 88 | if(!mThis_->powerStatus()) 89 | return; 90 | 91 | double calVel = (mSubmitCmdPos - mCmdPos) * mThis_->frequency(); 92 | 93 | if(calVel > 0 && !mEnablePositive) { 94 | mThis_->emergStop(MC_ERRORCODE_FORBIDDENPPOSMOVE); 95 | return; 96 | } else if(calVel < 0 && !mEnableNegative) { 97 | mThis_->emergStop(MC_ERRORCODE_FORBIDDENNPOSMOVE); 98 | return; 99 | } 100 | 101 | //printf("Axis error %lf %lf %lf %lf\n", mSubmitCmdPos, mCmdPos, calVel, mMotionLimit.mVelLimit); 102 | if(__isgt(fabs(calVel), mMotionLimit.mVelLimit)) { 103 | mThis_->emergStop(MC_ERRORCODE_CMDVELOVERLIMIT); 104 | return; 105 | } 106 | /* 107 | double calAcc = (calVel - mCalVel) * mThis_->frequency(); 108 | if(__isgt(fabs(calAcc), mMotionLimit.mAccLimit)) { 109 | mThis_->emergStop(MC_ERRORCODE_CMDACCOVERLIMIT); 110 | return; 111 | }*/ 112 | 113 | if(mRangeLimit.mSwLimitPositive && 114 | mRangeLimit.mLimitPositive < mThis_->sysPosToUser(mSubmitCmdPos) && 115 | calVel > 0) { 116 | mThis_->emergStop(MC_ERRORCODE_CMDPPOSOVERLIMIT); 117 | return; 118 | } 119 | 120 | if(mRangeLimit.mSwLimitNegative && 121 | mRangeLimit.mLimitNegative > mThis_->sysPosToUser(mSubmitCmdPos) && 122 | calVel < 0) { 123 | mThis_->emergStop(MC_ERRORCODE_CMDNPOSOVERLIMIT); 124 | return; 125 | } 126 | 127 | mCmdPos = mSubmitCmdPos; 128 | mCalVel = calVel; 129 | 130 | double _2147 = fabs(toSystemLogic(2147483648.0)); 131 | double _4294 = fabs(toSystemLogic(4294967296.0)); 132 | if(mCmdPos >= _2147) { 133 | mEncoderOverflowOffset = -_4294; 134 | } else if(mCmdPos < -_2147) { 135 | mEncoderOverflowOffset = _4294; 136 | } else { 137 | mEncoderOverflowOffset = 0; 138 | } 139 | 140 | if(mEncoderOverflowOffset) { 141 | URANUS_CALL_EVENT(mThis_->onPositionOffset, mThis_, mEncoderOverflowOffset); 142 | mCmdPos += mEncoderOverflowOffset; 143 | } 144 | 145 | double curDevPos = toSystemLogic(mServo->pos()); 146 | double posDiff = fabs(mCmdPos - curDevPos); 147 | 148 | if(mControl.mControlMode != MC_CONTROLMODE_VELOPENLOOP) { 149 | if(__isgt(posDiff, mMotionLimit.mPosLagLimit)) { 150 | posDiff -= fabs(toSystemLogic(4294967296.0)); 151 | posDiff = fabs(posDiff); 152 | if(__isgt(posDiff, mMotionLimit.mPosLagLimit)) { 153 | mThis_->emergStop(MC_ERRORCODE_POSLAGOVERLIMIT); 154 | return; 155 | } 156 | } 157 | } 158 | 159 | updateCmdPosToDev(); 160 | } 161 | 162 | void AxisBase::AxisBaseImpl::servoStatusMaintains(void) 163 | { 164 | //处理错误重置 165 | if(mNeedReset) { 166 | if(mThis_->errorCode()) { //存在错误尝试恢复 167 | bool isDone = false; 168 | //驱动器恢复 169 | MC_ServoErrorCode devErrorCode = mServo->resetError(isDone); 170 | if(devErrorCode) { //驱动器恢复过程中出错，恢复失败 171 | mDevErrorCode = devErrorCode; 172 | mNeedReset = false; 173 | } else if(isDone) { //恢复成功 174 | mDevErrorCode = 0; 175 | mErrorCode = MC_ERRORCODE_GOOD; 176 | mNeedReset = false; 177 | } 178 | } else { //不存在错误则直接恢复 179 | mNeedReset = false; 180 | } 181 | } 182 | 183 | if(!mThis_->errorCode() && !mPowerStatusValid) { 184 | //处理驱动器使能 185 | if(mPowerStatus) { //开 186 | //同步实际参数到指令参数 187 | mCmdVel = mCmdAcc = 0; 188 | mCmdPos = toSystemLogic(mServo->pos()); 189 | mSubmitCmdPos = mCmdPos; 190 | updateCmdPosToDev(); 191 | } 192 | bool isDone = false; 193 | mDevErrorCode = mServo->setPower(mPowerStatus, isDone); 194 | if(mDevErrorCode) { //使能失败 195 | mThis_->emergStop(MC_ERRORCODE_AXISHARDWARE); 196 | } else if(isDone) { //使能成功 197 | if(mPowerStatus) { 198 | mThis_->printLog(MC_LOGLEVEL_INFO, "Power on\n"); 199 | } else { 200 | mThis_->printLog(MC_LOGLEVEL_INFO, "Power off\n"); 201 | } 202 | mPowerStatusValid = true; 203 | URANUS_CALL_EVENT(mThis_->onPowerStatusChanged, mThis_, mPowerStatus); 204 | } 205 | } else if(!mPowerStatus && mPowerStatusValid) { 206 | //处理非使能时的指令与实际同步 207 | mCmdPos = toSystemLogic(mServo->pos()); 208 | mCmdVel = toSystemLogic(mServo->vel()); 209 | mCmdAcc = toSystemLogic(mServo->acc()); 210 | } 211 | } 212 | 213 | void AxisBase::AxisBaseImpl::updateCmdPosToDev(void) 214 | { 215 | double mCmdPosWithFF = mCmdPos + mControl.mFF * 0.01 * mCmdVel; 216 | 217 | switch(mControl.mControlMode) { 218 | case MC_CONTROLMODE_POSOPENLOOP: { //位置控制模式 219 | int32_t rawPos = toDevRaw(mCmdPosWithFF); 220 | mDevErrorCode = mServo->setPos(rawPos); 221 | break; 222 | } 223 | 224 | case MC_CONTROLMODE_VELCLOSELOOP: { //速度闭环控制模式 225 | int32_t rawVel = toDevRaw(mCmdPosWithFF) - mServo->pos(); 226 | rawVel *= mControl.mPKp; 227 | mDevErrorCode = mServo->setVel(rawVel); 228 | break; 229 | } 230 | 231 | case MC_CONTROLMODE_VELOPENLOOP: { //速度开环控制模式 232 | int32_t rawVel = toDevRaw(mCmdVel); 233 | mDevErrorCode = mServo->setVel(rawVel); 234 | break; 235 | } 236 | } 237 | 238 | if(mDevErrorCode) 239 | mThis_->emergStop(MC_ERRORCODE_AXISHARDWARE); 240 | } 241 | 242 | inline int32_t AxisBase::AxisBaseImpl::toDevRaw(double x) const 243 | { 244 | union { 245 | int32_t _32; 246 | int64_t _64; 247 | }raw; 248 | raw._64 = (int64_t)(x * mMetric.mDevUnitRatio); 249 | return raw._32; 250 | } 251 | 252 | inline double AxisBase::AxisBaseImpl::toSystemLogic(double x) const 253 | { 254 | return x / mMetric.mDevUnitRatio; 255 | } 256 | 257 | double AxisBase::AxisBaseImpl::moduloToLinear( 258 | double basePos, double targetPos, MC_Direction dir) const 259 | { 260 | if(!mMetric.mModulo) 261 | return targetPos; 262 | 263 | double targetPosMod = linearToModulo(targetPos); 264 | double basePosAlign = basePos - linearToModulo(basePos); 265 | double targetPosCurrent = basePosAlign + targetPosMod; 266 | 267 | switch(dir) { 268 | case MC_DIRECTION_POSITIVE: 269 | if(targetPosCurrent > basePos) 270 | return targetPosCurrent; 271 | else 272 | return targetPosCurrent + mMetric.mModulo; 273 | 274 | case MC_DIRECTION_NEGATIVE: 275 | if(targetPosCurrent < basePos) 276 | return targetPosCurrent; 277 | else 278 | return targetPosCurrent - mMetric.mModulo; 279 | 280 | case MC_DIRECTION_SHORTESTWAY: 281 | { 282 | double targetPosSide; 283 | if(targetPosCurrent > basePos) 284 | targetPosSide = targetPosCurrent - mMetric.mModulo; 285 | else 286 | targetPosSide = targetPosCurrent + mMetric.mModulo; 287 | 288 | double distCurrent = fabs(targetPosCurrent - basePos); 289 | double distSide = fabs(targetPosSide - basePos); 290 | if(distCurrent < distSide) 291 | return targetPosCurrent; 292 | else 293 | return targetPosSide; 294 | } 295 | 296 | case MC_DIRECTION_CURRENT: 297 | default: 298 | return targetPosCurrent; 299 | } 300 | } 301 | 302 | double AxisBase::AxisBaseImpl::linearToModulo(double pos) const 303 | { 304 | if(!mMetric.mModulo) 305 | return pos; 306 | 307 | double posMod = fmod(pos, mMetric.mModulo); 308 | if(posMod < 0) 309 | posMod += mMetric.mModulo; 310 | 311 | return posMod; 312 | } 313 | 314 | double AxisBase::AxisBaseImpl::packHomeOffset(double pos) const 315 | { 316 | double posWithHomeOffset = pos + mHomePos; 317 | double _2147 = fabs(toSystemLogic(2147483648.0)); 318 | double _4294 = fabs(toSystemLogic(4294967296.0)); 319 | if(posWithHomeOffset >= _2147) { 320 | posWithHomeOffset -= _4294; 321 | } else if(posWithHomeOffset < -_2147) { 322 | posWithHomeOffset += _4294; 323 | } 324 | return posWithHomeOffset; 325 | } 326 | 327 | double AxisBase::AxisBaseImpl::stripHomeOffset(double basePos, double pos) const 328 | { 329 | return pos - packHomeOffset(basePos) + basePos; 330 | } 331 | 332 | ///////////////////////////////////////////////////////////// 333 | 334 | AxisBase::AxisBase() 335 | { 336 | mImpl_ = new AxisBaseImpl(); 337 | mImpl_->mThis_ = this; 338 | } 339 | 340 | AxisBase::~AxisBase() 341 | { 342 | if(mImpl_->mServo) 343 | delete mImpl_->mServo; 344 | delete mImpl_; 345 | } 346 | 347 | void AxisBase::runCycle(void) 348 | { 349 | mImpl_->servoStatusMaintains(); 350 | mImpl_->processPositionLoop(); 351 | mImpl_->mServo->runCycle(frequency()); 352 | } 353 | 354 | void AxisBase::setServo(Servo* servo) 355 | { 356 | mImpl_->mServo = servo; 357 | } 358 | 359 | void AxisBase::setAxisName(const char* name) 360 | { 361 | strncpy(mImpl_->mAxisName, name, URANUS_AXISNAMESIZE); 362 | mImpl_->mAxisName[URANUS_AXISNAMESIZE - 1] = '\0'; 363 | } 364 | 365 | MC_ErrorCode AxisBase::setMetricInfo(const AxisMetricInfo& info) 366 | { 367 | if(powerStatus()) 368 | return MC_ERRORCODE_AXISPOWERON; 369 | 370 | if(errorCode()) 371 | return errorCode(); 372 | 373 | if(fabs(info.mDevUnitRatio) < 1.0 || 374 | fabs(info.mDevUnitRatio) > 1048576.0 || 375 | !std::isfinite(info.mDevUnitRatio)) 376 | return MC_ERRORCODE_CFGUNITRATIOOUTOFRANGE; 377 | 378 | if(info.mModulo < 0 || !std::isfinite(info.mModulo)) 379 | return MC_ERRORCODE_CFGMODULOILLEGAL; 380 | 381 | mImpl_->mMetric = info; 382 | 383 | return MC_ERRORCODE_GOOD; 384 | } 385 | 386 | MC_ErrorCode AxisBase::setRangeLimitInfo(const AxisRangeLimitInfo& info) 387 | { 388 | mImpl_->mRangeLimit = info; 389 | 390 | return MC_ERRORCODE_GOOD; 391 | } 392 | 393 | MC_ErrorCode AxisBase::setMotionLimitInfo(const AxisMotionLimitInfo& info) 394 | { 395 | if(powerStatus()) 396 | return MC_ERRORCODE_AXISPOWERON; 397 | 398 | if(info.mVelLimit <= 0 || !std::isfinite(info.mVelLimit)) 399 | return MC_ERRORCODE_CFGVELLIMITILLEGAL; 400 | 401 | if(info.mAccLimit <= 0 || !std::isfinite(info.mAccLimit)) 402 | return MC_ERRORCODE_CFGACCLIMITILLEGAL; 403 | 404 | if(info.mPosLagLimit <= 0 || !std::isfinite(info.mPosLagLimit)) 405 | return MC_ERRORCODE_CFGPOSLAGILLEGAL; 406 | 407 | mImpl_->mMotionLimit = info; 408 | 409 | return MC_ERRORCODE_GOOD; 410 | } 411 | 412 | MC_ErrorCode AxisBase::setControlInfo(const AxisControlInfo& info) 413 | { 414 | if(powerStatus()) 415 | return MC_ERRORCODE_AXISPOWERON; 416 | 417 | if(errorCode()) 418 | return errorCode(); 419 | 420 | if(info.mPKp < 0 || !std::isfinite(info.mPKp)) 421 | return MC_ERRORCODE_CFGPKPILLEGAL; 422 | 423 | if(info.mFF < 0 || !std::isfinite(info.mFF)) 424 | return MC_ERRORCODE_CFGFEEDFORWORDILLEGAL; 425 | 426 | switch(info.mControlMode) { 427 | case MC_CONTROLMODE_POSOPENLOOP: 428 | case MC_CONTROLMODE_VELCLOSELOOP: 429 | case MC_CONTROLMODE_VELOPENLOOP: 430 | break; 431 | 432 | default: 433 | return MC_ERRORCODE_CONTROLMODEILLEGAL; 434 | } 435 | 436 | mImpl_->mControl = info; 437 | 438 | return MC_ERRORCODE_GOOD; 439 | } 440 | 441 | MC_ErrorCode AxisBase::setHomePosition(double homePos) 442 | { 443 | if(!std::isfinite(homePos)) 444 | return MC_ERRORCODE_HOMEPOSITIONILLEGAL; 445 | 446 | double _2147 = fabs(mImpl_->toSystemLogic(2147483648.0)); 447 | double _4294 = fabs(mImpl_->toSystemLogic(4294967296.0)); 448 | 449 | if(homePos >= _2147) { 450 | homePos -= _4294; 451 | } else if(homePos < -_2147) { 452 | homePos += _4294; 453 | } 454 | 455 | printLog(MC_LOGLEVEL_DEBUG, 456 | "Set home pos %lf, previous home pos %lf, diff %lf\n", 457 | homePos, mImpl_->mHomePos, homePos - mImpl_->mHomePos); 458 | mImpl_->mHomePos = homePos; 459 | return MC_ERRORCODE_GOOD; 460 | } 461 | 462 | const AxisMetricInfo& AxisBase::metricInfo(void) const 463 | { 464 | return mImpl_->mMetric; 465 | } 466 | 467 | const AxisRangeLimitInfo& AxisBase::rangeLimitInfo(void) const 468 | { 469 | return mImpl_->mRangeLimit; 470 | } 471 | 472 | const AxisMotionLimitInfo& AxisBase::motionLimitInfo(void) const 473 | { 474 | return mImpl_->mMotionLimit; 475 | } 476 | 477 | const AxisControlInfo& AxisBase::controlInfo(void) const 478 | { 479 | return mImpl_->mControl; 480 | } 481 | 482 | double AxisBase::homePosition(void) const 483 | { 484 | return mImpl_->mHomePos; 485 | } 486 | 487 | MC_ErrorCode AxisBase::setPower( 488 | bool powerStatus, 489 | bool enablePositive, 490 | bool enableNegative, 491 | bool& isDone) 492 | { 493 | isDone = false; 494 | 495 | if(errorCode()) 496 | return errorCode(); 497 | 498 | if(mImpl_->mPowerStatusValid && 499 | (mImpl_->mPowerStatus == powerStatus)) { 500 | isDone = true; 501 | return MC_ERRORCODE_GOOD; 502 | } 503 | 504 | mImpl_->mPowerStatus = powerStatus; 505 | mImpl_->mEnablePositive = enablePositive; 506 | mImpl_->mEnableNegative = enableNegative; 507 | mImpl_->mPowerStatusValid = false; 508 | 509 | return MC_ERRORCODE_GOOD; 510 | } 511 | 512 | void AxisBase::emergStop(MC_ErrorCode errorCodeToSet) 513 | { 514 | if(errorCode()) 515 | return; 516 | 517 | mImpl_->mErrorCode = errorCodeToSet; 518 | mImpl_->mCmdVel = mImpl_->mCmdAcc = 0; 519 | mImpl_->mSubmitCmdPos = mImpl_->mCmdPos; 520 | mImpl_->mEncoderOverflowOffset = 0; 521 | mImpl_->mPowerStatusValid = false; 522 | mImpl_->mNeedReset = false; 523 | mImpl_->mServo->emergStop(); 524 | printLog(MC_LOGLEVEL_WARN, 525 | "EmergStop, ErrorID 0x%x, AxisErrorID 0x%x\n", 526 | errorCode(), devErrorCode()); 527 | URANUS_CALL_EVENT(onError, this, errorCode()); 528 | } 529 | 530 | MC_ErrorCode AxisBase::resetError(bool& isDone) 531 | { 532 | if(!errorCode()) { 533 | isDone = true; 534 | return MC_ERRORCODE_GOOD; 535 | } 536 | 537 | mImpl_->mNeedReset = true; 538 | return MC_ERRORCODE_GOOD; 539 | } 540 | 541 | MC_ErrorCode AxisBase::setPosition(double pos, double vel, double acc) 542 | { 543 | if(errorCode()) 544 | return errorCode(); 545 | 546 | if(!powerStatus()) 547 | return MC_ERRORCODE_AXISPOWEROFF; 548 | 549 | if(!std::isfinite(pos)) { 550 | emergStop(MC_ERRORCODE_POSINFINITY); 551 | return errorCode(); 552 | } 553 | 554 | mImpl_->mSubmitCmdPos = pos; 555 | //mImpl_->mCmdVel = (mImpl_->mSubmitCmdPos - mImpl_->mCmdPos) * frequency(); 556 | mImpl_->mCmdVel = vel; 557 | mImpl_->mCmdAcc = acc; 558 | return MC_ERRORCODE_GOOD; 559 | } 560 | 561 | const char* AxisBase::axisName(void) const 562 | { 563 | return mImpl_->mAxisName; 564 | } 565 | 566 | bool AxisBase::powerStatus(void) const 567 | { 568 | return mImpl_->mPowerStatus && mImpl_->mPowerStatusValid; 569 | } 570 | 571 | MC_ErrorCode AxisBase::errorCode(void) const 572 | { 573 | return mImpl_->mErrorCode; 574 | } 575 | 576 | MC_ServoErrorCode AxisBase::devErrorCode(void) const 577 | { 578 | return mImpl_->mDevErrorCode; 579 | } 580 | 581 | double AxisBase::cmdPosition(void) const 582 | { 583 | return mImpl_->mCmdPos; 584 | } 585 | 586 | double AxisBase::cmdVelocity(void) const 587 | { 588 | return mImpl_->mCmdVel; 589 | } 590 | 591 | double AxisBase::cmdAcceleration(void) const 592 | { 593 | return mImpl_->mCmdAcc; 594 | } 595 | 596 | double AxisBase::positionOffset(void) const 597 | { 598 | return mImpl_->mEncoderOverflowOffset; 599 | } 600 | 601 | double AxisBase::actPosition(void) const 602 | { 603 | return mImpl_->toSystemLogic(mImpl_->mServo->pos()); 604 | } 605 | 606 | double AxisBase::actVelocity(void) const 607 | { 608 | return mImpl_->toSystemLogic(mImpl_->mServo->vel()); 609 | } 610 | 611 | double AxisBase::actAcceleration(void) const 612 | { 613 | return mImpl_->toSystemLogic(mImpl_->mServo->acc()); 614 | } 615 | 616 | double AxisBase::actTorque(void) const 617 | { 618 | return mImpl_->mServo->torque(); 619 | } 620 | 621 | bool AxisBase::servoReadVal(int index, double& value) 622 | { 623 | return mImpl_->mServo->readVal(index, value); 624 | } 625 | 626 | bool AxisBase::servoWriteVal(int index, double value) 627 | { 628 | return mImpl_->mServo->writeVal(index, value); 629 | } 630 | 631 | double AxisBase::userPosToSys( 632 | double baseSysPos, double userPos, MC_Direction dir) const 633 | { 634 | double userPosNoHome = mImpl_->stripHomeOffset(baseSysPos, userPos); 635 | return mImpl_->moduloToLinear(baseSysPos, userPosNoHome, dir); 636 | } 637 | 638 | double AxisBase::sysPosToUser(double sysPos) const 639 | { 640 | return mImpl_->linearToModulo(mImpl_->packHomeOffset(sysPos)); 641 | } 642 | 643 | void AxisBase::printLog(MC_LogLevel level, const char* fmt, ...) 644 | { 645 | va_list ap; 646 | va_start(ap, fmt); 647 | vprintLog(level, fmt, ap); 648 | va_end(ap); 649 | } 650 | 651 | } 652 | -------------------------------------------------------------------------------- /motion/axis/AxisBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisBase.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_AXISBASE_HPP_ 26 | #define _URANUS_AXISBASE_HPP_ 27 | 28 | #include "Global.hpp" 29 | #include "Event.hpp" 30 | #include 31 | 32 | namespace Uranus { 33 | 34 | class Servo; 35 | class AxisBase 36 | { 37 | public: 38 | AxisBase(); 39 | virtual ~AxisBase(); 40 | 41 | void runCycle(void); 42 | 43 | void setServo(Servo* servo); 44 | 45 | void setAxisName(const char* name); 46 | MC_ErrorCode setMetricInfo(const AxisMetricInfo& info); 47 | MC_ErrorCode setRangeLimitInfo(const AxisRangeLimitInfo& info); 48 | MC_ErrorCode setMotionLimitInfo(const AxisMotionLimitInfo& info); 49 | MC_ErrorCode setControlInfo(const AxisControlInfo& info); 50 | MC_ErrorCode setHomePosition(double homePos); 51 | 52 | const char* axisName(void) const; 53 | const AxisMetricInfo& metricInfo(void) const; 54 | const AxisRangeLimitInfo& rangeLimitInfo(void) const; 55 | const AxisMotionLimitInfo& motionLimitInfo(void) const; 56 | const AxisControlInfo& controlInfo(void) const; 57 | double homePosition(void) const; 58 | 59 | MC_ErrorCode setPower( 60 | bool powerStatus, 61 | bool enablePositive, 62 | bool enableNegative, 63 | bool& isDone); 64 | 65 | void emergStop(MC_ErrorCode errorCodeToSet); 66 | MC_ErrorCode resetError(bool& isDone); 67 | MC_ErrorCode setPosition(double pos, double vel, double acc); 68 | 69 | bool powerStatus(void) const; 70 | MC_ErrorCode errorCode(void) const; 71 | MC_ServoErrorCode devErrorCode(void) const; 72 | 73 | double cmdPosition(void) const; 74 | double cmdVelocity(void) const; 75 | double cmdAcceleration(void) const; 76 | double positionOffset(void) const; 77 | 78 | double actPosition(void) const; 79 | double actVelocity(void) const; 80 | double actAcceleration(void) const; 81 | double actTorque(void) const; 82 | 83 | bool servoReadVal(int index, double& value); 84 | bool servoWriteVal(int index, double value); 85 | 86 | double userPosToSys(double baseSysPos, double userPos, MC_Direction dir) const; 87 | double sysPosToUser(double sysPos) const; 88 | 89 | void printLog(MC_LogLevel level, const char* fmt, ...); 90 | 91 | protected: //事件通知 92 | URANUS_DEFINE_EVENT(onError, AxisBase*, MC_ErrorCode); 93 | URANUS_DEFINE_EVENT(onPowerStatusChanged, AxisBase*, bool); 94 | URANUS_DEFINE_EVENT(onPositionOffset, AxisBase*, double); 95 | 96 | protected: 97 | virtual double frequency(void) = 0; 98 | virtual uint32_t tick(void) = 0; 99 | virtual void vprintLog(MC_LogLevel level, const char* fmt, va_list ap) = 0; 100 | 101 | private: 102 | class AxisBaseImpl; 103 | AxisBaseImpl* mImpl_; 104 | }; 105 | 106 | } 107 | 108 | #endif /** _URANUS_AXISBASE_HPP_ **/ 109 | -------------------------------------------------------------------------------- /motion/axis/AxisHoming.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisHoming.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "AxisHoming.hpp" 26 | #include "FunctionBlock.hpp" 27 | #include "ProfilePlanner.hpp" 28 | #include "MathUtils.hpp" 29 | #include "Event.hpp" 30 | 31 | namespace Uranus { 32 | 33 | typedef enum { 34 | MC_HOMINGSTEP_INIT = 0, 35 | MC_HOMINGSTEP_SEARCHSIG = 1, 36 | MC_HOMINGSTEP_REGRESSIONSIG = 2, 37 | MC_HOMINGSTEP_TOSIG = 3, 38 | }MC_HomingStep; 39 | 40 | struct AxisHomingInfoEx : public AxisHomingInfo 41 | { 42 | bool mHomingSigVal = false; //回零信号比对值 43 | }; 44 | 45 | class AxisHoming::AxisHomingImpl 46 | { 47 | public: 48 | AxisHomingInfoEx mHomingInfo; 49 | ProfilePlanner mPlanner; 50 | }; 51 | 52 | class HomingNode : virtual public AxisExeclNode 53 | { 54 | public: 55 | double mPos = 0; 56 | double mFinalPos = 0; 57 | MC_HomingStep mHomingStep = MC_HOMINGSTEP_INIT; 58 | 59 | protected: 60 | virtual MC_ErrorCode onExecuting(ExeclQueue* queue, ExeclNodeExecStat& stat) override; 61 | virtual void onPositionOffset(ExeclQueue* queue, double offset) override; 62 | }; 63 | 64 | MC_ErrorCode HomingNode::onExecuting( 65 | ExeclQueue* queue, ExeclNodeExecStat& stat) 66 | { 67 | AxisHoming* axis = dynamic_cast(queue); 68 | ProfilePlanner* planner = &axis->mImpl_->mPlanner; 69 | AxisHomingInfoEx* homingInfo = &axis->mImpl_->mHomingInfo; 70 | MC_ErrorCode err; 71 | 72 | switch(mHomingStep) { 73 | case MC_HOMINGSTEP_INIT: 74 | if(!homingInfo->mHomingSig) { //当前位置作为零点 75 | goto HOMINGSTEP_TOSIG; 76 | } else { //启动回零流程 77 | axis->printLog(MC_LOGLEVEL_INFO, 78 | "homing start, vel %lf, sig %p, bitoffset %d, trig %d\n", 79 | homingInfo->mHomingVelSearch, 80 | homingInfo->mHomingSig, 81 | homingInfo->mHomingSigBitOffset, 82 | homingInfo->mHomingSigVal); 83 | 84 | if(!homingInfo->mHomingVelSearch || !homingInfo->mHomingVelRegression) 85 | return MC_ERRORCODE_HOMINGVELILLEGAL; 86 | 87 | if(!homingInfo->mHomingAcc) 88 | return MC_ERRORCODE_HOMINGACCILLEGAL; 89 | 90 | double endPos = ProfilePlanner::calculateDist( 91 | axis->cmdVelocity(), 92 | homingInfo->mHomingVelSearch, 93 | homingInfo->mHomingAcc, 94 | homingInfo->mHomingAcc); 95 | 96 | planner->plan( 97 | axis->cmdPosition(), 98 | axis->cmdPosition() + endPos, 99 | axis->cmdVelocity(), 100 | homingInfo->mHomingVelSearch, 101 | homingInfo->mHomingVelSearch, 102 | homingInfo->mHomingAcc, 103 | homingInfo->mHomingAcc); 104 | 105 | mHomingStep = MC_HOMINGSTEP_SEARCHSIG; 106 | } 107 | break; 108 | 109 | case MC_HOMINGSTEP_SEARCHSIG: 110 | if((((*homingInfo->mHomingSig) >> homingInfo->mHomingSigBitOffset) & 0x1) == 111 | homingInfo->mHomingSigVal) { 112 | 113 | double endPos = ProfilePlanner::calculateDist( 114 | axis->cmdVelocity(), 115 | homingInfo->mHomingVelRegression, 116 | homingInfo->mHomingAcc, 117 | homingInfo->mHomingAcc); 118 | 119 | planner->plan( 120 | axis->cmdPosition(), 121 | axis->cmdPosition() + endPos, 122 | axis->cmdVelocity(), 123 | homingInfo->mHomingVelRegression, 124 | homingInfo->mHomingVelRegression, 125 | homingInfo->mHomingAcc, 126 | homingInfo->mHomingAcc); 127 | 128 | mHomingStep = MC_HOMINGSTEP_REGRESSIONSIG; 129 | 130 | axis->printLog(MC_LOGLEVEL_INFO, 131 | "homing regressing, vel %lf\n", 132 | homingInfo->mHomingVelRegression); 133 | } 134 | 135 | break; 136 | 137 | case MC_HOMINGSTEP_REGRESSIONSIG: 138 | if((((*homingInfo->mHomingSig) >> homingInfo->mHomingSigBitOffset) & 0x1) != 139 | homingInfo->mHomingSigVal) { 140 | HOMINGSTEP_TOSIG: 141 | mFinalPos = axis->actPosition(); 142 | 143 | planner->plan( 144 | axis->cmdPosition(), 145 | axis->cmdPosition() + 146 | ProfilePlanner::calculateDist( 147 | axis->cmdVelocity(), 148 | __EPSILON, 149 | homingInfo->mHomingAcc, 150 | homingInfo->mHomingAcc), 151 | axis->cmdVelocity(), 152 | homingInfo->mHomingVelRegression, 153 | 0.0, 154 | homingInfo->mHomingAcc, 155 | homingInfo->mHomingAcc); 156 | 157 | mHomingStep = MC_HOMINGSTEP_TOSIG; 158 | } 159 | 160 | break; 161 | 162 | case MC_HOMINGSTEP_TOSIG: 163 | if(planner->execute()) 164 | stat = EXECLNODEEXECSTAT_DONE; 165 | 166 | err = axis->setPosition( 167 | planner->getPosition(), 168 | planner->getVelocity(), 169 | planner->getAcceleration()); 170 | 171 | if(stat == EXECLNODEEXECSTAT_DONE && err == MC_ERRORCODE_GOOD) { 172 | err = axis->setHomePosition(mPos - mFinalPos); 173 | axis->printLog(MC_LOGLEVEL_INFO, 174 | "homing complete, new pos %lf\n", 175 | axis->homePosition()); 176 | } 177 | 178 | return err; 179 | } 180 | 181 | planner->execute(); 182 | 183 | err = axis->setPosition( 184 | planner->getPosition(), 185 | planner->getVelocity(), 186 | planner->getAcceleration()); 187 | 188 | return err; 189 | } 190 | 191 | void HomingNode::onPositionOffset(ExeclQueue* queue, double offset) 192 | { 193 | 194 | } 195 | 196 | AxisHoming::AxisHoming() 197 | { 198 | mImpl_ = new AxisHomingImpl(); 199 | 200 | URANUS_ADD_HANDLER(onPowerStatusChanged, onPowerStatusChangedHandler); 201 | URANUS_ADD_HANDLER(onPositionOffset, onPositionOffsetHandler); 202 | } 203 | 204 | AxisHoming::~AxisHoming() 205 | { 206 | delete mImpl_; 207 | } 208 | 209 | MC_ErrorCode AxisHoming::setHomingInfo(const AxisHomingInfo& info) 210 | { 211 | if(info.mHomingMode != MC_HOMINGMODE_DIRECT) { 212 | if(!info.mHomingVelSearch || !info.mHomingVelRegression) 213 | return MC_ERRORCODE_HOMINGVELILLEGAL; 214 | 215 | if(!info.mHomingAcc) 216 | return MC_ERRORCODE_HOMINGACCILLEGAL; 217 | 218 | if(!info.mHomingSig) 219 | return MC_ERRORCODE_HOMINGSIGILLEGAL; 220 | 221 | if(info.mHomingSigBitOffset < 0 || info.mHomingSigBitOffset > 7) 222 | return MC_ERRORCODE_HOMINGSIGILLEGAL; 223 | } 224 | 225 | switch(info.mHomingMode) { 226 | case MC_HOMINGMODE_DIRECT: 227 | mImpl_->mHomingInfo.mHomingSig = nullptr; 228 | return MC_ERRORCODE_GOOD; 229 | 230 | case MC_HOMINGMODE_MODE5: 231 | mImpl_->mHomingInfo.mHomingSigVal = true; 232 | case MC_HOMINGMODE_MODE6: 233 | mImpl_->mHomingInfo.mHomingVelSearch = 234 | -fabs(info.mHomingVelSearch); 235 | 236 | mImpl_->mHomingInfo.mHomingVelRegression = 237 | fabs(info.mHomingVelRegression); 238 | break; 239 | 240 | case MC_HOMINGMODE_MODE7: 241 | mImpl_->mHomingInfo.mHomingSigVal = true; 242 | case MC_HOMINGMODE_MODE8: 243 | mImpl_->mHomingInfo.mHomingVelSearch = 244 | fabs(info.mHomingVelSearch); 245 | 246 | mImpl_->mHomingInfo.mHomingVelRegression = 247 | -fabs(info.mHomingVelRegression); 248 | break; 249 | 250 | default: 251 | return MC_ERRORCODE_HOMINGMODEILLEGAL; 252 | } 253 | 254 | mImpl_->mHomingInfo.mHomingSig = info.mHomingSig; 255 | mImpl_->mHomingInfo.mHomingMode = info.mHomingMode; 256 | 257 | mImpl_->mHomingInfo.mHomingAcc = fabs(info.mHomingAcc); 258 | mImpl_->mHomingInfo.mHomingJerk = fabs(info.mHomingJerk); 259 | 260 | return MC_ERRORCODE_GOOD; 261 | } 262 | 263 | MC_ErrorCode AxisHoming::addHoming( 264 | FunctionBlock* fb, 265 | double pos, 266 | MC_BufferMode bufferMode, 267 | int32_t customId) 268 | { 269 | if(!std::isfinite(pos)) 270 | return MC_ERRORCODE_POSILLEGAL; 271 | 272 | HomingNode* node; 273 | MC_ErrorCode err = pushAndNewData( 274 | [&node, pos](void* baseNode) -> AxisExeclNode* { 275 | node = (HomingNode*)baseNode; 276 | new (node) HomingNode(); 277 | node->mPos = pos; 278 | return node; 279 | }, 280 | (bufferMode == MC_BUFFERMODE_ABORTING), 281 | fb, 282 | MC_AXISSTATUS_HOMING, 283 | MC_AXISSTATUS_STANDSTILL, 284 | customId); 285 | 286 | if(err) return err; 287 | 288 | return MC_ERRORCODE_GOOD; 289 | } 290 | 291 | void AxisHoming::onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus) 292 | { 293 | AxisHoming* this__ = dynamic_cast(this_); 294 | if(powerStatus) 295 | this__->mImpl_->mPlanner.setFrequency(this__->frequency()); 296 | } 297 | 298 | void AxisHoming::onPositionOffsetHandler(AxisBase* this_, double positionOffset) 299 | { 300 | AxisHoming* this__ = dynamic_cast(this_); 301 | this__->mImpl_->mPlanner.setPositionOffset(positionOffset); 302 | } 303 | 304 | } 305 | -------------------------------------------------------------------------------- /motion/axis/AxisHoming.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisHoming.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_AXISHOMING_HPP_ 26 | #define _URANUS_AXISHOMING_HPP_ 27 | 28 | #include "AxisMotionBase.hpp" 29 | 30 | namespace Uranus { 31 | 32 | class AxisHoming : virtual public AxisMotionBase 33 | { 34 | public: 35 | AxisHoming(); 36 | virtual ~AxisHoming(); 37 | 38 | public: 39 | MC_ErrorCode setHomingInfo(const AxisHomingInfo& info); 40 | 41 | MC_ErrorCode addHoming( 42 | FunctionBlock* fb, 43 | double pos, 44 | MC_BufferMode bufferMode = MC_BUFFERMODE_ABORTING, 45 | int32_t customId = 0); 46 | 47 | private: 48 | static void onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus); 49 | static void onPositionOffsetHandler(AxisBase* this_, double positionOffset); 50 | 51 | private: 52 | class AxisHomingImpl; 53 | AxisHomingImpl* mImpl_; 54 | friend class HomingNode; 55 | }; 56 | 57 | } 58 | #endif /** _URANUS_AXISHOMING_HPP_ **/ 59 | -------------------------------------------------------------------------------- /motion/axis/AxisMotion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisMotion.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "AxisMotion.hpp" 26 | 27 | namespace Uranus { 28 | 29 | AxisMotion::AxisMotion() 30 | { 31 | 32 | } 33 | 34 | AxisMotion::~AxisMotion() 35 | { 36 | 37 | } 38 | 39 | double AxisMotion::cmdPosition(void) const 40 | { 41 | return sysPosToUser(AxisBase::cmdPosition()); 42 | } 43 | 44 | double AxisMotion::actPosition(void) const 45 | { 46 | return sysPosToUser(AxisBase::actPosition()); 47 | } 48 | 49 | } 50 | -------------------------------------------------------------------------------- /motion/axis/AxisMotion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisMotion.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_AXISMOTION_HPP_ 26 | #define _URANUS_AXISMOTION_HPP_ 27 | 28 | #include "AxisMove.hpp" 29 | #include "AxisHoming.hpp" 30 | 31 | namespace Uranus { 32 | 33 | class AxisMotion : 34 | virtual public AxisMove, 35 | virtual public AxisHoming 36 | { 37 | public: 38 | AxisMotion(); 39 | virtual ~AxisMotion(); 40 | 41 | double cmdPosition(void) const; 42 | double actPosition(void) const; 43 | }; 44 | 45 | } 46 | 47 | #endif /** _URANUS_AXISMOTION_HPP_ **/ 48 | -------------------------------------------------------------------------------- /motion/axis/AxisMotionBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisMotionBase.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "AxisMotionBase.hpp" 26 | #include "FunctionBlock.hpp" 27 | //#include "AxesGroupBase.hpp" 28 | 29 | namespace Uranus { 30 | 31 | class AxisMotionBase::AxisMotionBaseImpl 32 | { 33 | public: 34 | //AxesGroupBase* mGroup = nullptr; 35 | }; 36 | 37 | MC_ErrorCode AxisExeclNode::onActive(ExeclQueue* queue) 38 | { 39 | AxisMotionBase* axis = dynamic_cast(queue); 40 | MC_ErrorCode err = axis->setStatus(mStatusActive); 41 | if(err) return err; 42 | 43 | axis->operationActive(mFb, mNodeCustomId); 44 | if(mFb) 45 | mFb->onOperationActive(mNodeCustomId); 46 | 47 | return MC_ERRORCODE_GOOD; 48 | } 49 | 50 | void AxisExeclNode::onAborted(ExeclQueue* queue) 51 | { 52 | AxisMotionBase* axis = dynamic_cast(queue); 53 | axis->operationAborted(mFb, mNodeCustomId); 54 | if(mFb) 55 | mFb->onOperationAborted(mNodeCustomId); 56 | } 57 | 58 | void AxisExeclNode::onDone(ExeclQueue* queue, bool& isHold) 59 | { 60 | AxisMotionBase* axis = dynamic_cast(queue); 61 | axis->setStatus(mStatusDone); 62 | axis->operationDone(mFb, mNodeCustomId); 63 | if(mFb) 64 | mFb->onOperationDone(mNodeCustomId); 65 | } 66 | 67 | void AxisExeclNode::onError(ExeclQueue* queue, MC_ErrorCode errorCode) 68 | { 69 | AxisMotionBase* axis = dynamic_cast(queue); 70 | axis->operationError(mFb, mNodeCustomId, errorCode); 71 | if(mFb) 72 | mFb->onOperationError(errorCode, mNodeCustomId); 73 | } 74 | 75 | AxisMotionBase::AxisMotionBase() 76 | { 77 | mImpl_ = new AxisMotionBaseImpl(); 78 | URANUS_ADD_HANDLER(onError, onErrorHandler); 79 | URANUS_ADD_HANDLER(onPowerStatusChanged, onPowerStatusChangedHandler); 80 | URANUS_ADD_HANDLER(onPositionOffset, onPositionOffsetHandler); 81 | } 82 | 83 | AxisMotionBase::~AxisMotionBase() 84 | { 85 | delete mImpl_; 86 | } 87 | 88 | void AxisMotionBase::runCycle(void) 89 | { 90 | processExeclNode(); 91 | AxisBase::runCycle(); 92 | } 93 | 94 | MC_ErrorCode AxisMotionBase::pushAndNewData( 95 | const std::function& constructor, 96 | bool abortFlag, 97 | FunctionBlock* fb, 98 | MC_AxisStatus statusActive, 99 | MC_AxisStatus statusDone, 100 | int32_t nodeCustomId) 101 | { 102 | if(errorCode()) 103 | return errorCode(); 104 | 105 | if(!powerStatus()) 106 | return MC_ERRORCODE_AXISPOWEROFF; 107 | 108 | if(abortFlag) { 109 | MC_ErrorCode err = setStatus(statusActive); 110 | if(err) return err; 111 | } 112 | 113 | return ExeclQueue::pushAndNewData( 114 | [&constructor, fb, statusActive, statusDone, nodeCustomId] 115 | (void* baseNode) -> AxisExeclNode* { 116 | AxisExeclNode* node = constructor(baseNode); 117 | node->mFb = fb; 118 | node->mStatusActive = statusActive; 119 | node->mStatusDone = statusDone; 120 | node->mNodeCustomId = nodeCustomId; 121 | return node; 122 | }, abortFlag); 123 | } 124 | 125 | void AxisMotionBase::onErrorHandler(AxisBase* this_, MC_ErrorCode errorCode) 126 | { 127 | AxisMotionBase* this__ = dynamic_cast(this_); 128 | this__->setAllNodesError(errorCode); 129 | } 130 | 131 | void AxisMotionBase::onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus) 132 | { 133 | AxisMotionBase* this__ = dynamic_cast(this_); 134 | this__->setAllNodesAborted(); 135 | /* 136 | AxesGroupBase* group = this__->mImpl_->mGroup; 137 | if(group) { 138 | URANUS_CALL_EVENT(group->onAxisPowerStatusChanged, group, this__, powerStatus); 139 | } 140 | */ 141 | } 142 | 143 | void AxisMotionBase::onPositionOffsetHandler(AxisBase* this_, double positionOffset) 144 | { 145 | AxisMotionBase* this__ = dynamic_cast(this_); 146 | 147 | AxisExeclNode* node = 148 | dynamic_cast(this__->ExeclQueue::front()); 149 | 150 | while(node) { 151 | node->onPositionOffset(this__, positionOffset); 152 | node = dynamic_cast(this__->ExeclQueue::next(node)); 153 | } 154 | } 155 | 156 | } 157 | -------------------------------------------------------------------------------- /motion/axis/AxisMotionBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisMotionBase.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_AXISMOTIONBASE_HPP_ 26 | #define _URANUS_AXISMOTIONBASE_HPP_ 27 | 28 | #include "AxisStatus.hpp" 29 | #include "ExeclQueue.hpp" 30 | #include "LinkList.hpp" 31 | 32 | namespace Uranus { 33 | 34 | //class AxesGroupBase; 35 | class FunctionBlock; 36 | class AxisExeclNode : virtual public ExeclNode 37 | { 38 | public: 39 | FunctionBlock* mFb = nullptr; 40 | MC_AxisStatus mStatusActive = MC_AXISSTATUS_STANDSTILL; 41 | MC_AxisStatus mStatusDone = MC_AXISSTATUS_STANDSTILL; 42 | int32_t mNodeCustomId = 0; 43 | 44 | protected: 45 | virtual MC_ErrorCode onActive(ExeclQueue* queue) override; 46 | virtual void onAborted(ExeclQueue* queue) override; 47 | virtual void onDone(ExeclQueue* queue, bool& isHold) override; 48 | virtual void onError(ExeclQueue* queue, MC_ErrorCode errorCode) override; 49 | 50 | public: 51 | virtual void onPositionOffset(ExeclQueue* queue, double positionOffset) = 0; 52 | }; 53 | 54 | class AxisMotionBase : 55 | virtual public AxisStatus, 56 | virtual public ExeclQueue, 57 | public LinkNode 58 | { 59 | public: 60 | AxisMotionBase(); 61 | virtual ~AxisMotionBase(); 62 | void runCycle(void); 63 | MC_ErrorCode pushAndNewData( 64 | const std::function& constructor, 65 | bool abortFlag, 66 | FunctionBlock* fb, 67 | MC_AxisStatus statusActive, 68 | MC_AxisStatus statusDone, 69 | int32_t nodeCustomId); 70 | 71 | public: //外部继承获取 72 | virtual void operationActive(FunctionBlock* fb, int32_t customId){} 73 | virtual void operationAborted(FunctionBlock* fb, int32_t customId){} 74 | virtual void operationDone(FunctionBlock* fb, int32_t customId){} 75 | virtual void operationError( 76 | FunctionBlock* fb, int32_t customId, MC_ErrorCode errorCode){} 77 | 78 | private: 79 | static void onErrorHandler(AxisBase* this_, MC_ErrorCode errorCode); 80 | static void onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus); 81 | static void onPositionOffsetHandler(AxisBase* this_, double positionOffset); 82 | 83 | private: 84 | class AxisMotionBaseImpl; 85 | AxisMotionBaseImpl* mImpl_; 86 | }; 87 | 88 | } 89 | 90 | #endif /** _URANUS_AXISMOTIONBASE_HPP_ **/ 91 | -------------------------------------------------------------------------------- /motion/axis/AxisMove.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisMove.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "AxisMove.hpp" 26 | #include "FunctionBlock.hpp" 27 | #include "ProfilesPlanner.hpp" 28 | #include "MathUtils.hpp" 29 | #include "Event.hpp" 30 | 31 | namespace Uranus { 32 | 33 | class MoveNode : virtual public AxisExeclNode, public ProfileNode 34 | { 35 | public: 36 | bool mNeedPlan = true; 37 | bool mIsHold = false; 38 | 39 | protected: 40 | virtual MC_ErrorCode onExecuting(ExeclQueue* queue, ExeclNodeExecStat& stat) override; 41 | virtual void onDone(ExeclQueue* queue, bool& isHold) override; 42 | virtual void onPositionOffset(ExeclQueue* queue, double positionOffset) override; 43 | }; 44 | 45 | class AxisMove::AxisMoveImpl 46 | { 47 | public: 48 | AxisMove* mThis_; 49 | ProfilesPlanner mPlanner; 50 | 51 | public: 52 | MC_ErrorCode addMove( 53 | FunctionBlock* fb, 54 | double pos, 55 | double vel, 56 | double acc, 57 | double dec, 58 | double endVel, 59 | double jerk, 60 | MC_ShiftingMode shiftingMode, 61 | MC_Direction dir, 62 | MC_BufferMode bufferMode, 63 | MC_AxisStatus statusActive, 64 | MC_AxisStatus statusDone, 65 | bool isHold, 66 | int32_t customId); 67 | }; 68 | 69 | MC_ErrorCode MoveNode::onExecuting( 70 | ExeclQueue* queue, ExeclNodeExecStat& stat) 71 | { 72 | AxisMove* axis = dynamic_cast(queue); 73 | ProfilesPlanner* planner = &axis->mImpl_->mPlanner; 74 | 75 | if(mNeedPlan) { 76 | mNeedPlan = false; 77 | 78 | bool ret = planner->plan( 79 | this, 80 | axis->cmdPosition(), 81 | axis->cmdVelocity(), 82 | axis->cmdAcceleration()); 83 | 84 | axis->printLog(MC_LOGLEVEL_DEBUG, 85 | "MovePos %lf -> %lf, Vel %lf -> %lf, With MaxVel %lf, MaxAcc %lf, MaxDec %lf, Jerk %lf\n", 86 | axis->cmdPosition(), 87 | mEndPos, 88 | axis->cmdVelocity(), 89 | mEndVel, 90 | mVel, 91 | mAcc, 92 | mDec, 93 | mJerk); 94 | 95 | if(!ret) { 96 | stat = EXECLNODEEXECSTAT_FASTDONE; 97 | planner->execute(); 98 | axis->setPosition( 99 | planner->getEndPosition(), 100 | planner->getEndVelocity(), 101 | 0); 102 | return MC_ERRORCODE_GOOD; 103 | } 104 | } 105 | 106 | if(planner->execute()) 107 | stat = EXECLNODEEXECSTAT_DONE; 108 | 109 | return axis->setPosition( 110 | planner->getPosition(), 111 | planner->getVelocity(), 112 | planner->getAcceleration()); 113 | } 114 | 115 | void MoveNode::onDone(ExeclQueue* queue, bool& isHold) 116 | { 117 | AxisExeclNode::onDone(queue, isHold); 118 | isHold = mIsHold; 119 | } 120 | 121 | void MoveNode::onPositionOffset(ExeclQueue* queue, double positionOffset) 122 | { 123 | mStartPos += positionOffset; 124 | mEndPos += positionOffset; 125 | } 126 | 127 | MC_ErrorCode AxisMove::AxisMoveImpl::addMove( 128 | FunctionBlock* fb, 129 | double pos, 130 | double vel, 131 | double acc, 132 | double dec, 133 | double endVel, 134 | double jerk, 135 | MC_ShiftingMode shiftingMode, 136 | MC_Direction dir, 137 | MC_BufferMode bufferMode, 138 | MC_AxisStatus statusActive, 139 | MC_AxisStatus statusDone, 140 | bool isHold, 141 | int32_t customId) 142 | { 143 | MoveNode* node, * nodePrev; 144 | MC_ErrorCode err; 145 | 146 | //速度参数检测 147 | if((vel < 0 && !std::isnan(pos)) || !std::isfinite(vel)) 148 | return MC_ERRORCODE_VELILLEGAL; 149 | 150 | //加速度参数检测 151 | if(acc <= 0 || !std::isfinite(acc) || dec <= 0 || !std::isfinite(dec)) 152 | return MC_ERRORCODE_ACCILLEGAL; 153 | 154 | //位置参数检测 155 | if(std::isinf(pos)) 156 | return MC_ERRORCODE_POSILLEGAL; 157 | 158 | //起始位置处理 159 | double startPos, startVel, startAcc; 160 | if(shiftingMode == MC_SHIFTINGMODE_ADDITIVE || 161 | bufferMode != MC_BUFFERMODE_ABORTING) { //使用最后一个功能块终点位置 162 | if(!mThis_->operationRemains()) goto USE_CURRENT; 163 | nodePrev = dynamic_cast(mThis_->back()); 164 | if(!nodePrev) return MC_ERRORCODE_FAILEDTOBUFFER; 165 | startPos = nodePrev->mEndPos; 166 | startVel = nodePrev->mEndVel; 167 | startAcc = nodePrev->mEndAcc; 168 | } else { //使用当前位置 169 | USE_CURRENT: 170 | startPos = mThis_->cmdPosition(); 171 | startVel = mThis_->cmdVelocity(); 172 | startAcc = mThis_->cmdAcceleration(); 173 | } 174 | 175 | //特殊位置处理 176 | if(std::isnan(pos)) { //不清楚位置 177 | pos = ProfilePlanner::calculateDist(startVel, vel, acc, dec); 178 | pos += startPos; 179 | } else { 180 | switch(shiftingMode) { 181 | case MC_SHIFTINGMODE_ABSOLUTE: { //绝对位置的模量与零点偏移转换 182 | pos = mThis_->userPosToSys(startPos, pos, dir); 183 | break; 184 | } 185 | 186 | case MC_SHIFTINGMODE_RELATIVE: //相对位置 187 | case MC_SHIFTINGMODE_ADDITIVE: //增量位置 188 | pos += startPos; 189 | break; 190 | 191 | default: 192 | return MC_ERRORCODE_SHIFTINGMODEILLEGAL; 193 | } 194 | } 195 | 196 | //添加队列 197 | err = mThis_->pushAndNewData( 198 | [&](void* baseNode) -> AxisExeclNode* { 199 | //构造数据 200 | node = (MoveNode*)baseNode; 201 | new (node) MoveNode(); 202 | node->mStartPos = startPos; 203 | node->mStartVel = startVel; 204 | node->mStartAcc = startAcc; 205 | node->mEndPos = pos; 206 | node->mEndVel = endVel; 207 | node->mEndAcc = 0; 208 | node->mVel = vel; 209 | node->mAcc = acc; 210 | node->mDec = dec; 211 | node->mJerk = jerk; 212 | 213 | node->mIsHold = isHold; 214 | return node; 215 | }, 216 | (bufferMode == MC_BUFFERMODE_ABORTING), 217 | fb, 218 | statusActive, 219 | statusDone, 220 | customId); 221 | 222 | return err; 223 | } 224 | 225 | AxisMove::AxisMove() 226 | { 227 | mImpl_ = new AxisMoveImpl(); 228 | mImpl_->mThis_ = this; 229 | 230 | URANUS_ADD_HANDLER(onPowerStatusChanged, onPowerStatusChangedHandler); 231 | URANUS_ADD_HANDLER(onPositionOffset, onPositionOffsetHandler); 232 | URANUS_ADD_HANDLER(onAllNodesAborted, onAllNodesAbortedHandler); 233 | URANUS_ADD_HANDLER(onAllNodesError, onAllNodesErrorHandler); 234 | } 235 | 236 | AxisMove::~AxisMove() 237 | { 238 | delete mImpl_; 239 | } 240 | 241 | MC_ErrorCode AxisMove::addMovePos( 242 | FunctionBlock* fb, 243 | double pos, 244 | double vel, 245 | double acc, 246 | double dec, 247 | double jerk, 248 | MC_ShiftingMode shiftingMode, 249 | MC_Direction dir, 250 | MC_BufferMode bufferMode, 251 | int32_t customId) 252 | { 253 | if(!vel) 254 | return MC_ERRORCODE_VELILLEGAL; 255 | 256 | return mImpl_->addMove( 257 | fb, 258 | pos, 259 | vel, 260 | acc, 261 | dec, 262 | 0, 263 | jerk, 264 | shiftingMode, 265 | dir, 266 | bufferMode, 267 | MC_AXISSTATUS_DISCRETEMOTION, 268 | MC_AXISSTATUS_STANDSTILL, 269 | false, 270 | customId); 271 | } 272 | 273 | MC_ErrorCode AxisMove::addMovePosCont( 274 | FunctionBlock* fb, 275 | double pos, 276 | double vel, 277 | double acc, 278 | double dec, 279 | double endVel, 280 | double jerk, 281 | MC_ShiftingMode shiftingMode, 282 | MC_Direction dir, 283 | MC_BufferMode bufferMode, 284 | int32_t customId) 285 | { 286 | if(!endVel || !vel) 287 | return MC_ERRORCODE_VELILLEGAL; 288 | 289 | return mImpl_->addMove( 290 | fb, 291 | pos, 292 | vel, 293 | acc, 294 | dec, 295 | endVel, 296 | jerk, 297 | shiftingMode, 298 | dir, 299 | bufferMode, 300 | MC_AXISSTATUS_CONTINUOUSMOTION, 301 | MC_AXISSTATUS_CONTINUOUSMOTION, 302 | true, 303 | customId); 304 | } 305 | 306 | MC_ErrorCode AxisMove::addMoveVel( 307 | FunctionBlock* fb, 308 | double vel, 309 | double acc, 310 | double dec, 311 | double jerk, 312 | MC_BufferMode bufferMode, 313 | int32_t customId) 314 | { 315 | if(!vel) 316 | return MC_ERRORCODE_VELILLEGAL; 317 | 318 | return mImpl_->addMove( 319 | fb, 320 | NAN, 321 | vel, 322 | acc, 323 | dec, 324 | vel, 325 | jerk, 326 | MC_SHIFTINGMODE_ABSOLUTE, 327 | MC_DIRECTION_CURRENT, 328 | bufferMode, 329 | MC_AXISSTATUS_CONTINUOUSMOTION, 330 | MC_AXISSTATUS_CONTINUOUSMOTION, 331 | true, 332 | customId); 333 | } 334 | 335 | MC_ErrorCode AxisMove::addHalt( 336 | FunctionBlock* fb, 337 | double dec, 338 | double jerk, 339 | MC_BufferMode bufferMode, 340 | int32_t customId) 341 | { 342 | return mImpl_->addMove( 343 | fb, 344 | NAN, 345 | __EPSILON, 346 | dec, 347 | dec, 348 | 0, 349 | jerk, 350 | MC_SHIFTINGMODE_ABSOLUTE, 351 | MC_DIRECTION_CURRENT, 352 | bufferMode, 353 | MC_AXISSTATUS_DISCRETEMOTION, 354 | MC_AXISSTATUS_STANDSTILL, 355 | false, 356 | customId); 357 | } 358 | 359 | MC_ErrorCode AxisMove::addStop( 360 | FunctionBlock* fb, 361 | double dec, 362 | double jerk, 363 | int32_t customId) 364 | { 365 | return mImpl_->addMove( 366 | fb, 367 | NAN, 368 | __EPSILON, 369 | dec, 370 | dec, 371 | 0, 372 | jerk, 373 | MC_SHIFTINGMODE_ABSOLUTE, 374 | MC_DIRECTION_CURRENT, 375 | MC_BUFFERMODE_ABORTING, 376 | MC_AXISSTATUS_STOPPING, 377 | MC_AXISSTATUS_STOPPING, 378 | false, 379 | customId); 380 | } 381 | 382 | void AxisMove::cancelStopLater(void) 383 | { 384 | if(status() != MC_AXISSTATUS_STOPPING) 385 | return; 386 | 387 | if(!operationRemains()) { 388 | setStatus(MC_AXISSTATUS_STANDSTILL); 389 | } else { 390 | AxisExeclNode* node = dynamic_cast(front()); 391 | if(!node) 392 | return; 393 | 394 | if(node->mStatusDone == MC_AXISSTATUS_STOPPING) 395 | node->mStatusDone = MC_AXISSTATUS_STANDSTILL; 396 | } 397 | } 398 | 399 | void AxisMove::onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus) 400 | { 401 | AxisMove* this__ = dynamic_cast(this_); 402 | if(powerStatus) 403 | this__->mImpl_->mPlanner.setFrequency(this__->frequency()); 404 | } 405 | 406 | void AxisMove::onPositionOffsetHandler(AxisBase* this_, double positionOffset) 407 | { 408 | AxisMove* this__ = dynamic_cast(this_); 409 | this__->mImpl_->mPlanner.setPositionOffset(positionOffset); 410 | } 411 | 412 | void AxisMove::onAllNodesAbortedHandler(ExeclQueue* this_) 413 | { 414 | //AxisMove* this__ = dynamic_cast(this_); 415 | } 416 | 417 | void AxisMove::onAllNodesErrorHandler( 418 | ExeclQueue* this_, MC_ErrorCode errorCodeToSet) 419 | { 420 | //AxisMove* this__ = dynamic_cast(this_); 421 | } 422 | 423 | } 424 | -------------------------------------------------------------------------------- /motion/axis/AxisMove.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisMove.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_AXISMOVE_HPP_ 26 | #define _URANUS_AXISMOVE_HPP_ 27 | 28 | #include "AxisMotionBase.hpp" 29 | 30 | namespace Uranus { 31 | 32 | class AxisMove : virtual public AxisMotionBase 33 | { 34 | public: 35 | AxisMove(); 36 | virtual ~AxisMove(); 37 | 38 | MC_ErrorCode addMovePos( 39 | FunctionBlock* fb, 40 | double pos, 41 | double vel, 42 | double acc, 43 | double dec, 44 | double jerk, 45 | MC_ShiftingMode shiftingMode = MC_SHIFTINGMODE_ABSOLUTE, 46 | MC_Direction dir = MC_DIRECTION_CURRENT, 47 | MC_BufferMode bufferMode = MC_BUFFERMODE_ABORTING, 48 | int32_t customId = 0); 49 | 50 | MC_ErrorCode addMovePosCont( 51 | FunctionBlock* fb, 52 | double pos, 53 | double vel, 54 | double acc, 55 | double dec, 56 | double endVel, 57 | double jerk, 58 | MC_ShiftingMode shiftingMode = MC_SHIFTINGMODE_ABSOLUTE, 59 | MC_Direction dir = MC_DIRECTION_CURRENT, 60 | MC_BufferMode bufferMode = MC_BUFFERMODE_ABORTING, 61 | int32_t customId = 0); 62 | 63 | MC_ErrorCode addMoveVel( 64 | FunctionBlock* fb, 65 | double vel, 66 | double acc, 67 | double dec, 68 | double jerk, 69 | MC_BufferMode bufferMode = MC_BUFFERMODE_ABORTING, 70 | int32_t customId = 0); 71 | 72 | MC_ErrorCode addHalt( 73 | FunctionBlock* fb, 74 | double dec, 75 | double jerk, 76 | MC_BufferMode bufferMode = MC_BUFFERMODE_ABORTING, 77 | int32_t customId = 0); 78 | 79 | MC_ErrorCode addStop( 80 | FunctionBlock* fb, 81 | double dec, 82 | double jerk, 83 | int32_t customId = 0); 84 | 85 | void cancelStopLater(void); 86 | 87 | private: 88 | static void onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus); 89 | static void onPositionOffsetHandler(AxisBase* this_, double positionOffset); 90 | static void onAllNodesAbortedHandler(ExeclQueue* this_); 91 | static void onAllNodesErrorHandler(ExeclQueue* this_, MC_ErrorCode errorCodeToSet); 92 | 93 | private: 94 | class AxisMoveImpl; 95 | AxisMoveImpl* mImpl_; 96 | friend class MoveNode; 97 | }; 98 | 99 | } 100 | #endif /** _URANUS_AXISMOVE_HPP_ **/ 101 | -------------------------------------------------------------------------------- /motion/axis/AxisStatus.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisStatus.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "AxisStatus.hpp" 26 | #include 27 | namespace Uranus { 28 | 29 | class AxisStatus::AxisStatusImpl 30 | { 31 | public: 32 | MC_ErrorCode statusToError(void); 33 | 34 | MC_AxisStatus mStatus = MC_AXISSTATUS_DISABLED; 35 | }; 36 | 37 | MC_ErrorCode AxisStatus::AxisStatusImpl::statusToError(void) 38 | { 39 | switch(mStatus) { 40 | case MC_AXISSTATUS_DISABLED: 41 | return MC_ERRORCODE_AXISDISABLED; 42 | case MC_AXISSTATUS_STANDSTILL: 43 | return MC_ERRORCODE_AXISSTANDSTILL; 44 | case MC_AXISSTATUS_HOMING: 45 | return MC_ERRORCODE_AXISHOMING; 46 | case MC_AXISSTATUS_DISCRETEMOTION: 47 | return MC_ERRORCODE_AXISDISCRETEMOTION; 48 | case MC_AXISSTATUS_CONTINUOUSMOTION: 49 | return MC_ERRORCODE_AXISCONTINUOUSMOTION; 50 | case MC_AXISSTATUS_SYNCHRONIZEDMOTION: 51 | return MC_ERRORCODE_AXISSYNCHRONIZEDMOTION; 52 | case MC_AXISSTATUS_STOPPING: 53 | return MC_ERRORCODE_AXISSTOPPING; 54 | case MC_AXISSTATUS_ERRORSTOP: 55 | return MC_ERRORCODE_AXISERRORSTOP; 56 | } 57 | 58 | return MC_ERRORCODE_AXISDISABLED; 59 | } 60 | 61 | AxisStatus::AxisStatus() 62 | { 63 | mImpl_ = new AxisStatusImpl(); 64 | URANUS_ADD_HANDLER(onError, onErrorHandler); 65 | URANUS_ADD_HANDLER(onPowerStatusChanged, onPowerStatusChangedHandler); 66 | } 67 | 68 | AxisStatus::~AxisStatus() 69 | { 70 | delete mImpl_; 71 | } 72 | 73 | MC_AxisStatus AxisStatus::status(void) 74 | { 75 | return mImpl_->mStatus; 76 | } 77 | 78 | MC_ErrorCode AxisStatus::testStatus(MC_AxisStatus status) 79 | { 80 | if(status == mImpl_->mStatus) 81 | return MC_ERRORCODE_GOOD; 82 | 83 | switch(mImpl_->mStatus) { 84 | case MC_AXISSTATUS_ERRORSTOP: 85 | case MC_AXISSTATUS_DISABLED: 86 | goto FAILED; 87 | 88 | case MC_AXISSTATUS_STANDSTILL: 89 | switch(status) { 90 | case MC_AXISSTATUS_DISCRETEMOTION: 91 | case MC_AXISSTATUS_CONTINUOUSMOTION: 92 | case MC_AXISSTATUS_SYNCHRONIZEDMOTION: 93 | case MC_AXISSTATUS_STOPPING: 94 | case MC_AXISSTATUS_HOMING: 95 | goto SUCCESS; 96 | default: 97 | goto FAILED; 98 | } 99 | 100 | case MC_AXISSTATUS_DISCRETEMOTION: 101 | case MC_AXISSTATUS_CONTINUOUSMOTION: 102 | case MC_AXISSTATUS_SYNCHRONIZEDMOTION: 103 | switch(status) { 104 | case MC_AXISSTATUS_DISCRETEMOTION: 105 | case MC_AXISSTATUS_CONTINUOUSMOTION: 106 | case MC_AXISSTATUS_SYNCHRONIZEDMOTION: 107 | case MC_AXISSTATUS_STANDSTILL: 108 | case MC_AXISSTATUS_STOPPING: 109 | goto SUCCESS; 110 | default: 111 | goto FAILED; 112 | } 113 | 114 | case MC_AXISSTATUS_HOMING: 115 | switch(status) { 116 | case MC_AXISSTATUS_STANDSTILL: 117 | case MC_AXISSTATUS_STOPPING: 118 | goto SUCCESS; 119 | default: 120 | goto FAILED; 121 | } 122 | 123 | case MC_AXISSTATUS_STOPPING: 124 | switch(status) { 125 | case MC_AXISSTATUS_STANDSTILL: 126 | goto SUCCESS; 127 | default: 128 | goto FAILED; 129 | } 130 | 131 | default: 132 | goto FAILED; 133 | } 134 | 135 | SUCCESS: 136 | return MC_ERRORCODE_GOOD; 137 | 138 | FAILED: 139 | return mImpl_->statusToError(); 140 | } 141 | 142 | MC_ErrorCode AxisStatus::setStatus(MC_AxisStatus status) 143 | { 144 | MC_ErrorCode err = testStatus(status); 145 | if(err) return err; 146 | 147 | mImpl_->mStatus = status; 148 | return MC_ERRORCODE_GOOD; 149 | } 150 | 151 | void AxisStatus::onErrorHandler(AxisBase* this_, MC_ErrorCode errorCode) 152 | { 153 | AxisStatus* this__ = dynamic_cast(this_); 154 | this__->mImpl_->mStatus = MC_AXISSTATUS_ERRORSTOP; 155 | } 156 | 157 | void AxisStatus::onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus) 158 | { 159 | AxisStatus* this__ = dynamic_cast(this_); 160 | this__->mImpl_->mStatus = 161 | powerStatus? MC_AXISSTATUS_STANDSTILL: MC_AXISSTATUS_DISABLED; 162 | } 163 | 164 | } 165 | -------------------------------------------------------------------------------- /motion/axis/AxisStatus.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AxisStatus.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_AXISSTATUS_HPP_ 26 | #define _URANUS_AXISSTATUS_HPP_ 27 | 28 | #include "AxisBase.hpp" 29 | 30 | namespace Uranus { 31 | 32 | class AxisStatus : virtual public AxisBase 33 | { 34 | public: 35 | AxisStatus(); 36 | virtual ~AxisStatus(); 37 | 38 | MC_AxisStatus status(void); 39 | MC_ErrorCode setStatus(MC_AxisStatus status); 40 | MC_ErrorCode testStatus(MC_AxisStatus status); 41 | 42 | private: 43 | static void onErrorHandler(AxisBase* this_, MC_ErrorCode errorCode); 44 | static void onPowerStatusChangedHandler(AxisBase* this_, bool powerStatus); 45 | 46 | private: 47 | class AxisStatusImpl; 48 | AxisStatusImpl* mImpl_; 49 | }; 50 | 51 | } 52 | 53 | #endif /** _URANUS_AXISSTATUS_HPP_ **/ 54 | 55 | -------------------------------------------------------------------------------- /motion/utils/MathUtils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MathUtils.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_MATHUTILS_HPP_ 26 | #define _URANUS_MATHUTILS_HPP_ 27 | 28 | #include 29 | 30 | namespace Uranus { 31 | 32 | #ifndef __square 33 | #define __square(x) ((x)*(x)) 34 | #endif 35 | 36 | #ifndef __cube 37 | #define __cube(x) (__square(x)*(x)) 38 | #endif 39 | 40 | #ifndef __EPSILON 41 | #define __EPSILON 0.00001 42 | #endif 43 | 44 | #ifndef __iszero 45 | #define __iszero(x) (fabs(x) < __EPSILON) 46 | #endif 47 | 48 | #ifndef __isnotzero 49 | #define __isnotzero(x) (fabs(x) > __EPSILON) 50 | #endif 51 | 52 | #ifndef __iseq 53 | #define __iseq(a,b) __iszero((a)-(b)) 54 | #endif 55 | 56 | #ifndef __isne 57 | #define __isne(a,b) __isnotzero((a)-(b)) 58 | #endif 59 | 60 | #ifndef __isne 61 | #define __isne(a,b) __isnotzero((a)-(b)) 62 | #endif 63 | 64 | #ifndef __isgt 65 | #define __isgt(a,b)((a) > (b) + __EPSILON) 66 | #endif 67 | 68 | #ifndef __isls 69 | #define __isls(a,b)((a) < (b) - __EPSILON) 70 | #endif 71 | 72 | inline bool isOpposite(double num1, double num2) { 73 | if(!num1 || !num2) 74 | return false; 75 | else 76 | return (num1<0) != (num2<0); 77 | } 78 | 79 | } 80 | 81 | #endif /** _URANUS_MATHUTILS_HPP_ **/ 82 | -------------------------------------------------------------------------------- /motion/utils/ProfilePlanner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ProfilePlanner.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | /* lagacy code */ 26 | 27 | #include "ProfilePlanner.hpp" 28 | #include 29 | #include 30 | #include 31 | #include "MathUtils.hpp" 32 | 33 | //#define MC_DEBUG 1 34 | 35 | namespace Uranus 36 | { 37 | 38 | typedef ProfilePlanner::Segment Segment; 39 | typedef ProfilePlanner::ProfilePlannerData ProfilePlannerData; 40 | 41 | static int route_calculate(Segment* segments, 42 | double shift, double start_vel, double vel, 43 | double acc, double dec, double& end_vel); 44 | 45 | static bool route_param_pre_process( 46 | double& vel, double& acc, double& dec, double& end_vel); 47 | 48 | static int route_verify_and_shift( 49 | Segment* segments, double end_vel, double pos_offset); 50 | 51 | static bool check_results(const Segment* segments, int length); 52 | 53 | static void route_discretization( 54 | Segment* segments, int length, uint32_t frequency, double& prev_t_remain); 55 | 56 | static int tiny_segment_merge( 57 | Segment* segments, int length, uint32_t frequency, int32_t& new_start_tick); 58 | 59 | static double cal_shift( 60 | double start_vel, double end_vel, double acc, double* t); 61 | 62 | static inline bool is_acc_neg(double start_vel, double end_vel); 63 | 64 | void print_all(Segment* segments, int num) 65 | { 66 | for(int i=0; istart_position = (StartPos); \ 86 | (Segment)->end_position = (EndPos); \ 87 | (Segment)->start_vel = (StartVel); \ 88 | (Segment)->acc = (Acc); (Segment)->t = (T); } 89 | 90 | ProfilePlanner::ProfilePlanner() 91 | { 92 | memset(this, 0, sizeof(ProfilePlanner)); 93 | frequency = 1000; 94 | } 95 | 96 | bool ProfilePlanner::setFrequency(uint32_t _frequency) 97 | { 98 | if(!_frequency || _frequency > 100000) 99 | return false; 100 | 101 | frequency = _frequency; 102 | return true; 103 | } 104 | 105 | double ProfilePlanner::limitStartVel(double dist, double start_vel, double end_vel, double dec) 106 | { 107 | if(start_vel == 0.0) 108 | return start_vel; 109 | 110 | if(((start_vel<0.0) != (end_vel<0.0)) && (end_vel != 0.0)) 111 | return start_vel; 112 | 113 | double vel_limit = sqrt(2 * fabs(dist) * fabs(dec) + __square(end_vel)); 114 | 115 | if(vel_limit < fabs(start_vel)) 116 | start_vel = (start_vel>0.0)? vel_limit: -vel_limit; 117 | 118 | return start_vel; 119 | } 120 | 121 | double ProfilePlanner::calculateDist(double start_vel, double end_vel, double acc, double dec) 122 | { 123 | acc = fabs(acc); 124 | dec = fabs(dec); 125 | 126 | if(isOpposite(start_vel, end_vel)) 127 | return (start_vel * fabs(start_vel) / dec + end_vel * fabs(end_vel) / acc) * 0.5; 128 | else 129 | { 130 | double cc = (fabs(start_vel) > fabs(end_vel))? dec: acc; 131 | return (start_vel + end_vel) * fabs(start_vel - end_vel) / cc * 0.5; 132 | } 133 | } 134 | 135 | bool ProfilePlanner::plan( 136 | double start_position, double end_position, 137 | double start_vel, double vel, double end_vel, 138 | double acc, double dec) 139 | { 140 | double shift; 141 | int route_calculate_result; 142 | 143 | if(__iseq(start_position, end_position) && __iseq(start_vel, end_vel)) 144 | { 145 | data.number_segment = 0; 146 | goto EXIT; 147 | } 148 | 149 | if(!route_param_pre_process(vel, acc, dec, end_vel)) 150 | return false; 151 | 152 | #ifdef MC_DEBUG 153 | printf( 154 | "get a cmd: pos %lf -> %lf, vel %lf -> %lf -> %lf, acc: %lf, dec: %lf\n", 155 | start_position, end_position, start_vel, vel, end_vel, acc, dec); 156 | #endif 157 | 158 | pushData(); 159 | 160 | data.current_tick = 0; 161 | data.current_segment = 0; 162 | memset(data.segments, 0, sizeof(Segment) * MAX_ROUTE_SEGMENT_NUM); 163 | shift = end_position - start_position; 164 | 165 | route_calculate_result = route_calculate(data.segments, shift, start_vel, vel, acc, dec, end_vel); 166 | 167 | switch(route_calculate_result) 168 | { 169 | case 0: //all success 170 | case 1: //real end_vel < given end_vel 171 | break; 172 | case 2: //real end_vel > given end_vel 173 | popData(); 174 | return false; 175 | } 176 | 177 | data.number_segment = 178 | route_verify_and_shift(data.segments, end_vel, start_position); 179 | 180 | if(!check_results(data.segments, data.number_segment)) 181 | { 182 | popData(); 183 | return false; 184 | } 185 | 186 | if(data_backup.current_segment < data_backup.number_segment) 187 | data.t_remain = 1.0 / frequency; 188 | 189 | route_discretization(data.segments, data.number_segment, frequency, data.t_remain); 190 | 191 | data.number_segment = 192 | tiny_segment_merge(data.segments, data.number_segment, frequency, data.current_tick); 193 | 194 | #ifdef MC_DEBUG 195 | print_all(data.segments, data.number_segment); 196 | #endif 197 | 198 | EXIT: 199 | data.frequency = frequency; 200 | data.position = start_position; 201 | data.velocity = start_vel; 202 | 203 | input_info.start_position = start_position; 204 | input_info.end_position = end_position; 205 | input_info.start_vel = start_vel; 206 | input_info.end_vel = end_vel; 207 | 208 | if(!data.number_segment) 209 | return false; 210 | 211 | if(route_calculate_result == 1) 212 | return false; 213 | 214 | return true; 215 | } 216 | 217 | bool ProfilePlanner::execute(void) 218 | { 219 | if(data.current_segment >= data.number_segment) 220 | { 221 | data.velocity = input_info.end_vel; 222 | data.position += data.velocity / data.frequency; 223 | input_info.end_position = data.position; 224 | data.t_remain = 1.0 / data.frequency; 225 | return true; 226 | } 227 | 228 | double t = (double)(data.current_tick) / data.frequency; 229 | 230 | double acc_2 = data.segments[data.current_segment].acc * t / 2; 231 | 232 | data.velocity = data.segments[data.current_segment].start_vel + acc_2; 233 | data.position = data.segments[data.current_segment].start_position + data.velocity * t; 234 | data.velocity += acc_2; 235 | 236 | if(data.current_tick == data.segments[data.current_segment].tick && 237 | !data.segments[data.current_segment].magic_flags) 238 | { 239 | goto SEGMENT_SUCCESS; 240 | } 241 | else if(data.current_tick > data.segments[data.current_segment].tick) 242 | { 243 | data.position = data.segments[data.current_segment].end_position; 244 | 245 | if(data.segments[data.current_segment].magic_flags) 246 | { 247 | data.t_remain = 0.0; 248 | data.velocity = 0; 249 | } 250 | 251 | goto SEGMENT_SUCCESS; 252 | } 253 | ++data.current_tick; 254 | 255 | return false; 256 | 257 | SEGMENT_SUCCESS: 258 | data.current_tick = 0; 259 | 260 | if(++data.current_segment == data.number_segment) 261 | return true; 262 | else 263 | return false; 264 | } 265 | 266 | int ProfilePlanner::readStatus(void) 267 | { 268 | if((data.current_segment >= data.number_segment) || 269 | (data.segments[data.current_segment].acc == 0.0)) 270 | { 271 | if(data.velocity == 0.0) 272 | return 0; 273 | else 274 | return 1; 275 | } 276 | 277 | if((data.segments[data.current_segment].acc > 0.0) != (data.velocity > 0.0)) 278 | return 3; 279 | else 280 | return 2; 281 | } 282 | 283 | double ProfilePlanner::getPosition(void) 284 | { 285 | return data.position; 286 | } 287 | 288 | double ProfilePlanner::getVelocity(void) 289 | { 290 | return data.velocity; 291 | } 292 | 293 | double ProfilePlanner::getAcceleration(void) 294 | { 295 | if(data.current_segment >= data.number_segment) 296 | { 297 | return 0; 298 | } 299 | else 300 | { 301 | return data.segments[data.current_segment].acc; 302 | } 303 | } 304 | 305 | double ProfilePlanner::getStartPosition(void) 306 | { 307 | return input_info.start_position; 308 | } 309 | 310 | double ProfilePlanner::getEndPosition(void) 311 | { 312 | return input_info.end_position; 313 | } 314 | 315 | void ProfilePlanner::setPositionOffset(double pos) 316 | { 317 | if(data.current_segment >= data.number_segment) 318 | { 319 | data.position += pos; 320 | input_info.end_position = data.position; 321 | } 322 | } 323 | 324 | double ProfilePlanner::getStartVelocity(void) 325 | { 326 | return input_info.start_vel; 327 | } 328 | 329 | double ProfilePlanner::getEndVelocity(void) 330 | { 331 | return input_info.end_vel; 332 | } 333 | 334 | void ProfilePlanner::resetRemain(uint32_t freq) 335 | { 336 | data.t_remain = 1.0 / freq; 337 | } 338 | 339 | void ProfilePlanner::pushData(void) 340 | { 341 | data_backup = data; 342 | } 343 | 344 | void ProfilePlanner::popData(void) 345 | { 346 | data = data_backup; 347 | } 348 | 349 | void ProfilePlanner::popData(void); 350 | 351 | static int route_calculate(Segment* segments, 352 | double shift, double start_vel, double vel, 353 | double acc, double dec, double& end_vel) 354 | { 355 | int result = 0; 356 | double shift_tmp = shift; 357 | double end_vel_tmp = end_vel; 358 | double shift_pre = 0.0; 359 | 360 | #if 0 361 | //set velocity arrow 362 | if(isOpposite(start_vel, end_vel)) 363 | { 364 | double t_reverse_shift; 365 | double reverse_shift_point = 366 | cal_shift(start_vel, 0, dec, &t_reverse_shift) + 367 | cal_shift(end_vel, 0, acc, &t_reverse_shift); 368 | 369 | if(shift < reverse_shift_point) 370 | vel = -vel; 371 | } 372 | else 373 | { 374 | } 375 | #endif 376 | 377 | // imperfect 378 | if((shift < 0.0) || (!shift && (end_vel < start_vel))) 379 | vel = -vel; 380 | 381 | /** 382 | * if shift and start_vel are not at the same direction, 383 | * dec to 0 in advance. 384 | **/ 385 | if(isOpposite(shift_tmp, start_vel)) 386 | { 387 | double t; 388 | shift_pre = cal_shift(start_vel, 0.0, dec, &t); 389 | 390 | set_route_segment(segments, 391 | 0.0, shift_pre, start_vel, (shift_pre<0)? dec: -dec, t); 392 | 393 | start_vel = 0.0; 394 | shift_tmp -= shift_pre; 395 | } 396 | else 397 | { 398 | set_route_segment(segments, 0.0, 0.0, 0.0, 0.0, 0.0); 399 | } 400 | 401 | double v_uni = vel; 402 | double acc1 = 0.0, acc2 = 0.0, acc3 = 0.0; 403 | double t_acc1 = 0.0, t_acc2 = 0.0, t_acc3 = 0.0, t_uni = 0.0; 404 | double s_acc1 = 0.0, s_acc2 = 0.0, s_acc3 = 0.0, s_uni = 0.0; 405 | double s_sum; 406 | 407 | /** 408 | * if vel and end_vel are not at the same direction, 409 | * cal the last direction info in advance. 410 | **/ 411 | if(isOpposite(vel, end_vel)) 412 | { 413 | acc3 = is_acc_neg(vel, end_vel)? -acc: acc; 414 | s_acc3 = cal_shift(0.0, end_vel, acc3, &t_acc3); 415 | shift_tmp -= s_acc3; 416 | end_vel_tmp = 0.0; 417 | } 418 | 419 | //determine whether inputs param overtravel 420 | acc2 = (fabs(start_vel) > fabs(end_vel_tmp))? dec: acc; 421 | if(is_acc_neg(start_vel, end_vel_tmp)) {acc2 = -acc2;} 422 | s_acc2 = cal_shift(start_vel, end_vel_tmp, acc2, &t_acc2); 423 | 424 | if(__isne(s_acc2, shift_tmp) && (fabs(s_acc2) > fabs(shift_tmp))) 425 | { 426 | /** overtravel, change end_vel **/ 427 | end_vel_tmp = sqrt(__square(start_vel) + 2 * acc2 * shift_tmp); 428 | 429 | if(fabs(end_vel) > end_vel_tmp) 430 | result = 1; 431 | else 432 | result = 2; 433 | 434 | if(shift_tmp < 0.0) {end_vel_tmp = -end_vel_tmp;} 435 | end_vel = end_vel_tmp; 436 | //v_uni = start_vel; 喵喵喵？ 437 | t_acc2 = (end_vel - start_vel) / acc2; 438 | s_acc2 = shift_tmp; 439 | } 440 | 441 | /*****************************************/ 442 | if(fabs(start_vel) < fabs(vel)) 443 | acc1 = acc; 444 | else 445 | acc1 = dec; 446 | 447 | if(is_acc_neg(start_vel, vel)) 448 | acc1 = -acc1; 449 | /*****************************************/ 450 | if(fabs(vel) < fabs(end_vel_tmp)) 451 | acc2 = acc; 452 | else 453 | acc2 = dec; 454 | 455 | if(is_acc_neg(vel, end_vel_tmp)) 456 | acc2 = -acc2; 457 | /*****************************************/ 458 | 459 | s_acc1 = cal_shift(start_vel, vel, acc1, &t_acc1); 460 | s_acc2 = cal_shift(vel, end_vel_tmp, acc2, &t_acc2); 461 | 462 | s_sum = s_acc1 + s_acc2; 463 | 464 | // v_uni != vel, calculate new values 465 | if(fabs(s_sum) > fabs(shift_tmp) && __isne(s_sum, shift_tmp)) 466 | { 467 | v_uni = sqrt( 468 | ((2 * shift_tmp + (start_vel * start_vel / acc1) - 469 | (end_vel_tmp * end_vel_tmp / acc2)) * 470 | (acc1 * acc2)) / (acc2 - acc1)); 471 | 472 | if(vel < 0.0) {v_uni = -v_uni;} 473 | s_acc1 = cal_shift(start_vel, v_uni, acc1, &t_acc1); 474 | s_acc2 = cal_shift(v_uni, end_vel_tmp, acc2, &t_acc2); 475 | } 476 | else 477 | { 478 | s_uni = shift_tmp - s_sum; 479 | t_uni = fabs(s_uni / vel); 480 | } 481 | 482 | shift_tmp = shift_pre; 483 | 484 | set_route_segment(segments + 1, 485 | shift_tmp, shift_pre + s_acc1, start_vel, acc1, t_acc1); 486 | 487 | shift_tmp = shift_pre + s_acc1; 488 | 489 | set_route_segment(segments + 2, 490 | shift_tmp, shift_tmp + s_uni, v_uni, 0.0, t_uni); 491 | 492 | shift_tmp += s_uni; 493 | 494 | set_route_segment(segments + 3, 495 | shift_tmp, shift_tmp + s_acc2, v_uni, acc2, t_acc2); 496 | 497 | shift_tmp += s_acc2; 498 | 499 | set_route_segment(segments + 4, 500 | shift_tmp, shift, end_vel_tmp, acc3, t_acc3); 501 | 502 | //print_all(segments); 503 | 504 | return result; 505 | } 506 | 507 | static int route_verify_and_shift( 508 | Segment* segments, double end_vel, double pos_offset) 509 | { 510 | int num=0; 511 | 512 | for(int i=0; i 31 | 32 | namespace Uranus 33 | { 34 | 35 | #define MAX_ROUTE_SEGMENT_NUM 5 36 | 37 | class ProfilePlanner 38 | { 39 | public: 40 | typedef struct 41 | { 42 | double start_position; 43 | double end_position; 44 | double start_vel; 45 | double end_vel; 46 | }InputInfo; 47 | 48 | typedef struct 49 | { 50 | double start_position; 51 | double end_position; 52 | double start_vel; 53 | double acc; 54 | double t; 55 | int32_t tick; 56 | int magic_flags; 57 | }Segment; 58 | 59 | typedef struct 60 | { 61 | double position; 62 | double velocity; 63 | int32_t current_tick; 64 | uint32_t frequency; 65 | uint32_t current_segment; 66 | uint32_t number_segment; 67 | double t_remain; 68 | Segment segments[MAX_ROUTE_SEGMENT_NUM]; 69 | }ProfilePlannerData; 70 | 71 | public: 72 | ProfilePlannerData data; 73 | ProfilePlannerData data_backup; 74 | InputInfo input_info; 75 | uint32_t frequency; 76 | 77 | public: 78 | ProfilePlanner(); 79 | virtual ~ProfilePlanner(){} 80 | 81 | bool setFrequency(uint32_t frequency); 82 | 83 | static double limitStartVel(double dist, double start_vel, double end_vel, double dec); 84 | 85 | static double calculateDist(double start_vel, double end_vel, double acc, double dec); 86 | 87 | bool plan( 88 | double start_position, double end_position, 89 | double start_vel, double vel, double end_vel, 90 | double acc, double dec); 91 | 92 | bool execute(void); 93 | 94 | /** 95 | * return 96 | * 0: standstill 97 | * 1: constant velocity 98 | * 2: accelerating 99 | * 3: decelerating 100 | **/ 101 | int readStatus(void); 102 | 103 | double getPosition(void); 104 | 105 | double getVelocity(void); 106 | 107 | double getAcceleration(void); 108 | 109 | double getStartPosition(void); 110 | 111 | double getEndPosition(void); 112 | 113 | double getStartVelocity(void); 114 | 115 | double getEndVelocity(void); 116 | 117 | void resetRemain(uint32_t freq); 118 | 119 | void setPositionOffset(double pos); 120 | 121 | private: 122 | void pushData(void); 123 | 124 | void popData(void); 125 | }; 126 | 127 | void print_all(ProfilePlanner::Segment* segments, int num = 5); 128 | 129 | } 130 | 131 | #endif /** _URANUS_PROFILEPLANNER_HPP_ **/ 132 | -------------------------------------------------------------------------------- /motion/utils/ProfilesPlanner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ProfilesPlanner.cpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #include "ProfilesPlanner.hpp" 26 | #include "ProfilePlanner.hpp" 27 | #include "MathUtils.hpp" 28 | 29 | namespace Uranus { 30 | 31 | class ProfilesPlanner::ProfilesPlannerImpl 32 | { 33 | public: 34 | int mPlanRet = -1; 35 | double mStartPos = 0; 36 | double mStartVel = 0; 37 | double mStartAcc = 0; //reserved 38 | }; 39 | 40 | ProfilesPlanner::ProfilesPlanner() 41 | { 42 | mImpl_ = new ProfilesPlannerImpl(); 43 | } 44 | 45 | ProfilesPlanner::~ProfilesPlanner() 46 | { 47 | delete mImpl_; 48 | } 49 | 50 | bool ProfilesPlanner::plan( 51 | ProfileNode* node, double startPos, double startVel, double startAcc) 52 | { 53 | return ProfilePlanner::plan( 54 | startPos, node->mEndPos, 55 | startVel, node->mVel, node->mEndVel, 56 | node->mAcc, node->mDec); 57 | } 58 | 59 | }; 60 | -------------------------------------------------------------------------------- /motion/utils/ProfilesPlanner.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ProfilesPlanner.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_PROFILESPLANNER_HPP_ 26 | #define _URANUS_PROFILESPLANNER_HPP_ 27 | 28 | #include "ExeclQueue.hpp" 29 | #include "ProfilePlanner.hpp" 30 | 31 | namespace Uranus { 32 | 33 | class ProfileNode : virtual public ExeclNode 34 | { 35 | public: 36 | double mStartPos = 0; 37 | double mStartVel = 0; 38 | double mStartAcc = 0; //保留 39 | 40 | double mEndPos = 0; 41 | double mEndVel = 0; 42 | double mEndAcc = 0; //保留 43 | 44 | double mVel = 0; 45 | double mAcc = 0; 46 | double mDec = 0; 47 | double mJerk = 0; 48 | 49 | friend class ProfilesPlanner; 50 | }; 51 | 52 | class ProfilesPlanner : public ProfilePlanner 53 | { 54 | public: 55 | ProfilesPlanner(); 56 | virtual ~ProfilesPlanner(); 57 | 58 | bool plan( 59 | ProfileNode* node, 60 | double startPos, 61 | double startVel, 62 | double startAcc); 63 | 64 | private: 65 | class ProfilesPlannerImpl; 66 | ProfilesPlannerImpl* mImpl_; 67 | }; 68 | 69 | } 70 | 71 | #endif /** _URANUS_PROFILESPLANNER_HPP_ **/ 72 | --------------------------------------------------------------------------------