|
发表于 2019-2-24 22:18:03
|
显示全部楼层
我也分享一个之前做的S加减速曲线计算方法
- /*
- 主计算函数
- 本来想做成一个通用的函数,但是目前手上的项目需求很明确,时间也不多,所以临时做了个版本
- 该函数并不具有通用性,几个参数考虑的重要程度不一样,会有不同的取舍,我是按下面这几条思想做的,供大家参考,有发现不合理的地方请大家指正
- 计算中优先考虑的是运动距离必须跟用户设定的保持一致
- 另外四个变加速过程的捷度被认为是受动力源和机械结构限制,所以也保持跟用户设定一致,不能修改
- 其他参数在计算过程中发现逻辑冲突的,会优先考虑修改最大加减速
- 加减速参数满足条件但用户设定行程比较短,会考虑使用一个比用户设定速度更小的参数计算,直到满足要求
- */
- void MainWindow::Update(bool isV, bool isSave, bool PorV)
- {
- /*计算结果缓存,笨办法开两个比较大的数组*/
- QVector<double> x(1024*1024),v(1024*1024),s(1024*1024);
-
- ui->txLog->clear();
-
- /*读取用户设定参数*/
- TimeStep = ui->nTimeStep->text().toDouble() * 0.000001; //s
- Smax = ui->nS->text().toDouble();
- Vmax = ui->nVmax->text().toDouble();
- ACCmax = ui->nACCmax->text().toDouble();
- DCCmax = ui->nDCCmax->text().toDouble();
- J1 = ui->nJ1->text().toDouble();
- J2 = ui->nJ2->text().toDouble();
- J3 = ui->nJ3->text().toDouble();
- J4 = ui->nJ4->text().toDouble();
-
- int xcount = 0;
-
- ui->txLog->append("Calculate begin.");
- ui->txLog->append("----");
-
- double S1,S2,S3,S4,S5,S6,S7;
- double V1,V2,V3,V4;
- int PlotPointer = 0;
- bool fFirst = true;
- bool fS4Neg = false;
- do
- {
- /*判断用户设定的捷度加减速度和最大速度能不能使各段曲线衔接上*/
- V1 = (ACCmax*ACCmax)/(2*J1);
- V2 = Vmax - (ACCmax*ACCmax)/(2*J2);
- V3 = Vmax - (DCCmax*DCCmax)/(2*J3);
- V4 = (DCCmax*DCCmax)/(2*J4);
- if(fFirst)
- {
- ui->txLog->append("Velocity at end of S1 (V1): "+QString::number(V1, 10, 8));
- ui->txLog->append("Velocity at begin of S3 (V2): "+QString::number(V2, 10, 8));
- ui->txLog->append("Velocity at end of S5 (V3): :"+QString::number(V3, 10, 8));
- ui->txLog->append("Velocity at begin of S7 (V4): "+QString::number(V4, 10, 8));
- }
- if(V2<V1)//加速度出现曲线冲突,修改最大加速度
- {
- ACCmax = sqrt((2*J1*J2*Vmax)/(J1+J2));
- ui->nACCmax->setText(QString::number(ACCmax, 10, 8));
- V1 = (ACCmax*ACCmax)/(2*J1);
- V2 = Vmax - (ACCmax*ACCmax)/(2*J2);
- if(fFirst)
- {
- ui->txLog->append("----");
- ui->txLog->append("[-ERROR-]: V2 < V1, try to set a smaller ACCmax.");
- ui->txLog->append("First try V2 = V1, ACCmax = sqrt((2*J1*J2*Vmax)/(J1+J2)) = "+QString::number(ACCmax, 10, 8)+".");
- ui->txLog->append("Velocity at end of S1 (V1): "+QString::number(V1, 10, 8));
- ui->txLog->append("Velocity at begin of S3 (V2): "+QString::number(V2, 10, 8));
- }
- }
- if(V3<V4)//减速段出现曲线冲突,修改最大减速度
- {
- DCCmax = sqrt((2*J3*J4*Vmax)/(J3+J4));
- ui->nDCCmax->setText(QString::number(DCCmax, 10, 8));
- V3 = Vmax - (DCCmax*DCCmax)/(2*J3);
- V4 = (DCCmax*DCCmax)/(2*J4);
- if(fFirst)
- {
- ui->txLog->append("----");
- ui->txLog->append("[-ERROR-]: V3 < V4, try to set a smaller DCCmax.");
- ui->txLog->append("First try V3 = V4, DCCmax = sqrt((2*J3*J4*Vmax)/(J3+J4)) = "+QString::number(DCCmax, 10, 8)+".");
- ui->txLog->append("Velocity at end of S5 (V3): "+QString::number(V3, 10, 8));
- ui->txLog->append("Velocity at begin of S7 (V4): "+QString::number(V4, 10, 8));
- }
- }
-
- /*计算七段运动距离,判断是否符合用户设定的运动距离*/
- S1 = (ACCmax*ACCmax*ACCmax)/(6*J1*J1);
- S2 = (V2*V2 - V1*V1)/(2*ACCmax);
- S3 = Vmax*ACCmax/J2 - (ACCmax*ACCmax*ACCmax)/(6*J2*J2);
-
- S5 = Vmax*DCCmax/J3 - (DCCmax*DCCmax*DCCmax)/(6*J3*J3);
- S6 = (V3*V3 - V4*V4)/(2*DCCmax);
- S7 = (DCCmax*DCCmax*DCCmax)/(6*J4*J4);
-
- S4 = Smax - (S1+S2+S3+S5+S6+S7);
- /*第四段位移为负,说明加减速段位移太长,考虑减少最大速度使加减速段时间缩短,行程缩短*/
- if(S4 < 0)
- {
- fS4Neg = true;
- Vmax -=0.001;
- }
- fFirst = false;
- }while(S4 < 0);
-
- /*输出最终参与计算的运动参数*/
- ui->nVmax->setText(QString::number(Vmax, 10, 8));
- if(fS4Neg)
- {
- ui->txLog->append("----");
- ui->txLog->append("[-ERROR-]: Smax too small, try smaller Vmax = "+QString::number(Vmax, 10, 8));
- ui->txLog->append("Recalc with new Smax.");
-
- ui->txLog->append("ACCmax = "+QString::number(ACCmax, 10, 8)+".");
- ui->txLog->append("Velocity at end of S1 (V1): "+QString::number(V1, 10, 8));
- ui->txLog->append("Velocity at begin of S3 (V2): "+QString::number(V2, 10, 8));
- ui->txLog->append("DCCmax = "+QString::number(DCCmax, 10, 8)+".");
- ui->txLog->append("Velocity at end of S5 (V3): "+QString::number(V3, 10, 8));
- ui->txLog->append("Velocity at begin of S7 (V4): "+QString::number(V4, 10, 8));
- }
- ui->txLog->append("----");
- ui->txLog->append("Fianl S1~S7 : "+QString::number(S1, 10, 8)+", "+QString::number(S2, 10, 8)
- +", "+QString::number(S3, 10, 8)+", "+QString::number(S4, 10, 8)
- +", "+QString::number(S5, 10, 8)+", "+QString::number(S6, 10, 8)+", "+QString::number(S7, 10, 8));
-
- /*计算各段时间*/
- double T1,T2,T3,T4,T5,T6,T7;
- T1 = ACCmax/J1;
- T2 = (V2-V1)/ACCmax;
- T3 = ACCmax/J2;
- T4 = S4/Vmax;
- T5 = DCCmax/J3;
- T6 = (V3-V4)/DCCmax;
- T7 = DCCmax/J4;
- ui->txLog->append("Fianl T1~T7 : "+QString::number(T1, 10, 8)+", "+QString::number(T2, 10, 8)
- +", "+QString::number(T3, 10, 8)+", "+QString::number(T4, 10, 8)
- +", "+QString::number(T5, 10, 8)+", "+QString::number(T6, 10, 8)+", "+QString::number(T7, 10, 8));
-
-
- double AllCount = T1+T2+T3+T4+T5+T6+T7;
- if((AllCount/TimeStep)>1024*1024)
- {
- ui->txLog->append("----");
- ui->txLog->append("Data count exceed max buffer limit, please set a bigger TimeStep.");
- return;
- }
-
- int TempCount = 0;
- double TempTime = 0.0;
-
- double Astart = 0.0;
- double Vstart = 0.0;
- double Sstart = 0.0;
-
- /*分七段生成位移曲线和速度曲线*/
- while(Sstart < Smax)
- {
- //加加速 S=(J*T^3)/6 V=(J*T^2)/2
- if(TempTime <= T1)
- {
- Sstart = J1*TempTime*TempTime*TempTime/6.0;
- Vstart = 0.5*J1*TempTime*TempTime;
- }
- //匀加速 S=V0*T+(A*T^2)/2 V=V0+A*T
- else if(TempTime <= T1+T2)
- {
- Sstart = S1 + V1*(TempTime-T1) + 0.5*ACCmax*(TempTime-T1)*(TempTime-T1);
- Vstart = V1 + ACCmax*(TempTime-T1);
- }
- //减加速 曲线公式参考加加速段,需要做一点变换
- else if(TempTime <= T1+T2+T3)
- {
- Sstart = S1 + S2 + Vmax*(TempTime-T1-T2) - ((J2*T3*T3*T3/6)-(J2*(T1+T2+T3-TempTime)*(T1+T2+T3-TempTime)*(T1+T2+T3-TempTime)/6));
- Vstart = Vmax - (0.5*J2*(T1+T2+T3-TempTime)*(T1+T2+T3-TempTime));
- }
- //匀速
- else if(TempTime <= T1+T2+T3+T4)
- {
- Sstart = S1 + S2 + S3 + Vmax*(TempTime-T1-T2-T3);
- Vstart = Vmax;
- }
- //加减速 曲线公式参考加加速段,需要做一点变换
- else if(TempTime <= T1+T2+T3+T4+T5)
- {
- Sstart = S1 + S2 + S3 + S4 + Vmax*(TempTime-T1-T2-T3-T4) - J3*(TempTime-T1-T2-T3-T4)*(TempTime-T1-T2-T3-T4)*(TempTime-T1-T2-T3-T4)/6.0;
- Vstart = Vmax - 0.5*J3*(TempTime-T1-T2-T3-T4)*(TempTime-T1-T2-T3-T4);
- }
- //匀减速
- else if(TempTime <= T1+T2+T3+T4+T5+T6)
- {
- Sstart = S1 + S2 + S3 + S4 + S5 + V3*(TempTime-T1-T2-T3-T4-T5)-0.5*DCCmax*(TempTime-T1-T2-T3-T4-T5)*(TempTime-T1-T2-T3-T4-T5);
- Vstart = V3 - DCCmax*(TempTime-T1-T2-T3-T4-T5);
- }
- //减减速 曲线公式参考加加速段,需要做一点变换
- else
- {
- Sstart = Smax - J4*(T1+T2+T3+T4+T5+T6+T7-TempTime)*(T1+T2+T3+T4+T5+T6+T7-TempTime)*(T1+T2+T3+T4+T5+T6+T7-TempTime)/6.0;
- Vstart = 0.5*J4*(T1+T2+T3+T4+T5+T6+T7-TempTime)*(T1+T2+T3+T4+T5+T6+T7-TempTime);
- }
-
- x[TempCount] = TempCount;
- s[TempCount] = Sstart;
- v[TempCount] = Vstart;
- xcount = TempCount;
- TempTime += TimeStep;
- TempCount ++;
- }
-
- ui->txLog->append("----");
- ui->txLog->append("All data count : "+QString::number(xcount, 10, 0)+" data in "+QString::number(AllCount, 10, 8)+"s.");
-
-
- /*判断绘图区域绘制速度曲线还是位移曲线*/
- if(isV)
- {
- ui->customPlot->xAxis->setRange(0,xcount);
- ui->customPlot->yAxis->setRange(0,Vmax);
- ui->customPlot->graph(0)->setData(x,v);
- }
- else
- {
- ui->customPlot->xAxis->setRange(0,xcount);
- ui->customPlot->yAxis->setRange(0,Smax);
- ui->customPlot->graph(0)->setData(x,s);
- }
-
- ui->customPlot->replot();
- }
复制代码 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|