├── .gitattributes ├── .gitignore ├── MotionControl.pro ├── MotionControl.pro.user ├── MotionControler.cpp ├── MotionControler.h ├── README.TXT └── main.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear in the root of a volume 35 | .DocumentRevisions-V100 36 | .fseventsd 37 | .Spotlight-V100 38 | .TemporaryItems 39 | .Trashes 40 | .VolumeIcon.icns 41 | 42 | # Directories potentially created on remote AFP share 43 | .AppleDB 44 | .AppleDesktop 45 | Network Trash Folder 46 | Temporary Items 47 | .apdisk 48 | -------------------------------------------------------------------------------- /MotionControl.pro: -------------------------------------------------------------------------------- 1 | QT += core 2 | QT -= gui 3 | 4 | TARGET = MotionControl 5 | CONFIG += console 6 | CONFIG -= app_bundle 7 | 8 | TEMPLATE = app 9 | 10 | SOURCES += main.cpp \ 11 | MotionControler.cpp 12 | 13 | HEADERS += \ 14 | MotionControler.h 15 | 16 | -------------------------------------------------------------------------------- /MotionControl.pro.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | EnvironmentId 7 | {65d20304-a93b-4827-8c76-30e535fa41cb} 8 | 9 | 10 | ProjectExplorer.Project.ActiveTarget 11 | 1 12 | 13 | 14 | ProjectExplorer.Project.EditorSettings 15 | 16 | true 17 | false 18 | true 19 | 20 | Cpp 21 | 22 | CppGlobal 23 | 24 | 25 | 26 | QmlJS 27 | 28 | QmlJSGlobal 29 | 30 | 31 | 2 32 | UTF-8 33 | false 34 | 4 35 | false 36 | 80 37 | true 38 | true 39 | 1 40 | true 41 | false 42 | 0 43 | true 44 | 0 45 | 8 46 | true 47 | 0 48 | true 49 | true 50 | true 51 | false 52 | 53 | 54 | 55 | ProjectExplorer.Project.PluginSettings 56 | 57 | 58 | 59 | ProjectExplorer.Project.Target.0 60 | 61 | Desktop Qt 5.5.0 MSVC2013 64bit 62 | Desktop Qt 5.5.0 MSVC2013 64bit 63 | qt.55.win64_msvc2013_64_kit 64 | 1 65 | 0 66 | 0 67 | 68 | F:/work/B_Spline/Qt_project/build-MotionControl-Desktop_Qt_5_5_0_MSVC2013_64bit-Debug 69 | 70 | 71 | true 72 | qmake 73 | 74 | QtProjectManager.QMakeBuildStep 75 | false 76 | true 77 | 78 | false 79 | false 80 | false 81 | 82 | 83 | true 84 | Make 85 | 86 | Qt4ProjectManager.MakeStep 87 | 88 | false 89 | -j8 install 90 | 91 | 92 | 2 93 | 构建 94 | 95 | ProjectExplorer.BuildSteps.Build 96 | 97 | 98 | 99 | true 100 | Make 101 | 102 | Qt4ProjectManager.MakeStep 103 | 104 | true 105 | clean 106 | 107 | 108 | 1 109 | 清理 110 | 111 | ProjectExplorer.BuildSteps.Clean 112 | 113 | 2 114 | false 115 | 116 | Debug 117 | 118 | Qt4ProjectManager.Qt4BuildConfiguration 119 | 2 120 | true 121 | 122 | 123 | F:/work/B_Spline/Qt_project/build-MotionControl-Desktop_Qt_5_5_0_MSVC2013_64bit-Release 124 | 125 | 126 | true 127 | qmake 128 | 129 | QtProjectManager.QMakeBuildStep 130 | false 131 | true 132 | 133 | false 134 | false 135 | false 136 | 137 | 138 | true 139 | Make 140 | 141 | Qt4ProjectManager.MakeStep 142 | 143 | false 144 | 145 | 146 | 147 | 2 148 | 构建 149 | 150 | ProjectExplorer.BuildSteps.Build 151 | 152 | 153 | 154 | true 155 | Make 156 | 157 | Qt4ProjectManager.MakeStep 158 | 159 | true 160 | clean 161 | 162 | 163 | 1 164 | 清理 165 | 166 | ProjectExplorer.BuildSteps.Clean 167 | 168 | 2 169 | false 170 | 171 | Release 172 | 173 | Qt4ProjectManager.Qt4BuildConfiguration 174 | 0 175 | true 176 | 177 | 2 178 | 179 | 180 | 0 181 | 部署 182 | 183 | ProjectExplorer.BuildSteps.Deploy 184 | 185 | 1 186 | 在本地部署 187 | 188 | ProjectExplorer.DefaultDeployConfiguration 189 | 190 | 1 191 | 192 | 193 | 194 | false 195 | false 196 | false 197 | false 198 | true 199 | 0.01 200 | 10 201 | true 202 | 1 203 | 25 204 | 205 | 1 206 | true 207 | false 208 | true 209 | valgrind 210 | 211 | 0 212 | 1 213 | 2 214 | 3 215 | 4 216 | 5 217 | 6 218 | 7 219 | 8 220 | 9 221 | 10 222 | 11 223 | 12 224 | 13 225 | 14 226 | 227 | 2 228 | 229 | MotionControl 230 | 231 | Qt4ProjectManager.Qt4RunConfiguration:F:/work/B_Spline/Qt_project/MotionControl/MotionControl.pro 232 | 233 | MotionControl.pro 234 | false 235 | true 236 | 237 | 3768 238 | false 239 | true 240 | false 241 | false 242 | true 243 | 244 | 1 245 | 246 | 247 | 248 | ProjectExplorer.Project.Target.1 249 | 250 | Desktop Qt 5.5.0 MinGW 32bit 251 | Desktop Qt 5.5.0 MinGW 32bit 252 | qt.55.win32_mingw492_kit 253 | 0 254 | 0 255 | 0 256 | 257 | F:/work/B_Spline/Qt_project/build-MotionControl-Desktop_Qt_5_5_0_MinGW_32bit-Debug 258 | 259 | 260 | true 261 | qmake 262 | 263 | QtProjectManager.QMakeBuildStep 264 | false 265 | true 266 | 267 | false 268 | false 269 | false 270 | 271 | 272 | true 273 | Make 274 | 275 | Qt4ProjectManager.MakeStep 276 | 277 | false 278 | 279 | 280 | 281 | 2 282 | 构建 283 | 284 | ProjectExplorer.BuildSteps.Build 285 | 286 | 287 | 288 | true 289 | Make 290 | 291 | Qt4ProjectManager.MakeStep 292 | 293 | true 294 | clean 295 | 296 | 297 | 1 298 | 清理 299 | 300 | ProjectExplorer.BuildSteps.Clean 301 | 302 | 2 303 | false 304 | 305 | Debug 306 | 307 | Qt4ProjectManager.Qt4BuildConfiguration 308 | 2 309 | true 310 | 311 | 312 | F:/work/B_Spline/Qt_project/build-MotionControl-Desktop_Qt_5_5_0_MinGW_32bit-Release 313 | 314 | 315 | true 316 | qmake 317 | 318 | QtProjectManager.QMakeBuildStep 319 | false 320 | true 321 | 322 | false 323 | false 324 | false 325 | 326 | 327 | true 328 | Make 329 | 330 | Qt4ProjectManager.MakeStep 331 | 332 | false 333 | 334 | 335 | 336 | 2 337 | 构建 338 | 339 | ProjectExplorer.BuildSteps.Build 340 | 341 | 342 | 343 | true 344 | Make 345 | 346 | Qt4ProjectManager.MakeStep 347 | 348 | true 349 | clean 350 | 351 | 352 | 1 353 | 清理 354 | 355 | ProjectExplorer.BuildSteps.Clean 356 | 357 | 2 358 | false 359 | 360 | Release 361 | 362 | Qt4ProjectManager.Qt4BuildConfiguration 363 | 0 364 | true 365 | 366 | 2 367 | 368 | 369 | 0 370 | 部署 371 | 372 | ProjectExplorer.BuildSteps.Deploy 373 | 374 | 1 375 | 在本地部署 376 | 377 | ProjectExplorer.DefaultDeployConfiguration 378 | 379 | 1 380 | 381 | 382 | 383 | false 384 | false 385 | false 386 | false 387 | true 388 | 0.01 389 | 10 390 | true 391 | 1 392 | 25 393 | 394 | 1 395 | true 396 | false 397 | true 398 | valgrind 399 | 400 | 0 401 | 1 402 | 2 403 | 3 404 | 4 405 | 5 406 | 6 407 | 7 408 | 8 409 | 9 410 | 10 411 | 11 412 | 12 413 | 13 414 | 14 415 | 416 | 2 417 | 418 | MotionControl 419 | 420 | Qt4ProjectManager.Qt4RunConfiguration:F:/work/B_Spline/Qt_project/MotionControl/MotionControl.pro 421 | 422 | MotionControl.pro 423 | false 424 | true 425 | 426 | 3768 427 | false 428 | true 429 | false 430 | false 431 | true 432 | 433 | 1 434 | 435 | 436 | 437 | ProjectExplorer.Project.TargetCount 438 | 2 439 | 440 | 441 | ProjectExplorer.Project.Updater.FileVersion 442 | 18 443 | 444 | 445 | Version 446 | 18 447 | 448 | 449 | -------------------------------------------------------------------------------- /MotionControler.cpp: -------------------------------------------------------------------------------- 1 | #include "MotionControler.h" 2 | #include 3 | #include 4 | #include 5 | 6 | MotionControler::MotionControler() 7 | { 8 | /* 初始状态参数配置 */ 9 | axisNum = 1; 10 | currentPos = QVector(axisNum + 1, 0); 11 | currentVelocity = QVector(axisNum + 1, 0); 12 | currentAcceloration = QVector(axisNum + 1, 0); 13 | currentVelocityTCP = 0; 14 | currentAccelorationTCP = 0; 15 | 16 | endPos = QVector(axisNum + 1, 0); 17 | endVelocity = QVector(axisNum + 1, 0); 18 | endVelocityTCP = 0; 19 | 20 | remainPos = QVector(axisNum + 1, 0); 21 | remainPosTCP = 0; 22 | T = 1e-3; 23 | 24 | multiper = QVector(axisNum + 1, 1); 25 | origin = QVector(axisNum + 1, 0); 26 | error = 0.01; 27 | status = free; 28 | 29 | velocityLimitTCP = 0; 30 | accelorationLimitTCP = 0; 31 | 32 | interpolationCounter = 0; 33 | timer = new QTimer(); 34 | // connect(timer, SIGNAL(timeout()), this, SLOT(SendCurrentPos())); 35 | // timer->start(1); 36 | } 37 | /*! 38 | * \brief MotionControler::TPlan 指定期望时间的T形速度规划,相对运动方式 39 | * \return 插补距离 40 | */ 41 | QVector MotionControler::TPlanBaseTime(qreal time) 42 | { 43 | int numOfInterpolation = (int) (time / T); /* 插补点个数为总时间除以插补周期 */ 44 | QVector pos(numOfInterpolation); /* 定义数组长度为numOfInterpolation */ 45 | bool succeed = false; /* 规划成功标志 */ 46 | int j = 0; 47 | float delta; /* 解一元二次方程中的解的判据 */ 48 | float V1; /* 匀速段的速度 */ 49 | float V[2]; 50 | float t1; /* 匀速段开始时间 */ 51 | float t2; /* 匀速段结束时间 */ 52 | while (j <= 4){ 53 | j++; 54 | /*情况1：加速度-匀速-减速先假设后验证*/ 55 | delta = 4 * (accelorationLimitTCP * time + currentVelocityTCP + endVelocityTCP) 56 | * (accelorationLimitTCP * time + currentVelocityTCP + endVelocityTCP) 57 | - 8 * (currentVelocityTCP * currentVelocityTCP + endVelocityTCP * endVelocityTCP + 2 * accelorationLimitTCP * remainPosTCP); 58 | if (delta < 0 ){ 59 | succeed = false; 60 | }else if (delta == 0){ 61 | V1 = (accelorationLimitTCP * time + currentVelocityTCP + endVelocityTCP) / 2; 62 | if (~(V1 >= currentVelocityTCP && V1 >= endVelocityTCP 63 | && qAbs(V1) <= velocityLimitTCP 64 | && (time - (V1 - currentVelocityTCP) / accelorationLimitTCP - (endVelocityTCP - V1) / (-accelorationLimitTCP)) >= 0)) 65 | { 66 | succeed = true; //有解 67 | } 68 | }else { 69 | V[0] = (2 * (accelorationLimitTCP * time + currentVelocityTCP + endVelocityTCP) + qSqrt(delta)) / 4; 70 | V[1] = (2 * (accelorationLimitTCP * time + currentVelocityTCP + endVelocityTCP) - qSqrt(delta)) / 4; 71 | if (V[0] >= currentVelocityTCP && V[0] >= endVelocityTCP 72 | && qAbs(V[0]) <= velocityLimitTCP 73 | && (time - (V[0] - currentVelocityTCP) / accelorationLimitTCP - (endVelocityTCP - V[0]) / (-accelorationLimitTCP)) >= 0) 74 | { 75 | V1 = V[0]; 76 | succeed = true; //有解 77 | }else if (V[1] >= currentVelocityTCP && V[1] >= endVelocityTCP 78 | && qAbs(V[1]) <= velocityLimitTCP 79 | && (time - (V[1] - currentVelocityTCP) / accelorationLimitTCP - (endVelocityTCP - V[1]) / (-accelorationLimitTCP)) >= 0) 80 | { 81 | V1 = V[1]; 82 | succeed = true; 83 | }else{ 84 | succeed = false; 85 | } 86 | } 87 | if (succeed){ 88 | t1 = (V1 - currentVelocityTCP) / accelorationLimitTCP; 89 | t2 = time - (endVelocityTCP - V1) / (-accelorationLimitTCP); 90 | float t = 0; 91 | for (int i = 0; i < numOfInterpolation; i++){ 92 | t += T; 93 | if (t <= t1){ 94 | pos[i] = currentVelocityTCP * t + 0.5 * accelorationLimitTCP * t * t; 95 | }else if (t > t1 && t <= t2){ 96 | pos[i] = (0 + currentVelocityTCP * t1 + 0.5 * accelorationLimitTCP * t1 * t1) 97 | + V1 * (t - t1); 98 | }else{ 99 | pos[i] = (0 + currentVelocityTCP * t1 + 0.5 * accelorationLimitTCP * t1 * t1) 100 | + V1 * (t2 - t1) 101 | + V1 * (t - t2) 102 | - 0.5 * accelorationLimitTCP * (t- t2) * (t - t2); 103 | } 104 | } 105 | break; 106 | } 107 | 108 | /*情况2：加速-匀速-加速先假设后验证*/ 109 | V1 = (2 * accelorationLimitTCP * remainPosTCP - endVelocityTCP * endVelocityTCP + currentVelocityTCP * currentVelocityTCP) 110 | / (2 * accelorationLimitTCP * time + 2 * currentVelocityTCP - 2 * endVelocityTCP); 111 | //succeed = true; 112 | if ( (V1 >= currentVelocityTCP && V1 <= endVelocityTCP && qAbs(V1) <= velocityLimitTCP 113 | && (time - (V1 - currentVelocityTCP) / accelorationLimitTCP - (endVelocityTCP - V1) / accelorationLimitTCP >= 0)) ){ 114 | succeed = true; 115 | } 116 | if (succeed){ 117 | t1 = (V1 - currentVelocityTCP) / accelorationLimitTCP; 118 | t2 = time - (endVelocityTCP - V1) / accelorationLimitTCP; 119 | float t = 0; 120 | for (int i = 0; i < numOfInterpolation; i++){ 121 | t += T; 122 | if (t <= t1){ 123 | pos[i] = currentVelocityTCP * t + 0.5 * accelorationLimitTCP * t * t; 124 | }else if (t > t1 && t <= t2){ 125 | pos[i] = (0 + currentVelocityTCP * t1 + 0.5 * accelorationLimitTCP * t1 * t1) 126 | + V1 * (t - t1); 127 | }else{ 128 | pos[i] = (0 + currentVelocityTCP * t1 + 0.5 * accelorationLimitTCP * t1 * t1) 129 | + V1 * (t2 - t1) 130 | + V1 * (t - t2) 131 | + 0.5 * accelorationLimitTCP * (t- t2) * (t - t2); 132 | } 133 | } 134 | break; 135 | } 136 | 137 | /*情况3：减速-匀速-加速先假设后验证*/ 138 | delta = 4 * (-accelorationLimitTCP * time + currentVelocityTCP + endVelocityTCP) 139 | * (-accelorationLimitTCP * time + currentVelocityTCP + endVelocityTCP) 140 | - 8 * (currentVelocityTCP * currentVelocityTCP + endVelocityTCP * endVelocityTCP - 2 * accelorationLimitTCP * remainPosTCP); 141 | if (delta < 0){ 142 | succeed = false; 143 | }else if (delta == 0){ 144 | V1 = (currentVelocityTCP + endVelocityTCP - accelorationLimitTCP *time) / 2; 145 | if (~(V1 <= currentVelocityTCP && V1 <= endVelocityTCP && qAbs(V1) <= velocityLimitTCP 146 | && time - (V1 - currentVelocityTCP)/(-accelorationLimitTCP) - (endVelocityTCP - V1) / accelorationLimitTCP >= 0)){ 147 | succeed = false; 148 | } 149 | }else{ 150 | V[0] = (2 * (currentVelocityTCP + endVelocityTCP - accelorationLimitTCP * time) + qSqrt(delta)) / 4; 151 | 152 | V[1] = (2 * (currentVelocityTCP + endVelocityTCP - accelorationLimitTCP * time) - qSqrt(delta)) / 4; 153 | if (V[0] <= currentVelocityTCP && V[0] <= endVelocityTCP && qAbs(V[0]) <= velocityLimitTCP 154 | && time - (V[0] - currentVelocityTCP)/(-accelorationLimitTCP) - (endVelocityTCP - V[0]) / accelorationLimitTCP >= 0){ 155 | V1 = V[0]; 156 | succeed = true; 157 | }else if(V[1] <= currentVelocityTCP && V[1] <= endVelocityTCP && qAbs(V[1]) <= velocityLimitTCP 158 | && time - (V[1] - currentVelocityTCP)/(-accelorationLimitTCP) - (endVelocityTCP - V[1]) / accelorationLimitTCP >= 0){ 159 | V1 = V[1]; 160 | succeed = true; 161 | }else{ 162 | succeed = false; 163 | } 164 | } 165 | if (succeed){ 166 | t1 = (V1 - currentVelocityTCP) / (-accelorationLimitTCP); 167 | t2 = time - (endVelocityTCP - V1) / accelorationLimitTCP; 168 | float t = 0; 169 | for (int i = 0; i < numOfInterpolation; i++){ 170 | t += T; 171 | if (t <= t1){ 172 | pos[i] = currentVelocityTCP * t - 0.5 * accelorationLimitTCP * t * t; 173 | }else if (t > t1 && t <= t2){ 174 | pos[i] = (0 + currentVelocityTCP * t1 - 0.5 * accelorationLimitTCP * t1 * t1) 175 | + V1 * (t - t1); 176 | }else{ 177 | pos[i] = (0 + currentVelocityTCP * t1 - 0.5 * accelorationLimitTCP * t1 * t1) 178 | + V1 * (t2 - t1) 179 | + V1 * (t - t2) 180 | + 0.5 * accelorationLimitTCP * (t- t2) * (t - t2); 181 | } 182 | } 183 | break; 184 | } 185 | 186 | /*情况4：减速-匀速-减速*/ 187 | V1 = (2 * accelorationLimitTCP * remainPosTCP + endVelocityTCP * endVelocityTCP - currentVelocityTCP * currentVelocityTCP) 188 | / (2 * accelorationLimitTCP * time - 2 * currentVelocityTCP + 2 * endVelocityTCP); 189 | //succeed = true; 190 | if (V1 <= currentVelocityTCP && V1 >= endVelocityTCP && qAbs(V1) <= velocityLimitTCP 191 | && (time - (V1 - currentVelocityTCP) / (-accelorationLimitTCP) - (endVelocityTCP - V1) / (-accelorationLimitTCP) >= 0)){ 192 | succeed = true; 193 | } 194 | if (succeed){ 195 | t1 = (V1 - currentVelocityTCP) / (-accelorationLimitTCP); 196 | t2 = time - (endVelocityTCP - V1) / (-accelorationLimitTCP); 197 | float t = 0; 198 | for (int i = 0; i < numOfInterpolation; i++){ 199 | t += T; 200 | if (t <= t1){ 201 | pos[i] = currentVelocityTCP * t - 0.5 * accelorationLimitTCP * t * t; 202 | }else if (t > t1 && t <= t2){ 203 | pos[i] = (0 + currentVelocityTCP * t1 - 0.5 * accelorationLimitTCP * t1 * t1) 204 | + V1 * (t - t1); 205 | }else{ 206 | pos[i] = (0 + currentVelocityTCP * t1 - 0.5 * accelorationLimitTCP * t1 * t1) 207 | + V1 * (t2 - t1) 208 | + V1 * (t - t2) 209 | - 0.5 * accelorationLimitTCP * (t- t2) * (t - t2); 210 | } 211 | } 212 | break; 213 | } 214 | } 215 | if (succeed) { 216 | currentVelocityTCP = pos.first() / T; 217 | return pos; 218 | } else { 219 | return QVector(); 220 | } 221 | } 222 | 223 | /*! 224 | * \brief MotionControler::TPlan 指定期望速度的T形速度规划，相对运动方式 225 | * \return 各轴下一个插补周期的目标位置 226 | */ 227 | bool MotionControler::TPlanBaseVelocity() 228 | { 229 | bool succeed = false; 230 | /*情况1：加速度-减速*/ 231 | if (remainPosTCP > 0 232 | && (2 * velocityLimitTCP * velocityLimitTCP - startVelocityTCP * startVelocityTCP - endVelocityTCP * endVelocityTCP) / (2 * accelorationLimitTCP) >= remainPosTCP ){ 233 | succeed = true; 234 | V1 = qSqrt(accelorationLimitTCP * remainPosTCP + 0.5 * startVelocityTCP * startVelocityTCP + 0.5 * endVelocityTCP * endVelocityTCP); 235 | t1 = (V1 - startVelocityTCP) / accelorationLimitTCP; 236 | t2 = (2 * V1 - startVelocityTCP - endVelocityTCP) / accelorationLimitTCP; 237 | interpolationNum = (int) (t2 / T) - 1; 238 | interpolationType = type0; 239 | } 240 | /* 情况2：加速-匀速-减速*/ 241 | else if (remainPosTCP > 0 && (2 * velocityLimitTCP * velocityLimitTCP - startVelocityTCP * startVelocityTCP - endVelocityTCP * endVelocityTCP) / (2 * accelorationLimitTCP) < remainPosTCP ){ 242 | succeed = true; 243 | t1 = (velocityLimitTCP - startVelocityTCP) / accelorationLimitTCP; 244 | t2 = t1 + (remainPosTCP - 245 | (2 * velocityLimitTCP * velocityLimitTCP - startVelocityTCP * startVelocityTCP - endVelocityTCP * endVelocityTCP) 246 | / (2 * accelorationLimitTCP)) / velocityLimitTCP; 247 | t3 = t2 + (velocityLimitTCP - endVelocityTCP) / accelorationLimitTCP; 248 | interpolationNum = (int) (t3 / T) - 1; 249 | interpolationType = type1; 250 | } 251 | /* 情况3：减速-加速*/ 252 | else if (remainPosTCP < 0 && (2 * velocityLimitTCP * velocityLimitTCP - startVelocityTCP * startVelocityTCP -endVelocityTCP * endVelocityTCP) / (2 * accelorationLimitTCP) > (-remainPosTCP)) { 253 | succeed = true; 254 | V1 = -qSqrt(-accelorationLimitTCP * remainPosTCP - 0.5 * startVelocityTCP * startVelocityTCP -0.5 * endVelocityTCP * endVelocityTCP); 255 | t1 = (V1 - startVelocityTCP) / (-accelorationLimitTCP); 256 | t2 = (-2 * V1 + startVelocityTCP + endVelocityTCP) / accelorationLimitTCP; 257 | interpolationNum = (int) (t2 / T) - 1; 258 | interpolationType = type2; 259 | } 260 | 261 | /*情况4：减速-匀速-加速*/ 262 | else if (remainPosTCP < 0 && (2 * velocityLimitTCP * velocityLimitTCP - startVelocityTCP * startVelocityTCP -endVelocityTCP * endVelocityTCP) / (2 * accelorationLimitTCP) <= (-remainPosTCP)) { 263 | succeed = true; 264 | float t1 = (startVelocityTCP + velocityLimitTCP) / accelorationLimitTCP; 265 | float t2 = t1 + (remainPosTCP + 266 | (2 * velocityLimitTCP * velocityLimitTCP - startVelocityTCP * startVelocityTCP - endVelocityTCP * endVelocityTCP) 267 | / (2 * accelorationLimitTCP)) / (-velocityLimitTCP); 268 | float t3 = t2 + (endVelocityTCP + velocityLimitTCP) / accelorationLimitTCP; 269 | interpolationNum = (int) (t3 / T) - 1; 270 | interpolationType = type3; 271 | } 272 | /* 其它情况：无法规划*/ 273 | else { 274 | succeed = false; 275 | } 276 | return succeed; 277 | } 278 | 279 | /*! 280 | * \brief MotionControler::Update 更新运动控制器各状态参数 281 | * 1、每个伺服周期调用一次 282 | * 2、进行运动规划 283 | */ 284 | void MotionControler::Update() 285 | { 286 | if (status != free) { 287 | qreal interpolation; 288 | if (interpolationCounter == 0) { /* 首次规划 */ 289 | UpdateRemainPos(); 290 | TPlanBaseVelocity(); 291 | interpolation = GetInterpolation(interpolationCounter); 292 | } 293 | if (interpolationCounter <= interpolationNum && interpolationCounter >= 1 && status != free) { 294 | interpolation = GetInterpolation(interpolationCounter) 295 | - GetInterpolation(interpolationCounter - 1); /* 插补距离 */ 296 | } else { 297 | interpolation = 0; 298 | } 299 | UpdateCurrentPos(); 300 | UpdateRemainPos(); 301 | currentVelocityTCP = interpolation / T; /* 更新当前速度 */ 302 | qDebug() << interpolationCounter; 303 | qDebug() << "remainPosTCP: " << remainPosTCP; 304 | qDebug() << "interpolationTable: " << GetInterpolation(interpolationCounter); 305 | qDebug() << "currentPos: " << (UpdateCurrentPos().isEmpty() ? QVector(axisNum + 1, 0) : UpdateCurrentPos()); 306 | interpolationCounter++; 307 | if (interpolationCounter > interpolationNum){ 308 | status = free; 309 | timer->stop(); 310 | } 311 | } 312 | } 313 | /*! 314 | * \brief MotionControler::UpdateCurrentPos 更新当前位置 315 | */ 316 | QVector MotionControler::UpdateCurrentPos() 317 | { 318 | qreal remainTotal = 0; 319 | for (int index = 0; index <= axisNum; index++) { 320 | remainTotal += qPow(endPos[index] - startPos[index], 2); 321 | } 322 | remainTotal = qSqrt(remainTotal); 323 | if (qAbs(remainTotal) < error) { /* 说明已经到达目标位置 */ 324 | return QVector(); 325 | } 326 | qreal coeficiency = GetInterpolation(interpolationCounter) / remainTotal; 327 | for (int index = 0; index <= axisNum; index++) { 328 | qreal remain = endPos[index] - startPos[index]; 329 | currentPos[index] = startPos[index] + coeficiency * remain; 330 | } 331 | QVector pos(axisNum + 1); 332 | for (int index = 0; index <= axisNum; index++){ 333 | pos[index] = multiper[index] * (currentPos[index] + origin[index]); /* 加上原点偏移 */ 334 | } 335 | return pos; 336 | } 337 | /*! 338 | * \brief MotionControler::GetInterpolation 获取插补点 339 | * \param index 340 | * \return 341 | */ 342 | qreal MotionControler::GetInterpolation(int index) 343 | { 344 | index = qBound(0, index, interpolationNum); 345 | qreal t = (index + 1) * T; 346 | switch (interpolationType) { 347 | case type0: 348 | if (t <= t1){ 349 | return currentVelocityTCP * t + 0.5 * accelorationLimitTCP * t * t; 350 | }else{ 351 | return currentVelocityTCP * t1 + 0.5 * accelorationLimitTCP * t1 * t1 352 | + V1 * (t - t1) 353 | - 0.5 * accelorationLimitTCP * (t - t1) * (t - t1); 354 | } 355 | break; 356 | case type1: 357 | if (t <= t1){ 358 | return currentVelocityTCP * t + 0.5 * accelorationLimitTCP * t * t; 359 | }else if (t > t1 && t <= t2) { 360 | return currentVelocityTCP * t1+ 0.5 * accelorationLimitTCP * t1 * t1 361 | + velocityLimitTCP * (t - t1); 362 | }else { 363 | return currentVelocityTCP * t1+ 0.5 * accelorationLimitTCP * t1 * t1 364 | + velocityLimitTCP * (t2 - t1) 365 | + velocityLimitTCP * (t - t2) - 0.5 * accelorationLimitTCP * (t - t2) * (t - t2); 366 | } 367 | break; 368 | case type2: 369 | if (t <= t1){ 370 | return currentVelocityTCP * t - 0.5 * accelorationLimitTCP * t * t; 371 | }else { 372 | return currentVelocityTCP * t1 - 0.5 * accelorationLimitTCP * t1 * t1 373 | + V1 * (t - t1) + 0.5 * accelorationLimitTCP * (t - t1) * (t - t1); 374 | } 375 | break; 376 | case type3: 377 | if (t <= t1){ 378 | return currentVelocityTCP * t - 0.5 * accelorationLimitTCP * t * t; 379 | }else if (t > t1 && t <= t2){ 380 | return currentVelocityTCP * t1 - 0.5 * accelorationLimitTCP * t1 * t1 381 | - velocityLimitTCP * (t - t1); 382 | }else { 383 | return currentVelocityTCP * t1 - 0.5 * accelorationLimitTCP * t1 * t1 384 | - velocityLimitTCP * (t2 - t1) 385 | - velocityLimitTCP * (t - t2) + 0.5 * accelorationLimitTCP * (t - t2) * (t - t2); 386 | } 387 | break; 388 | default: 389 | break; 390 | } 391 | return 0; 392 | } 393 | 394 | void MotionControler::MoveAbs(QVector pos) 395 | { 396 | if (status == free) { /* 取消if条件后可以运动中更改运动指令 */ 397 | SetEndPos(pos); 398 | status = moveAbs; 399 | timer->start((int)T); 400 | } 401 | } 402 | 403 | void MotionControler::MoveAbs(qreal pos, int axis) 404 | { 405 | if (status == free){ 406 | SetEndPos(pos, axis); 407 | status = moveAbs; 408 | timer->start((int)T); 409 | } 410 | } 411 | 412 | void MotionControler::Move(QVector pos) 413 | { 414 | for (int index = 0; index <= axisNum; index++) { 415 | pos[index] += currentPos[index]; 416 | } 417 | MoveAbs(pos); 418 | status = move; 419 | } 420 | 421 | void MotionControler::Move(qreal pos, int axis) 422 | { 423 | pos += currentPos[axis]; 424 | MoveAbs(pos, axis); 425 | status = move; 426 | } 427 | /*! 428 | * \brief MotionControler::RapidStop 快速停止，加速度无限大 429 | * 直接停在当前位置 430 | */ 431 | void MotionControler::RapidStop() 432 | { 433 | status = free; 434 | timer->stop(); 435 | } 436 | /*! 437 | * \brief MotionControler::Stop 以最大加速度减速直到停止 438 | */ 439 | void MotionControler::Stop() 440 | { 441 | //计算出以最大加速度减速后停下来的位置，将其设置成endPos,执行move指令 442 | qreal stopDistance = qPow(currentVelocityTCP, 2) / (2 * accelorationLimitTCP); 443 | qreal coefficiency = stopDistance / UpdateRemainPos(); 444 | QVector stopPos(axisNum + 1); 445 | for (int index = 0; index <= axisNum; index++) { 446 | stopPos[index] = currentPos[index] + coefficiency * (endPos[index] - currentPos[index]); 447 | } 448 | SetEndPos(stopPos); /* 重置终点 */ 449 | } 450 | /*! 451 | * \brief MotionControler::DefPos 重定义当前轴位置 452 | */ 453 | void MotionControler::DefPos(qreal pos, int axis) 454 | { 455 | origin[axis] = currentPos[axis] + origin[axis] - pos; 456 | currentPos[axis] = pos; 457 | } 458 | 459 | void MotionControler::DefPos(QVector pos) 460 | { 461 | for (int index = 0; index <= axisNum; index++){ 462 | origin[index] = currentPos[index] + origin[index] - pos[index]; 463 | currentPos[index] = pos[index]; 464 | } 465 | } 466 | /*! 467 | * \brief MotionControler::UpdateRemainPos 更新剩余距离 468 | * \return 返回剩余距离 469 | */ 470 | qreal MotionControler::UpdateRemainPos() 471 | { 472 | qreal remain = 0; 473 | for (int index = 0; index <= axisNum; index++) { 474 | remainPos[index] = endPos[index] - currentPos[index]; 475 | remain += qPow(remainPos[index], 2); 476 | } 477 | remainPosTCP = qSqrt(remain); 478 | return remainPosTCP; 479 | } 480 | /*! 481 | * \brief MotionControler::SetEndPos 设置终点，会导致重新进行运动规划 482 | * \param pos 483 | */ 484 | void MotionControler::SetEndPos(QVector pos) 485 | { 486 | if (endPos == pos){ 487 | return; 488 | } else { 489 | status = moveAbs; /* 当MType != free时才会进行运动规划 */ 490 | endPos = pos; 491 | interpolationCounter = 0; 492 | // interpolationTable = QVector(); /* 重置插值列表 */ 493 | startPos = currentPos; 494 | startVelocityTCP = currentVelocityTCP; 495 | } 496 | } 497 | 498 | void MotionControler::SetEndPos(qreal pos, int axis) 499 | { 500 | if (endPos[axis] == pos) { 501 | return; 502 | } else { 503 | status = moveAbs; 504 | endPos[axis] = pos; 505 | interpolationCounter = 0; 506 | // interpolationTable = QVector(); 507 | startPos = currentPos; 508 | startVelocityTCP = currentVelocityTCP; 509 | 510 | } 511 | } 512 | /*! 513 | * \brief MotionControler::SetVelocityLimitTCP 重置期望速度 514 | * 1、如果MType = free，则不会带来运动规划 515 | * 2、如果MType != free，则会重新进行运动规划 516 | * \param vel 517 | */ 518 | void MotionControler::SetVelocityLimitTCP(qreal vel) 519 | { 520 | if (velocityLimitTCP == vel) { 521 | return; 522 | } else { 523 | velocityLimitTCP = vel; 524 | interpolationCounter = 0; 525 | // interpolationTable = QVector(); 526 | startPos = currentPos; 527 | startVelocityTCP = currentVelocityTCP; 528 | 529 | } 530 | } 531 | /*! 532 | * \brief MotionControler::SetAccelorationLimitTCP 重置期望加速度 533 | * 1、如果MType = free，则不会带来运动规划 534 | * 2、如果MType != free，则会重新进行运动规划 535 | * 3、更新误差限 536 | * \param acc 537 | */ 538 | void MotionControler::SetAccelorationLimitTCP(qreal acc) 539 | { 540 | if (accelorationLimitTCP == acc) { 541 | return; 542 | } else { 543 | accelorationLimitTCP = acc; 544 | interpolationCounter = 0; 545 | // interpolationTable = QVector(); 546 | startPos = currentPos; 547 | startVelocityTCP = currentVelocityTCP; 548 | error = accelorationLimitTCP * qPow(T, 2); 549 | } 550 | } 551 | /*! 552 | * \brief MotionControler::sendCurrentPos slot,由定时器触发， 553 | * 用来激活castCurrentPos()信号，广播伺服位置 554 | */ 555 | void MotionControler::SendCurrentPos() 556 | { 557 | Update(); 558 | if (status != free) { 559 | // emit CastCurrentPos(UpdateCurrentPos()); 560 | } 561 | } 562 | -------------------------------------------------------------------------------- /MotionControler.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTIONCONTROLER_H 2 | #define MOTIONCONTROLER_H 3 | 4 | #include 5 | #include 6 | 7 | class QTimer; 8 | /*! 9 | * \brief 运动控制器类 10 | * 1、核心是TPlanBaseVelocity()进行T形速度规划,计算出位移曲线的分段函数GetInterpolation(int index), 11 | * 2、运动指令MoveAbs()和Move() 12 | * 3.1、信号CastCurrentPos(QVector)每T时间对外广播每个伺服周期的指令位置 13 | * 3.2、亦可不使用signal，外部调用update()更新一次伺服位置后再调用GetCurrentPos()得到当前伺服周期的位置 14 | */ 15 | class MotionControler : QObject 16 | { 17 | public: 18 | /* 运动状态枚举 */ 19 | enum motionStatus{ 20 | moveAbs, 21 | move, 22 | free, 23 | }; 24 | enum curveType{ 25 | type0, //加速-减速 26 | type1, //加速度-匀速-减速 27 | type2, //减速-加速度 28 | type3, //减速-匀速-加速 29 | }; 30 | 31 | MotionControler(); 32 | 33 | /* 运动指令 */ 34 | void MoveAbs(QVector pos); 35 | void MoveAbs(qreal pos, int axis); 36 | void Move(QVector pos); 37 | void Move(qreal pos, int axis); 38 | void RapidStop(); 39 | void Stop(); 40 | 41 | /* 运动规划 */ 42 | QVector TPlanBaseTime(qreal time); 43 | bool TPlanBaseVelocity(); 44 | void Update(); 45 | qreal UpdateRemainPos(); 46 | QVector UpdateCurrentPos(); 47 | qreal GetInterpolation(int index); 48 | inline motionStatus GetStatus() {return status;} 49 | 50 | /* 更改以下运动参数，可能会重新进行运动规划 */ 51 | void SetEndPos(QVector pos); 52 | void SetEndPos(qreal pos, int axis); 53 | void SetVelocityLimitTCP(qreal vel); 54 | void SetAccelorationLimitTCP(qreal acc); 55 | //未定义、未使用 56 | void SetVelocityLimit(QVector vel); 57 | void SetAccelorationLimit(QVector acc); 58 | 59 | /* 参数设置 */ 60 | inline void SetCurrentPos(QVector pos) {currentPos = pos;} 61 | inline void SetCurrentPos(qreal pos, int axis) {currentPos[axis] = pos;} 62 | 63 | inline void SetEndVelocityTCP(qreal vel) {endVelocityTCP = vel;} 64 | inline void SetEndVelocity(QVector vel) {endVelocity = vel;} 65 | inline void SetEndVelocity(qreal vel, int axis) {endVelocity[axis] = vel;} 66 | inline void SetT(qreal t) { T = t;} 67 | inline void SetError(qreal err) {error = err;} //最小建议值为accelorationLimitTCP * T * T 68 | inline void SetAxisNum(int num) {axisNum = num;} 69 | 70 | /* 定义轴位置 */ 71 | void DefPos(qreal pos, int axis); 72 | void DefPos(QVector pos); 73 | 74 | /* 参数获取 */ 75 | QVector GetCurrentPos() {return currentPos;} 76 | qreal GetCurrentPos(int axis) {return currentPos[axis];} 77 | QVector GetCurrentVel() {return currentVelocity;} 78 | qreal GetCurrentVelTCP() {return currentVelocityTCP;} 79 | QVector GetCurrentAcc() {return currentAcceloration;} 80 | qreal GetCurrentAccTCP() {return currentAccelorationTCP;} 81 | 82 | 83 | signals: 84 | void CastCurrentPos(QVector pos); /* 向外广播伺服位置 */ 85 | //private slots: 86 | public: 87 | void SendCurrentPos(); /* 由定时器每1ms触发一次 */ 88 | 89 | private: 90 | int axisNum; /* 轴个数 axisNum + 1 */ 91 | 92 | QVector currentPos; /* 各轴当前位置 */ 93 | qreal currentVelocityTCP; /* 当前的TCP速度 */ 94 | QVector currentVelocity; /* 各轴当前速度，利用速度分解得到 */ 95 | qreal currentAccelorationTCP; /* 当前合成加速度，即TCP加速度 */ 96 | QVector currentAcceloration; /* 各轴当前加速度，利用加速度分解得到 */ 97 | 98 | QVector startPos; /* 速度规划时的起始位置：每次规划前需要更新成当前位置 */ 99 | qreal startVelocityTCP; 100 | QVector endPos; /* 终点的各轴位置 */ 101 | qreal endVelocityTCP; /* 终点的TCP速度 */ 102 | QVector endVelocity; /* 终点的各轴速度 */ 103 | 104 | qreal remainPosTCP; /* 剩下的总路程 */ 105 | QVector remainPos; /* 剩下的各轴位移 */ 106 | 107 | motionStatus status; /* 当前运动状态 */ 108 | 109 | QVector velocityLimit; /* 各轴速度限制 */ 110 | qreal velocityLimitTCP; /* TCP速度限制 */ 111 | QVector accelorationLimit; /* 各轴加速度限制 */ 112 | qreal accelorationLimitTCP; /* TCP加速度限制 */ 113 | 114 | QVector multiper; /* 轴增益 */ 115 | QVector origin; /* 各轴原点偏移 */ 116 | 117 | qreal T; /* 伺服周期 */ 118 | /* 插补过程中的参数 */ 119 | int interpolationCounter; /* 一次速度规划中的插值索引 */ 120 | int interpolationNum; /* 一次速度规划中的插值点总个数 */ 121 | int interpolationType; 122 | qreal t1, t2, t3; /* T形运动规划中间变量 */ 123 | qreal V1; 124 | 125 | qreal error; /* 终点判定误差 */ 126 | 127 | QTimer *timer; /* 需要在运动指令执行后开始，运动指令完成后关闭 128 | status = free时关闭 129 | status != free时开启 */ 130 | 131 | }; 132 | 133 | #endif // MOTIONCONTROLER_H 134 | -------------------------------------------------------------------------------- /README.TXT: -------------------------------------------------------------------------------- 1 | class MotionControl 2 | /*! 3 | * \brief 运动控制器类 4 | * 1、核心是TPlanBaseVelocity()进行T形速度规划,计算出位移曲线的分段函数GetInterpolation(int index) 5 | * 2、运动指令 6 | * 2.1、运动指令MoveAbs()和Move() 7 | 2.2、调用终点设置函数SetEndPos()，也会导致生成一个新的运动指令，实现对终点的跟随 8 | * 3、指令位置的刷新与获取 9 | * 3.1、信号CastCurrentPos(QVector)每T时间对外广播每个伺服周期的指令位置 10 | * 3.2、亦可不使用signal，外部调用update()更新一次伺服位置后再调用GetCurrentPos()得到当前伺服周期的位置 11 | */ -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char *argv[]) 8 | { 9 | // QCoreApplication a(argc, argv); 10 | 11 | MotionControler mc; 12 | // 状态配置 13 | mc.SetAccelorationLimitTCP(50000); 14 | mc.SetVelocityLimitTCP(80); 15 | mc.SetEndVelocityTCP(0); 16 | mc.SetError(500 * 1e-6); 17 | int axisNum = 1; 18 | QVector endPos(axisNum + 1); 19 | for (int index = 0; index <= axisNum; index++) { 20 | endPos[index] = index + 1; 21 | } 22 | mc.MoveAbs(endPos); 23 | int count = 0; 24 | QString path("motionPlan.csv"); 25 | QFile csvFile(path); 26 | if(!csvFile.open(QIODevice::WriteOnly)){ 27 | return 0; 28 | } 29 | QTextStream csv(&csvFile); 30 | while (1) { 31 | if (mc.GetStatus() == MotionControler::free) { 32 | break; 33 | qDebug() << "test"; 34 | } 35 | mc.SendCurrentPos(); 36 | // csv << mc.UpdateRemainPos() << endl; 37 | count++; 38 | if (count == 10){ 39 | mc.SetAccelorationLimitTCP(5000); 40 | // break; 41 | // mc.MoveAbs(QVector(mc.axisNum + 1, 1)); 42 | } 43 | 44 | } 45 | qDebug() << endl; 46 | qDebug() << "total interpolation points number: " << count; 47 | qDebug() << "finish to motion planning"; 48 | qDebug() << endl; 49 | // return a.exec(); 50 | return 0; 51 | } 52 | 53 | --------------------------------------------------------------------------------