├── LICENSE ├── README.md └── motionPlan ├── MotionPlan.pro ├── MotionPlan.pro.user ├── autorun.cpp ├── autorun.h ├── main.cpp ├── mainwindow.cpp ├── mainwindow.h ├── mainwindow.ui ├── motionplan.cpp ├── motionplan.h ├── mychart.cpp ├── mychart.h ├── mychart.ui └── pic.png /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 WangZhangMing 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 简介 2 | 编写了一种静态前瞻式7段S型速度规划，实现边计算边运动，并在运动过程中可平稳减速暂停以及启动。 3 | 目前算法在运动过程中不支持修改位移、速度、加速度等条件，只是是在这些条件确定下实现的前瞻速度规划。 4 | ## 有暂停情况 5 | ![image](https://user-images.githubusercontent.com/71966407/210471677-d0aefab9-9ba8-49b0-bdf9-286240e52c6a.png) 6 | 7 | ## 无暂停情况 8 | ![image](https://user-images.githubusercontent.com/71966407/210471748-ea088f10-6fc6-4c73-bc70-3c31f77a065b.png) 9 | -------------------------------------------------------------------------------- /motionPlan/MotionPlan.pro: -------------------------------------------------------------------------------- 1 | #------------------------------------------------- 2 | # 3 | # Project created by QtCreator 2021-09-15T17:02:59 4 | # 5 | #------------------------------------------------- 6 | 7 | QT += core gui charts 8 | 9 | greaterThan(QT_MAJOR_VERSION, 4): QT += widgets 10 | 11 | TARGET = motionPlan 12 | TEMPLATE = app 13 | 14 | # The following define makes your compiler emit warnings if you use 15 | # any feature of Qt which as been marked as deprecated (the exact warnings 16 | # depend on your compiler). Please consult the documentation of the 17 | # deprecated API in order to know how to port your code away from it. 18 | 19 | DEFINES += QT_DEPRECATED_WARNINGS motionDebug 20 | 21 | # You can also make your code fail to compile if you use deprecated APIs. 22 | # In order to do so, uncomment the following line. 23 | # You can also select to disable deprecated APIs only up to a certain version of Qt. 24 | #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 25 | 26 | 27 | SOURCES += main.cpp\ 28 | mainwindow.cpp \ 29 | autorun.cpp \ 30 | motionplan.cpp \ 31 | mychart.cpp 32 | 33 | HEADERS += mainwindow.h \ 34 | autorun.h \ 35 | motionplan.h \ 36 | mychart.h 37 | 38 | FORMS += mainwindow.ui \ 39 | mychart.ui 40 | 41 | -------------------------------------------------------------------------------- /motionPlan/MotionPlan.pro.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | EnvironmentId 7 | {68c40faf-792f-418f-91e3-8199f7a41a75} 8 | 9 | 10 | ProjectExplorer.Project.ActiveTarget 11 | 0 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 | true 45 | 0 46 | 8 47 | true 48 | 1 49 | true 50 | true 51 | true 52 | false 53 | 54 | 55 | 56 | ProjectExplorer.Project.PluginSettings 57 | 58 | 59 | true 60 | 61 | 62 | 63 | ProjectExplorer.Project.Target.0 64 | 65 | Desktop Qt 5.12.2 GCC 64bit 66 | Desktop Qt 5.12.2 GCC 64bit 67 | qt.qt5.5122.gcc_64_kit 68 | 0 69 | 0 70 | 0 71 | 72 | /home/linux/work/personal/motionPlan/bin 73 | 74 | 75 | true 76 | qmake 77 | 78 | QtProjectManager.QMakeBuildStep 79 | true 80 | 81 | false 82 | false 83 | false 84 | 85 | 86 | true 87 | Make 88 | 89 | Qt4ProjectManager.MakeStep 90 | 91 | false 92 | 93 | 94 | false 95 | 96 | 2 97 | Build 98 | 99 | ProjectExplorer.BuildSteps.Build 100 | 101 | 102 | 103 | true 104 | Make 105 | 106 | Qt4ProjectManager.MakeStep 107 | 108 | true 109 | clean 110 | 111 | false 112 | 113 | 1 114 | Clean 115 | 116 | ProjectExplorer.BuildSteps.Clean 117 | 118 | 2 119 | false 120 | 121 | Debug 122 | Debug 123 | Qt4ProjectManager.Qt4BuildConfiguration 124 | 2 125 | true 126 | 127 | 128 | /home/linux/work/personal/motionPlan/bin 129 | 130 | 131 | true 132 | qmake 133 | 134 | QtProjectManager.QMakeBuildStep 135 | false 136 | 137 | false 138 | false 139 | true 140 | 141 | 142 | true 143 | Make 144 | 145 | Qt4ProjectManager.MakeStep 146 | 147 | false 148 | 149 | 150 | false 151 | 152 | 2 153 | Build 154 | 155 | ProjectExplorer.BuildSteps.Build 156 | 157 | 158 | 159 | true 160 | Make 161 | 162 | Qt4ProjectManager.MakeStep 163 | 164 | true 165 | clean 166 | 167 | false 168 | 169 | 1 170 | Clean 171 | 172 | ProjectExplorer.BuildSteps.Clean 173 | 174 | 2 175 | false 176 | 177 | Release 178 | Release 179 | Qt4ProjectManager.Qt4BuildConfiguration 180 | 0 181 | true 182 | 183 | 184 | /home/linux/work/personal/motionPlan/bin 185 | 186 | 187 | true 188 | qmake 189 | 190 | QtProjectManager.QMakeBuildStep 191 | true 192 | 193 | false 194 | true 195 | true 196 | 197 | 198 | true 199 | Make 200 | 201 | Qt4ProjectManager.MakeStep 202 | 203 | false 204 | 205 | 206 | false 207 | 208 | 2 209 | Build 210 | 211 | ProjectExplorer.BuildSteps.Build 212 | 213 | 214 | 215 | true 216 | Make 217 | 218 | Qt4ProjectManager.MakeStep 219 | 220 | true 221 | clean 222 | 223 | false 224 | 225 | 1 226 | Clean 227 | 228 | ProjectExplorer.BuildSteps.Clean 229 | 230 | 2 231 | false 232 | 233 | Profile 234 | Profile 235 | Qt4ProjectManager.Qt4BuildConfiguration 236 | 0 237 | true 238 | 239 | 3 240 | 241 | 242 | 0 243 | 部署 244 | 245 | ProjectExplorer.BuildSteps.Deploy 246 | 247 | 1 248 | Deploy Configuration 249 | 250 | ProjectExplorer.DefaultDeployConfiguration 251 | 252 | 1 253 | 254 | 255 | false 256 | false 257 | 1000 258 | 259 | true 260 | 261 | false 262 | false 263 | false 264 | false 265 | true 266 | 0.01 267 | 10 268 | true 269 | 1 270 | 25 271 | 272 | 1 273 | true 274 | false 275 | true 276 | valgrind 277 | 278 | 0 279 | 1 280 | 2 281 | 3 282 | 4 283 | 5 284 | 6 285 | 7 286 | 8 287 | 9 288 | 10 289 | 11 290 | 12 291 | 13 292 | 14 293 | 294 | 2 295 | 296 | MotionPlan 297 | 298 | Qt4ProjectManager.Qt4RunConfiguration:/home/linux/work/personal/motionPlan/MotionPlan.pro 299 | MotionPlan.pro 300 | 301 | 3768 302 | false 303 | true 304 | true 305 | false 306 | false 307 | true 308 | 309 | /home/linux/work/personal/motionPlan/bin 310 | 311 | 1 312 | 313 | 314 | 315 | ProjectExplorer.Project.TargetCount 316 | 1 317 | 318 | 319 | ProjectExplorer.Project.Updater.FileVersion 320 | 20 321 | 322 | 323 | Version 324 | 20 325 | 326 | 327 | -------------------------------------------------------------------------------- /motionPlan/autorun.cpp: -------------------------------------------------------------------------------- 1 | #include "autorun.h" 2 | 3 | autoRun::autoRun():motion(new motionPlan) 4 | { 5 | #ifdef motionDebug 6 | connect(motion,&motionPlan::sendMsg,this,&autoRun::sendMotionPlanMsg); 7 | #endif 8 | } 9 | 10 | autoRun::~autoRun() 11 | { 12 | delete motion; 13 | } 14 | 15 | void autoRun::run(){ 16 | static double path_moved=0; 17 | static double proportion=0,dis=0,vs=0,acc=0,time=0; 18 | static bool pause=false; 19 | path_moved=0; 20 | motion->ini_path_data(pathInit); 21 | while(motion->pathBusy()){ 22 | if(forceQuit) break; 23 | pause=path_pause; 24 | if(motion->performPath(pause)){ 25 | motion->get_move_msg(proportion,dis,vs,acc,time); 26 | emit send_data(dis-path_moved,dis,vs,acc,time); 27 | path_moved=dis; 28 | } 29 | } 30 | forceQuit=false; 31 | path_pause=false; 32 | emit send_cal_done_sig(); 33 | } 34 | -------------------------------------------------------------------------------- /motionPlan/autorun.h: -------------------------------------------------------------------------------- 1 | #ifndef AUTORUN_H 2 | #define AUTORUN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include //使用了QTime函数 13 | #include //qsrand和qrand这两个函数在这里面 14 | #include 15 | #include"motionplan.h" 16 | 17 | class autoRun : public QThread 18 | { 19 | Q_OBJECT 20 | public: 21 | autoRun(); 22 | ~autoRun(); 23 | pathInitData pathInit; 24 | bool path_pause=false; 25 | QString pathMode; 26 | 27 | bool forceQuit=false; 28 | 29 | protected: 30 | void run(); 31 | 32 | private: 33 | motionPlan *motion=nullptr; 34 | 35 | signals: 36 | void sendMotionPlanMsg(const QString &); 37 | void send_data(double,double,double,double,double); 38 | void send_cal_done_sig(); 39 | 40 | }; 41 | #endif // AUTORUN_H 42 | -------------------------------------------------------------------------------- /motionPlan/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include 3 | 4 | int main(int argc, char *argv[]) 5 | { 6 | QApplication a(argc, argv); 7 | MainWindow w; 8 | w.showMaximized(); 9 | 10 | return a.exec(); 11 | } 12 | -------------------------------------------------------------------------------- /motionPlan/mainwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include "ui_mainwindow.h" 3 | 4 | MainWindow::MainWindow(QWidget *parent) : 5 | QMainWindow(parent), 6 | ui(new Ui::MainWindow), 7 | disChart(new myChart(0,this)), 8 | spdChart(new myChart(1,this)), 9 | accChart(new myChart(2,this)), 10 | dataTableWidget(new QTableWidget(this)), 11 | autorun(new autoRun()) 12 | { 13 | ui->setupUi(this); 14 | setWindowTitle("静态前瞻式7段S型速度规划"); 15 | #ifdef motionDebug 16 | connect(autorun,&autoRun::sendMotionPlanMsg,this,&MainWindow::recMotionPlanMsg); 17 | #endif 18 | connect(autorun,&autoRun::send_data,this,&MainWindow::rec_data,Qt::BlockingQueuedConnection); 19 | connect(this,&MainWindow::send_data,disChart,&myChart::rec_data);//,Qt::BlockingQueuedConnection 20 | connect(this,&MainWindow::send_data,spdChart,&myChart::rec_data); 21 | connect(this,&MainWindow::send_data,accChart,&myChart::rec_data); 22 | connect(autorun,&autoRun::send_cal_done_sig,this,&MainWindow::rec_cal_done_sig); 23 | 24 | ui->lineEdit_V0->setValidator(new QDoubleValidator(this)); 25 | ui->lineEdit_Ve->setValidator(new QDoubleValidator(this)); 26 | ui->lineEdit_Jerk->setValidator(new QDoubleValidator(this)); 27 | ui->lineEdit_VMax->setValidator(new QDoubleValidator(this)); 28 | ui->lineEdit_distance->setValidator(new QDoubleValidator(this)); 29 | ui->lineEdit_acc->setValidator(new QDoubleValidator(this)); 30 | ui->lineEdit_planSingleTime->setValidator(new QDoubleValidator(this)); 31 | 32 | ui->mdiArea->setMouseTracking(true); 33 | ui->mdiArea->setContextMenuPolicy(Qt::CustomContextMenu); 34 | ui->mdiArea->addSubWindow(dataTableWidget,Qt::WindowMinMaxButtonsHint | Qt::WindowTitleHint); 35 | ui->mdiArea->addSubWindow(accChart,Qt::WindowMinMaxButtonsHint | Qt::WindowTitleHint); 36 | ui->mdiArea->addSubWindow(spdChart,Qt::WindowMinMaxButtonsHint | Qt::WindowTitleHint); 37 | ui->mdiArea->addSubWindow(disChart,Qt::WindowMinMaxButtonsHint | Qt::WindowTitleHint); 38 | ui->mdiArea->tileSubWindows(); 39 | ui->mdiArea->setOption(QMdiArea::DontMaximizeSubWindowOnActivation,true); 40 | 41 | ui->dockWidget->setWindowTitle(tr("参数设置")); 42 | dataTableWidget->setWindowTitle("规划数据"); 43 | initTableWidget(); 44 | } 45 | 46 | MainWindow::~MainWindow() 47 | { 48 | delete ui; 49 | if(autorun!=nullptr){ 50 | if(autorun->isRunning()){ 51 | autorun->forceQuit=true; 52 | autorun->quit(); 53 | autorun->wait(); 54 | } 55 | delete autorun; 56 | } 57 | delete disChart; 58 | delete spdChart; 59 | delete accChart; 60 | delete dataTableWidget; 61 | } 62 | 63 | void MainWindow::initTableWidget() 64 | { 65 | dataTableWidget->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); 66 | dataTableWidget->setEditTriggers(QAbstractItemView::NoEditTriggers); 67 | QTableWidgetItem *headerItem; 68 | QStringList widgethead; 69 | widgethead<<"总时间"<<"总位移"<<"当前时刻位移"<<"当前时刻速度"<<"当前时刻加速度"; 70 | dataTableWidget->setHorizontalHeaderLabels(widgethead); 71 | dataTableWidget->setColumnCount(widgethead.size()); 72 | for(int i=0;icolumnCount();i++) 73 | { 74 | headerItem=new QTableWidgetItem(widgethead.at(i)); 75 | dataTableWidget->setHorizontalHeaderItem(i,headerItem); 76 | } 77 | } 78 | 79 | void MainWindow::rec_data(double realData, double allData, double vs, double acc,double time) 80 | { 81 | static int row=0; 82 | dataTableWidget->setRowCount(dataTableWidget->rowCount()+1); 83 | row=dataTableWidget->rowCount()-1; 84 | dataTableWidget->setItem(row,0,new QTableWidgetItem(QString::number(time))); 85 | dataTableWidget->item (row,0)->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter); 86 | dataTableWidget->setItem(row,1,new QTableWidgetItem(QString::number(allData,'f',6))); 87 | dataTableWidget->item (row,1)->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter); 88 | dataTableWidget->setItem(row,2,new QTableWidgetItem(QString::number(realData))); 89 | dataTableWidget->item (row,2)->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter); 90 | dataTableWidget->setItem(row,3,new QTableWidgetItem(QString::number(vs))); 91 | dataTableWidget->item (row,3)->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter); 92 | dataTableWidget->setItem(row,4,new QTableWidgetItem(QString::number(acc))); 93 | dataTableWidget->item (row,4)->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter); 94 | dataTableWidget->scrollToBottom(); 95 | 96 | emit send_data(realData,allData,vs,acc,time); 97 | } 98 | 99 | void MainWindow::recMotionPlanMsg(const QString &arg) 100 | { 101 | ui->textEdit->append(arg); 102 | } 103 | 104 | void MainWindow::rec_cal_done_sig() 105 | { 106 | autorun->quit(); 107 | autorun->wait(); 108 | ui->btn_start->setText("启动"); 109 | } 110 | 111 | void MainWindow::on_btn_start_clicked() 112 | { 113 | if(!autorun->isRunning()){ 114 | autorun->pathInit.v0=ui->lineEdit_V0->text().toDouble(); 115 | autorun->pathInit.ve=ui->lineEdit_Ve->text().toDouble(); 116 | autorun->pathInit.Jerk=ui->lineEdit_Jerk->text().toDouble(); 117 | autorun->pathInit.VMax=ui->lineEdit_VMax->text().toDouble(); 118 | autorun->pathInit.cmd_pos=ui->lineEdit_distance->text().toDouble(); 119 | autorun->pathInit.max_acc=ui->lineEdit_acc->text().toDouble(); 120 | //autorun->pathInit.dec_jerk_muti=ui->doubleSpinBox_decMuti->value(); 121 | autorun->pathInit.deltaT=ui->lineEdit_planSingleTime->text().toDouble(); 122 | disChart->clearData(); 123 | spdChart->clearData(); 124 | accChart->clearData(); 125 | while(dataTableWidget->rowCount()>0) 126 | dataTableWidget->removeRow(0); 127 | ui->textEdit->clear(); 128 | autorun->start(); 129 | } 130 | else{ 131 | autorun->path_pause=false; 132 | ui->btn_start->setText("启动"); 133 | } 134 | } 135 | 136 | void MainWindow::on_btn_pause_clicked() 137 | { 138 | autorun->path_pause=true; 139 | ui->btn_start->setText("继续"); 140 | } 141 | 142 | void MainWindow::on_btn_cancelPlan_clicked() 143 | { 144 | if(autorun->isRunning()){ 145 | autorun->forceQuit=true; 146 | ui->btn_start->setText("启动"); 147 | } 148 | } 149 | 150 | void MainWindow::on_btn_showPoints_clicked() 151 | { 152 | static bool showPoint=false; 153 | if(showPoint) 154 | ui->btn_showPoints->setText("显示数据"); 155 | else 156 | ui->btn_showPoints->setText("隐藏数据"); 157 | disChart->showData(showPoint); 158 | spdChart->showData(showPoint); 159 | accChart->showData(showPoint); 160 | if(showPoint) showPoint=false; 161 | else showPoint=true; 162 | } 163 | 164 | void MainWindow::on_mdiArea_customContextMenuRequested(const QPoint &pos) 165 | { 166 | QAction act_menu("参数菜单显示", this); 167 | QAction act_cascadeShow("级联模式",this); 168 | QAction act_tileShow("平铺模式",this); 169 | connect(&act_menu, &QAction::triggered, this, &MainWindow::showMenu); 170 | connect(&act_cascadeShow, &QAction::triggered, this, &MainWindow::cascadeShow); 171 | connect(&act_tileShow, &QAction::triggered, this, &MainWindow::tileShow); 172 | QMenu menu,mdiShow("图表显示模式"); 173 | menu.addAction(&act_menu); 174 | menu.addMenu(&mdiShow); 175 | mdiShow.addAction(&act_cascadeShow); 176 | mdiShow.addAction(&act_tileShow); 177 | menu.exec(QCursor::pos()); 178 | } 179 | 180 | void MainWindow::cascadeShow() 181 | { 182 | ui->mdiArea->cascadeSubWindows(); 183 | } 184 | 185 | void MainWindow::tileShow() 186 | { 187 | ui->mdiArea->tileSubWindows(); 188 | } 189 | 190 | void MainWindow::showMenu() 191 | { 192 | if(ui->dockWidget->isHidden()) 193 | ui->dockWidget->show(); 194 | } 195 | -------------------------------------------------------------------------------- /motionPlan/mainwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef MAINWINDOW_H 2 | #define MAINWINDOW_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include"autorun.h" 12 | #include "mychart.h" 13 | 14 | using namespace QtCharts; 15 | 16 | namespace Ui { 17 | class MainWindow; 18 | } 19 | 20 | class MainWindow : public QMainWindow 21 | { 22 | Q_OBJECT 23 | 24 | public: 25 | explicit MainWindow(QWidget *parent = nullptr); 26 | ~MainWindow(); 27 | 28 | signals: 29 | void send_data(double, double, double,double,double); 30 | 31 | private slots: 32 | void recMotionPlanMsg(const QString &arg); 33 | 34 | void rec_data(double, double, double , double , double); 35 | 36 | void showMenu(); 37 | 38 | void cascadeShow(); 39 | 40 | void tileShow(); 41 | 42 | void rec_cal_done_sig(); 43 | 44 | void on_btn_start_clicked(); 45 | 46 | void on_btn_pause_clicked(); 47 | 48 | void on_btn_showPoints_clicked(); 49 | 50 | void on_btn_cancelPlan_clicked(); 51 | 52 | void on_mdiArea_customContextMenuRequested(const QPoint &pos); 53 | 54 | private: 55 | void initTableWidget(); 56 | 57 | Ui::MainWindow *ui; 58 | QPointerdisChart,spdChart,accChart; 59 | QPointerdataTableWidget; 60 | //myChart *disChart=nullptr; 61 | //myChart* spdChart=nullptr; 62 | //myChart* accChart=nullptr; 63 | //QTabWidget* dataTabWidget=nullptr; 64 | autoRun *autorun=nullptr; 65 | }; 66 | 67 | #endif // MAINWINDOW_H 68 | -------------------------------------------------------------------------------- /motionPlan/mainwindow.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 958 10 | 755 11 | 12 | 13 | 14 | 15 | Noto Serif CJK SC 16 | 12 17 | 18 | 19 | 20 | MainWindow 21 | 22 | 23 | 24 | 25 | 0 26 | 27 | 28 | 0 29 | 30 | 31 | 0 32 | 33 | 34 | 0 35 | 36 | 37 | 0 38 | 39 | 40 | 41 | 42 | Qt::ScrollBarAsNeeded 43 | 44 | 45 | Qt::ScrollBarAsNeeded 46 | 47 | 48 | QMdiArea::SubWindowView 49 | 50 | 51 | false 52 | 53 | 54 | false 55 | 56 | 57 | false 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | false 66 | 67 | 68 | 2 69 | 70 | 71 | 72 | 73 | 0 74 | 75 | 76 | 0 77 | 78 | 79 | 0 80 | 81 | 82 | 3 83 | 84 | 85 | 0 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | Noto Serif CJK SC 98 | 12 99 | 100 | 101 | 102 | 位移: 103 | 104 | 105 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | Noto Serif CJK SC 114 | 12 115 | 116 | 117 | 118 | 初速度: 119 | 120 | 121 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | Noto Serif CJK SC 130 | 12 131 | 132 | 133 | 134 | 最大速度: 135 | 136 | 137 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | Noto Serif CJK SC 146 | 12 147 | 148 | 149 | 150 | 末速度: 151 | 152 | 153 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | Noto Serif CJK SC 162 | 12 163 | 164 | 165 | 166 | 加速度: 167 | 168 | 169 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | Noto Serif CJK SC 178 | 12 179 | 180 | 181 | 182 | 加加速度: 183 | 184 | 185 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 186 | 187 | 188 | 189 | 190 | 191 | 192 | 时间单位: 193 | 194 | 195 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 0 208 | 0 209 | 210 | 211 | 212 | 213 | Noto Serif CJK SC 214 | 12 215 | 216 | 217 | 218 | 123.456 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 0 227 | 0 228 | 229 | 230 | 231 | 0 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 0 240 | 0 241 | 242 | 243 | 244 | 245 | Noto Serif CJK SC 246 | 12 247 | 248 | 249 | 250 | 1000 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 0 259 | 0 260 | 261 | 262 | 263 | 1 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 0 272 | 0 273 | 274 | 275 | 276 | 277 | Noto Serif CJK SC 278 | 12 279 | 280 | 281 | 282 | 5000 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 0 291 | 0 292 | 293 | 294 | 295 | 296 | Noto Serif CJK SC 297 | 12 298 | 299 | 300 | 301 | 50000 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 0 310 | 0 311 | 312 | 313 | 314 | 0.001 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | QFrame::StyledPanel 328 | 329 | 330 | QFrame::Raised 331 | 332 | 333 | 334 | 0 335 | 336 | 337 | 0 338 | 339 | 340 | 0 341 | 342 | 343 | 0 344 | 345 | 346 | 347 | 348 | 349 | 0 350 | 0 351 | 352 | 353 | 354 | 启动 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 0 363 | 0 364 | 365 | 366 | 367 | 暂停 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | QFrame::StyledPanel 378 | 379 | 380 | QFrame::Raised 381 | 382 | 383 | 384 | 0 385 | 386 | 387 | 0 388 | 389 | 390 | 0 391 | 392 | 393 | 0 394 | 395 | 396 | 397 | 398 | 399 | 0 400 | 0 401 | 402 | 403 | 404 | 取消规划 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 0 413 | 0 414 | 415 | 416 | 417 | 显示数据 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 0 429 | 0 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | -------------------------------------------------------------------------------- /motionPlan/motionplan.cpp: -------------------------------------------------------------------------------- 1 | #include "motionplan.h" 2 | 3 | motionPlan::motionPlan() 4 | { 5 | 6 | } 7 | 8 | motionPlan::~motionPlan() 9 | { 10 | 11 | } 12 | 13 | void motionPlan::ini_path_data(const pathInitData &pathInit) 14 | { 15 | cmd_pos=pathInit.cmd_pos; remaining_pos=pathInit.cmd_pos;; 16 | v0=fabs(pathInit.v0); Vmax=fabs(pathInit.VMax); ve=fabs(pathInit.ve); 17 | maxAcc=fabs(pathInit.max_acc); Jerk=fabs(pathInit.Jerk); 18 | deltaT=fabs(pathInit.deltaT); 19 | 20 | path_busy=true; can_do_path=false; 21 | decInit=false; dec_finished=false; path_pause_cmd=false; 22 | 23 | vs=v0; val_Auv=0; 24 | preView_phase=0; preViewDone=false; 25 | 26 | dec_move_pos=0; cur_move_pos=0; recordTime=0; 27 | 28 | ficureTime[0]=0; acc_t=0; 29 | moveDir = (cmd_pos >= 0) ? 1 : -1; 30 | judge_path_condition(); 31 | #ifdef motionDebug 32 | motionStatusMsg.clear(); 33 | #endif 34 | } 35 | 36 | bool motionPlan::pathBusy()//判断是否在规划中 37 | { 38 | if(path_busy) return true; 39 | else return false; 40 | } 41 | 42 | void motionPlan::judge_path_condition()//规划前进行初始条件判断 43 | { 44 | if(fabs(Vmax*deltaT/cmd_pos)>maxProportion) 45 | Vmax=fabs(maxProportion*cmd_pos/deltaT); 46 | if(VmaxcoverAccuracy){//初速度和末速度不相等的情况下 49 | double cal_ve=0,minDis=0,vmin=0; 50 | if(vs>ve) 51 | cal_ve=vs-Jerk*pow(deltaT,2); 52 | else 53 | cal_ve=vs+Jerk*pow(deltaT,2); 54 | minDis=(vs+cal_ve)*deltaT; 55 | vmin=qMax(vs,cal_ve); 56 | if(fabs(cmd_pos)fabs(cmd_pos)){//需要修正末速度 63 | double veTmp=ve,vsTmp=vs,vCenter=ve,minus_dis=0; 64 | bool fixFinished=false; 65 | while(!fixFinished){ 66 | vCenter=(veTmp+vsTmp)/2; 67 | get_time_point(vCenter,cal_vmax,cal_maxAcc,t1_tmp,t2_tmp,moveDis); 68 | if(fabs(minus_dis-(moveDis-fabs(cmd_pos)))fabs(cmd_pos)) veTmp=vCenter; 79 | else vsTmp=vCenter; 80 | } 81 | } 82 | } 83 | can_do_path=true;//末速度已满足要求，可以S型速度规划 84 | } 85 | } 86 | else{//初速度和末速度相等的情况下 87 | double cal_minDis=(vs+0.5*Jerk*pow(deltaT,2))*4.0*deltaT; 88 | double cal_vmin=vs+Jerk*pow(deltaT,2); 89 | if(cal_minDis>fabs(cmd_pos) || cal_vmin>Vmax || Jerk*deltaT>maxAcc){//距离太短或最大加速度太小或最大速度太小，退出S型速度规划 90 | can_do_path=false; 91 | } 92 | else can_do_path=true;//可以S型速度规划 93 | } 94 | if(can_do_path) emit sendMsg(tr("开始速度规划...\n参数满足7段S型速度规划条件!")); 95 | else emit sendMsg(tr("开始速度规划...\n参数无法满足7段S型速度规划条件!\n被迫进行匀速运动!")); 96 | } 97 | 98 | bool motionPlan::performPath(bool path_pause)//执行规划 99 | { 100 | if(!path_busy){ 101 | emit sendMsg(tr("没初始化参数,无法进行计算,请先初始化参数!")); 102 | return false; 103 | } 104 | if(can_do_path){//S型规划模式 105 | if(!path_pause_cmd || dec_finished) 106 | path_pause_cmd=path_pause; 107 | if(dec_finished){ 108 | if(path_pause_cmd) 109 | return false; 110 | else 111 | dec_finished=false; 112 | } 113 | else{ 114 | if(path_pause_cmd & !decInit){ 115 | if(val_Auv>0){//减速初始化 116 | double cal_vmax,cal_maxAcc,t1_tmp,t2_tmp,moveDis; 117 | get_time_point(dec_ve,cal_vmax,cal_maxAcc,t1_tmp,t2_tmp,moveDis); 118 | get_ficure_point(cal_vmax,dec_ve,t1_tmp,t2_tmp); 119 | preViewDone=false; 120 | preView_phase=2; 121 | } 122 | decInit=true; 123 | } 124 | } 125 | } 126 | else{//匀速模式 127 | path_pause_cmd=path_pause; 128 | if(path_pause_cmd) return false; 129 | } 130 | doMotionPlan(can_do_path); 131 | return true; 132 | } 133 | 134 | void motionPlan::doMotionPlan(bool doPath)//进入运动模式 135 | { 136 | #ifdef motionDebug 137 | static QString motionMsg; 138 | static bool canSendMsg=false; 139 | canSendMsg=false; 140 | #endif 141 | static bool haveMove=false; 142 | if(doPath){ 143 | if(!preViewDone){ 144 | haveMove=false; 145 | switch(preView_phase){ 146 | case 0:{//加加速运动 147 | if(preView(positiveAcceleration)){ 148 | #ifdef motionDebug 149 | if(motionStatusMsg!=tr("开始加加速运动阶段:\n")){ 150 | motionMsg=tr("开始加加速运动阶段:\n"); 151 | canSendMsg=true; 152 | } 153 | #endif 154 | movePreViewPos(positiveAcceleration); 155 | } 156 | else{ 157 | if(preView(steadyAcceleration)){ 158 | #ifdef motionDebug 159 | if(motionStatusMsg!=tr("开始匀速运动阶段:\n")){ 160 | motionMsg=tr("开始匀速运动阶段:\n"); 161 | canSendMsg=true; 162 | } 163 | #endif 164 | movePreViewPos(steadyAcceleration); 165 | preView_phase=1; 166 | } 167 | else{ 168 | preFicure(); 169 | if(acc_t!=0){ 170 | #ifdef motionDebug 171 | if(motionStatusMsg!=tr("开始减加速运动阶段:\n")){ 172 | motionMsg=tr("开始减加速运动阶段:\n"); 173 | canSendMsg=true; 174 | } 175 | #endif 176 | haveMove=true; 177 | movePreViewPos(negetiveAcceleration); 178 | acc_t--; 179 | } 180 | if(acc_t==0) preViewDone=true; 181 | else preView_phase=2; 182 | if(!haveMove){ 183 | if(acc_t==0) goto lastMove; 184 | else goto decAcc; 185 | } 186 | } 187 | } 188 | }break; 189 | case 1:{//匀加速运动 190 | if(preView(steadyAcceleration)){ 191 | #ifdef motionDebug 192 | if(fabs(val_Auv)ficureTime[3]) 252 | haveData=false; 253 | else 254 | haveData=true; 255 | if(!haveData) 256 | recordTime=ficureTime[3]+ficureTimeRecord; 257 | 258 | if(recordTime-ficureTimeRecord>ficureTime[2]){ 259 | #ifdef motionDebug 260 | if(ficureMoveDir>0){ 261 | if(motionStatusMsg!=tr("开始减减运动阶段:\n")){ 262 | motionMsg=tr("开始减减运动阶段:\n"); 263 | canSendMsg=true; 264 | } 265 | } 266 | else{ 267 | if(motionStatusMsg!=tr("开始减加运动阶段:\n")){ 268 | motionMsg=tr("开始减加运动阶段:\n"); 269 | canSendMsg=true; 270 | } 271 | } 272 | #endif 273 | timeTmp=recordTime-ficureTimeRecord-ficureTime[2]; 274 | moveFicurePos(positiveAcceleration,timeTmp); 275 | } 276 | else if(recordTime-ficureTimeRecord>ficureTime[1]){ 277 | #ifdef motionDebug 278 | if(val_Auv>0){ 279 | if(motionStatusMsg!=tr("开始匀加运动阶段:\n")){ 280 | motionMsg=tr("开始匀加运动阶段:\n"); 281 | canSendMsg=true; 282 | } 283 | } 284 | else{ 285 | if(motionStatusMsg!=tr("开始匀减运动阶段:\n")){ 286 | motionMsg=tr("开始匀减运动阶段:\n"); 287 | canSendMsg=true; 288 | } 289 | } 290 | #endif 291 | timeTmp=recordTime-ficureTimeRecord-ficureTime[1]; 292 | moveFicurePos(steadyAcceleration,timeTmp); 293 | } 294 | else if(recordTime-ficureTimeRecord>ficureTime[0]){ 295 | #ifdef motionDebug 296 | if(ficureMoveDir>0){ 297 | if(motionStatusMsg!=tr("开始加减运动阶段:\n")){ 298 | motionMsg=tr("开始加减运动阶段:\n"); 299 | canSendMsg=true; 300 | } 301 | } 302 | else{ 303 | if(motionStatusMsg!=tr("开始加加运动阶段:\n")){ 304 | motionMsg=tr("开始加加运动阶段:\n"); 305 | canSendMsg=true; 306 | } 307 | } 308 | #endif 309 | timeTmp=recordTime-ficureTimeRecord-ficureTime[0]; 310 | moveFicurePos(negetiveAcceleration,timeTmp); 311 | } 312 | else{ 313 | #ifdef motionDebug 314 | if(motionStatusMsg!=tr("开始匀速运动阶段:\n")){ 315 | motionMsg=tr("开始匀速运动阶段:\n"); 316 | canSendMsg=true; 317 | } 318 | #endif 319 | timeTmp=recordTime-ficureTimeRecord; 320 | cur_move_pos=moveDir *vs*timeTmp; 321 | } 322 | cur_move_pos-=recordDis; 323 | recordDis+=cur_move_pos; 324 | if(!haveData){ 325 | if(path_pause_cmd && fabs(remaining_pos-cur_move_pos)>coverAccuracy){ 326 | dec_finished=true; 327 | decInit=false; 328 | dec_move_pos+=(cmd_pos-remaining_pos+cur_move_pos); 329 | cmd_pos=remaining_pos-cur_move_pos; 330 | v0=vs; 331 | val_Auv=0; 332 | preView_phase=0; 333 | preViewDone=false; 334 | acc_t=0; 335 | ficureTime[0]=0; 336 | judge_path_condition(); 337 | } 338 | else 339 | path_busy=false; 340 | } 341 | } 342 | } 343 | else{ 344 | if((moveDir*Vmax*deltaT)/(cmd_pos+dec_move_pos)>maxProportion) 345 | cur_move_pos=maxProportion*(cmd_pos+dec_move_pos); 346 | else 347 | cur_move_pos=moveDir*Vmax*deltaT; 348 | recordTime+=deltaT; 349 | if(fabs(remaining_pos)Vmax || current_acc>maxAcc) return false; 390 | else if(((moveDir*cal_vmax*deltaT)/(cmd_pos+dec_move_pos))>maxProportion) return false; 391 | else{ 392 | move_dis+=moveDir*(current_vs*current_acc/Jerk+0.5*current_acc*pow(current_acc/Jerk,2)-Jerk*pow(current_acc/Jerk,3)*1.0/6.0); 393 | double cur_remaining_pos=remaining_pos-move_dis; 394 | double cal_maxAcc=sqrt(fabs(cal_vmax-ve)*Jerk); 395 | if(cal_maxAcc>maxAcc) 396 | cal_maxAcc=maxAcc; 397 | cal_maxAcc=(static_cast((cal_maxAcc+coverAccuracy)/(Jerk*deltaT)))*Jerk*deltaT; 398 | double t1_tmp=cal_maxAcc/Jerk; 399 | double t2_tmp=0; 400 | if(fabs(cal_maxAcc)>coverAccuracy) 401 | t2_tmp=(fabs(cal_vmax-ve)-pow(cal_maxAcc,2)/Jerk)/cal_maxAcc; 402 | double moveDis=(ve+cal_vmax)*t1_tmp+(cal_vmax+ve)*t2_tmp/2; 403 | if(fabs(moveDis)<=fabs(cur_remaining_pos)) return true; 404 | else return false; 405 | } 406 | } 407 | 408 | void motionPlan::preFicure()//全部前瞻完,结束前瞻 409 | { 410 | double cal_acc_dis=moveDir*(vs*val_Auv/Jerk+0.5*val_Auv*pow(val_Auv/Jerk,2)-Jerk*pow(val_Auv/Jerk,3)*1.0/6.0); 411 | double moveDis,cal_vmax,cal_maxAcc,t1_tmp,t2_tmp; 412 | get_time_point(ve,cal_vmax,cal_maxAcc,t1_tmp,t2_tmp,moveDis); 413 | ficureTime[0]=fabs(moveDis-fabs(remaining_pos-cal_acc_dis))/cal_vmax; 414 | get_ficure_point(cal_vmax,ve,t1_tmp,t2_tmp); 415 | } 416 | 417 | void motionPlan::get_time_point(const double vEnd,double &cal_vmax,double &cal_maxAcc,double &t1_tmp,double &t2_tmp,double &moveDis)//获取到达最大速度的时间,最大加速度,以及后半程各阶段时间 418 | { 419 | acc_t=static_cast((val_Auv+coverAccuracy)/Jerk/deltaT); 420 | cal_vmax=vs+val_Auv*val_Auv/Jerk-0.5*Jerk*pow(val_Auv/Jerk,2); 421 | cal_maxAcc=sqrt(fabs(cal_vmax-vEnd)*Jerk); 422 | if(cal_maxAcc>maxAcc) 423 | cal_maxAcc=maxAcc; 424 | cal_maxAcc=(static_cast((cal_maxAcc+coverAccuracy)/(Jerk*deltaT)))*Jerk*deltaT; 425 | t1_tmp=cal_maxAcc/Jerk; 426 | t2_tmp=0; 427 | if(fabs(cal_maxAcc)>coverAccuracy) 428 | t2_tmp=(fabs(cal_vmax-vEnd)-pow(cal_maxAcc,2)/Jerk)/cal_maxAcc; 429 | moveDis=(cal_vmax+vEnd)*t1_tmp+(cal_vmax+vEnd)*t2_tmp/2; 430 | } 431 | 432 | void motionPlan::get_ficure_point(const double vBegin,const double vEnd,const double t1_tmp,const double t2_tmp)//获取后半程时间,位移,速度,加速度节点信息 433 | { 434 | if(vBegin>vEnd) ficureMoveDir=1; 435 | else ficureMoveDir=-1; 436 | 437 | recordDis=0; 438 | 439 | ficureTimeRecord=recordTime+acc_t*deltaT; 440 | ficureTime[1]=t1_tmp+ficureTime[0]; 441 | ficureTime[2]=ficureTime[1]+t2_tmp; 442 | ficureTime[3]=ficureTime[2]+t1_tmp; 443 | 444 | ficureV[0]=vBegin; 445 | ficureAcc[0]=0; 446 | ficureDis[0]=moveDir*ficureV[0]*ficureTime[0]; 447 | 448 | ficureV[1]=ficureV[0]+0.5*(ficureAcc[0] + ficureAcc[0]-ficureMoveDir*Jerk * t1_tmp) * t1_tmp; 449 | ficureAcc[1]=ficureAcc[0]-ficureMoveDir*Jerk*t1_tmp; 450 | ficureDis[1]=ficureDis[0]+moveDir *(ficureV[0]*t1_tmp+0.5*ficureAcc[0]*pow(t1_tmp,2)-ficureMoveDir*1.0/6.0*Jerk*pow(t1_tmp,3)); 451 | 452 | ficureV[2]=ficureV[1]+ficureAcc[1]*t2_tmp; 453 | ficureAcc[2]=ficureAcc[1]; 454 | ficureDis[2]=ficureDis[1]+moveDir *(ficureV[0]+ve)*t2_tmp/2; 455 | } 456 | 457 | void motionPlan::moveFicurePos(enumType motionType,const double timeTmp) 458 | { 459 | switch (motionType){ 460 | case positiveAcceleration:{ 461 | cur_move_pos=ficureDis[2]+moveDir *(ficureV[2]*timeTmp+0.5*ficureAcc[2]*pow(timeTmp,2)+ficureMoveDir*1.0/6.0*Jerk*pow(timeTmp,3)); 462 | vs=ficureV[2]+0.5*(ficureAcc[2] + ficureAcc[2]+ficureMoveDir*Jerk * timeTmp) * timeTmp; 463 | val_Auv =ficureAcc[2]+ ficureMoveDir*Jerk * timeTmp; 464 | }break; 465 | 466 | case steadyAcceleration:{ 467 | cur_move_pos=ficureDis[1]+moveDir *(ficureV[1]*timeTmp+0.5*ficureAcc[1]*pow(timeTmp,2)); 468 | vs=ficureV[1]+ficureAcc[1] * timeTmp; 469 | }break; 470 | case negetiveAcceleration:{ 471 | cur_move_pos=ficureDis[0]+moveDir *(ficureV[0]*timeTmp+0.5*ficureAcc[0]*pow(timeTmp,2)-ficureMoveDir*1.0/6.0*Jerk*pow(timeTmp,3)); 472 | vs=ficureV[0]+0.5*(ficureAcc[0] + ficureAcc[0]-ficureMoveDir*Jerk * timeTmp) * timeTmp; 473 | val_Auv =ficureAcc[0]- ficureMoveDir*Jerk * timeTmp; 474 | }break;} 475 | } 476 | 477 | void motionPlan::movePreViewPos(enumType motionType)//下一时刻运动计算 478 | { 479 | switch (motionType){ 480 | case positiveAcceleration:{//加速度绝对值增大 481 | cur_move_pos=moveDir *(vs*deltaT+0.5*val_Auv*pow(deltaT,2)+1.0/6.0*Jerk*pow(deltaT,3)); 482 | vs+=0.5*(val_Auv + val_Auv+Jerk * deltaT) * deltaT; 483 | val_Auv += Jerk * deltaT; 484 | }break; 485 | case steadyAcceleration:{//加速度不变 486 | cur_move_pos=moveDir *(vs*deltaT+0.5*val_Auv*pow(deltaT,2)); 487 | vs+=val_Auv * deltaT; 488 | //val_Auv = val_Auv; 489 | }break; 490 | case negetiveAcceleration:{//加速度绝对值减小 491 | cur_move_pos=moveDir *(vs*deltaT+0.5*val_Auv*pow(deltaT,2)-1.0/6.0*Jerk*pow(deltaT,3)); 492 | vs+=0.5*(val_Auv + val_Auv-Jerk * deltaT) * deltaT; 493 | val_Auv -= Jerk * deltaT; 494 | }break;} 495 | recordTime+=deltaT; 496 | } 497 | 498 | void motionPlan::get_move_msg(double &curProportion,double &curPos_m,double &curVs_m, double &curAcc_m,double &curTime_m)//获得当前所走位移占比、总位移、速度、加速度、时间 499 | { 500 | if(fabs(cmd_pos+dec_move_pos)<=coverAccuracy) 501 | curProportion=0; 502 | else 503 | curProportion=(cmd_pos-remaining_pos+dec_move_pos)/(cmd_pos+dec_move_pos); 504 | curPos_m=cmd_pos-remaining_pos+dec_move_pos; 505 | curVs_m=vs; 506 | curAcc_m=val_Auv; 507 | curTime_m=recordTime; 508 | } 509 | 510 | #ifdef motionDebug 511 | void motionPlan::get_move_msg(QString &motionMsg) 512 | { 513 | motionMsg+=tr("当前位移:")+QString::number(cmd_pos-remaining_pos+dec_move_pos,'f',6)+"\n"; 514 | motionMsg+=tr("当前速度:")+QString::number(vs)+"\n"; 515 | motionMsg+=tr("当前加速度:")+QString::number(val_Auv)+"\n"; 516 | motionMsg+=tr("当前时间:")+QString::number(recordTime)+"\n"; 517 | } 518 | #endif 519 | -------------------------------------------------------------------------------- /motionPlan/motionplan.h: -------------------------------------------------------------------------------- 1 | #ifndef S_MOTIONPLAN_H 2 | #define S_MOTIONPLAN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct pathInitData{ 10 | double max_acc=0;//最大加速度 11 | double Jerk=0 ;//加加速度 12 | double cmd_pos=0 ;//位移 13 | double v0=0;//初速度 14 | double VMax=0;//最大速度 15 | double ve=0;//末速度 16 | double deltaT=0.001;//规划时最小时间单位(默认0.001s) 17 | }; 18 | 19 | class motionPlan : public QObject 20 | { 21 | Q_OBJECT 22 | public: 23 | motionPlan(); 24 | ~motionPlan(); 25 | bool performPath(bool path_pause=false);//执行规划 26 | #ifdef motionDebug 27 | void get_move_msg(QString &motionMsg); 28 | #endif 29 | void get_move_msg(double &curProportion,double &curPos_m,double &curVs_m, double &curAcc_m,double &curTime_m);//获得当前所走总位移、速度、加速度、时间 30 | void ini_path_data(const pathInitData &pathInit);//初始化参数 31 | bool pathBusy();//判断是否在规划中 32 | 33 | signals: 34 | void sendMsg(const QString &);//发送调试信息 35 | 36 | private: 37 | double deltaT=0.001; 38 | 39 | const double coverAccuracy=pow(10,-12);//弥补double精度 40 | const double maxProportion=1; 41 | 42 | #ifdef motionDebug 43 | QString motionStatusMsg; 44 | #endif 45 | 46 | double cmd_pos=0,remaining_pos=0,dec_move_pos=0,cur_move_pos=0; 47 | double v0=0,Vmax=0,ve=0,vs=0,dec_ve=0; 48 | double maxAcc=0,Jerk=0,val_Auv=0; 49 | double recordTime=0,ficureTimeRecord=0,recordDis=0; 50 | 51 | bool decInit=false, dec_finished=false,path_pause_cmd=false; 52 | bool path_busy=false,can_do_path=false; 53 | 54 | uint preView_phase=0; 55 | bool preViewDone=false; 56 | 57 | int moveDir=0; 58 | int ficureMoveDir=0; 59 | 60 | int acc_t=0; 61 | double ficureTime[4]={0,0,0,0}; 62 | double ficureV[3]={0,0,0}; 63 | double ficureAcc[3]={0,0,0}; 64 | double ficureDis[3]={0,0,0}; 65 | 66 | enum enumType{positiveAcceleration,steadyAcceleration,negetiveAcceleration}; 67 | void judge_path_condition();//进行规划条件判断 68 | void doMotionPlan(bool doPath);//运行单位时间计算 69 | bool preView(enumType motionType);//前瞻下一个单位时间,并做出判断 70 | void preFicure();//前瞻完成,将后半程各个阶段的信息计算出来 71 | void movePreViewPos(enumType motionType);//前瞻后运动一个单位时间的计算 72 | void moveFicurePos(enumType motionType, const double time);//后半程运动一个单位时间的计算 73 | void get_time_point(const double vEnd,double &cal_vmax,double &cal_maxAcc,double &t1_tmp,double &t2_tmp,double &moveDis);//获取到达最大速度的时间,最大加速度,以及后半程各阶段时间 74 | void get_ficure_point(const double vBegin,const double vEnd,const double t1_tmp,const double t2_tmp);//获取后半程时间,位移,速度,加速度节点信息 75 | }; 76 | 77 | 78 | #endif // S_MOTIONPLAN_H 79 | -------------------------------------------------------------------------------- /motionPlan/mychart.cpp: -------------------------------------------------------------------------------- 1 | #include "mychart.h" 2 | #include "ui_mychart.h" 3 | 4 | myChart::myChart(uchar type, QWidget *parent) : 5 | QWidget(parent), 6 | chartType(static_cast(type)), 7 | ui(new Ui::myChart) 8 | { 9 | ui->setupUi(this); 10 | setMouseTracking(true); 11 | ui->chartView->setAttribute(Qt::WA_TransparentForMouseEvents, true); 12 | switch (chartType) { 13 | case disChartType:setWindowTitle("位移时间曲线");break; 14 | case spdChartType:setWindowTitle("速度时间曲线");break; 15 | case accChartType:setWindowTitle("加速度时间曲线");break; 16 | } 17 | init_chart(); 18 | } 19 | 20 | myChart::~myChart() 21 | { 22 | delete ui; 23 | } 24 | 25 | void myChart::rec_data(double realData, double allData, double vs, double acc,double time) 26 | { 27 | QLineSeries *series=qobject_cast(ui->chartView->chart()->series().at(0)); 28 | static double chartData=0; 29 | static bool outRange=false; 30 | 31 | switch (chartType) { 32 | case disChartType:{ 33 | chartData=allData; 34 | series->setName("耗时:"+QString::number(time)+"秒 总位移:"+QString::number(allData)); 35 | }break; 36 | case spdChartType:{ 37 | chartData=vs; 38 | series->setName("耗时:"+QString::number(time)+"秒 当前速度:"+QString::number(vs)); 39 | }break; 40 | case accChartType:{ 41 | chartData=acc; 42 | series->setName("耗时:"+QString::number(time)+"秒 当前加速度:"+QString::number(acc)); 43 | }break;} 44 | 45 | series->append(time,chartData); 46 | if(time<=rangeX0){ 47 | rangeX0=time; 48 | outRange=true; 49 | } 50 | else if(time>=rangeX1){ 51 | rangeX1=time; 52 | outRange=true; 53 | } 54 | if(chartData<=rangeY0){ 55 | rangeY0=chartData; 56 | outRange=true; 57 | } 58 | else if(chartData>=rangeY1){ 59 | rangeY1=chartData; 60 | outRange=true; 61 | } 62 | if(outRange){ 63 | ui->chartView->chart()->axisX()->setRange(rangeX0,rangeX1); 64 | ui->chartView->chart()->axisY()->setRange(rangeY0,rangeY1); 65 | outRange=false; 66 | } 67 | } 68 | 69 | void myChart::init_chart() 70 | { 71 | QChart *chart=new QChart(); 72 | ui->chartView->setChart(chart); 73 | ui->chartView->setRenderHint(QPainter::Antialiasing); 74 | QPen pen; 75 | pen.setStyle(Qt::SolidLine); 76 | pen.setWidth(2); 77 | QValueAxis *axisX=new QValueAxis; 78 | axisX->setLabelFormat("%0.3f"); 79 | axisX->setTickCount(0); 80 | axisX->setTitleText("时间(秒)"); 81 | QValueAxis *axisY=new QValueAxis; 82 | axisY->setLabelFormat("%0.3f"); 83 | axisY->setTickCount(0); 84 | switch (chartType) { 85 | case disChartType:{ 86 | axisY->setTitleText("位移"); 87 | pen.setColor(Qt::red); 88 | }break; 89 | case spdChartType:{ 90 | axisY->setTitleText("速度"); 91 | pen.setColor(Qt::green); 92 | }break; 93 | case accChartType:{ 94 | axisY->setTitleText("加速度"); 95 | pen.setColor(Qt::blue); 96 | }break;} 97 | QLineSeries *series=new QLineSeries; 98 | series->setName("耗时"); 99 | series->setPen(pen); 100 | chart->addSeries(series); 101 | chart->setAxisX(axisX,series); 102 | chart->setAxisY(axisY,series); 103 | axisX->setRange(rangeX0,rangeX1); 104 | axisY->setRange(rangeY0,rangeY1); 105 | } 106 | 107 | void myChart::mousePressEvent(QMouseEvent *event) 108 | { 109 | if (event->button() == Qt::LeftButton){ 110 | drag_flag = true; 111 | m_lastPointF=event->pos(); 112 | setCursor(Qt::OpenHandCursor); 113 | } 114 | } 115 | 116 | void myChart::mouseReleaseEvent(QMouseEvent *event) 117 | { 118 | drag_flag = false; 119 | setCursor(Qt::ArrowCursor); 120 | } 121 | 122 | void myChart::mouseMoveEvent(QMouseEvent *event) 123 | { 124 | if (drag_flag){ 125 | QPoint curPos = event->pos(); 126 | QPoint offset = curPos - m_lastPointF; 127 | m_lastPointF = curPos; 128 | ui->chartView->chart()->scroll(-offset.x(), offset.y()); 129 | ui->chartView->chart()->update(); 130 | } 131 | } 132 | 133 | void myChart::wheelEvent(QWheelEvent *event) 134 | { 135 | const QPoint curPos = event->pos(); 136 | QPointF curVal = ui->chartView->chart()->mapToValue(QPointF(curPos)); 137 | const double factor = 1.1;//缩放比例 138 | QValueAxis *axisX=(QValueAxis *)ui->chartView->chart()->axisX(); 139 | QValueAxis *axisY=(QValueAxis *)ui->chartView->chart()->axisY(); 140 | const double xMin = axisX->min(); 141 | const double xMax = axisX->max(); 142 | const double yMin = axisY->min(); 143 | const double yMax = axisY->max(); 144 | const double xCentral = curVal.x(); 145 | const double yCentral = curVal.y(); 146 | double leftOffset=0,rightOffset=0,bottomOffset=0,topOffset=0; 147 | if (event->delta() > 0){//放大 148 | bottomOffset = 1.0 / factor * (yCentral - yMin); 149 | topOffset = 1.0 / factor * (yMax - yCentral); 150 | leftOffset = 1.0 / factor * (xCentral - xMin); 151 | rightOffset = 1.0 / factor * (xMax - xCentral); 152 | } 153 | else{//缩小 154 | bottomOffset = 1.0 * factor * (yCentral - yMin); 155 | topOffset = 1.0 * factor * (yMax - yCentral); 156 | leftOffset = 1.0 * factor * (xCentral - xMin); 157 | rightOffset = 1.0 * factor * (xMax - xCentral); 158 | } 159 | rangeX0=xCentral - leftOffset; 160 | rangeX1=xCentral + rightOffset; 161 | rangeY0=yCentral - bottomOffset; 162 | rangeY1=yCentral + topOffset; 163 | 164 | ui->chartView->chart()->axisX()->setRange(rangeX0,rangeX1); 165 | ui->chartView->chart()->axisY()->setRange(rangeY0,rangeY1); 166 | ui->chartView->chart()->update(); 167 | } 168 | 169 | void myChart::clearData() 170 | { 171 | QLineSeries *series=qobject_cast(ui->chartView->chart()->series().at(0)); 172 | series->clear(); 173 | } 174 | 175 | void myChart::showData(bool showPoint) 176 | { 177 | QLineSeries *series=qobject_cast(ui->chartView->chart()->series().at(0)); 178 | if(showPoint){ 179 | series->setPointsVisible(false); 180 | series->setPointLabelsVisible(false); 181 | } 182 | else{ 183 | series->setPointsVisible(true); 184 | series->setPointLabelsVisible(true); 185 | } 186 | } 187 | 188 | 189 | -------------------------------------------------------------------------------- /motionPlan/mychart.h: -------------------------------------------------------------------------------- 1 | #ifndef MYCHART_H 2 | #define MYCHART_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace QtCharts; 10 | 11 | namespace Ui { 12 | class myChart; 13 | } 14 | 15 | class myChart : public QWidget 16 | { 17 | Q_OBJECT 18 | 19 | public: 20 | explicit myChart(uchar type,QWidget *parent = nullptr); 21 | ~myChart(); 22 | void clearData(); 23 | void showData(bool showPoint); 24 | enum chartShowType{disChartType=0,spdChartType,accChartType}; 25 | 26 | protected: 27 | void wheelEvent(QWheelEvent* event); 28 | 29 | void mousePressEvent(QMouseEvent *event); 30 | 31 | void mouseMoveEvent(QMouseEvent *event); 32 | 33 | void mouseReleaseEvent(QMouseEvent *event); 34 | 35 | public slots: 36 | void rec_data(double, double, double , double , double); 37 | 38 | private: 39 | void init_chart(); 40 | 41 | chartShowType chartType; 42 | bool drag_flag=false; 43 | QPoint m_lastPointF; 44 | double rangeX0=0,rangeX1=1,rangeY0=0,rangeY1=1; 45 | 46 | Ui::myChart *ui; 47 | }; 48 | 49 | #endif // MYCHART_H 50 | -------------------------------------------------------------------------------- /motionPlan/mychart.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | myChart 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 300 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 0 19 | 20 | 21 | 0 22 | 23 | 24 | 0 25 | 26 | 27 | 0 28 | 29 | 30 | 0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | QChartView 40 | QGraphicsView 41 |
qchartview.h
42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /motionPlan/pic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhangming-wang/motionPlan/15146bad7fbaac76512e9e99ca02bafdc6b03520/motionPlan/pic.png --------------------------------------------------------------------------------