首先致谢！
感谢@xuuyann的两篇文章
机器人学回炉重造（5-2）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划_xuuyann的博客-CSDN博客_梯形加减速曲线S曲线C语言实现，利用robomodule+STM32F429+直流伺服电机进行简单验证_xuuyann的博客-CSDN博客_robomodule
目录
1首先.C文件做了添加
/* scurve.c */
#include "S_Profile.h"
SCurve_Handle_t *pSCurve[2]; //定义两个SCurve_Handle_t类型的指针
S_User_ParHandle_t *pSUser[2]; //定义两个S_User_ParHandle_t类型的指针
S_User_ParHandle_t pSUserM1 =
{
.preq1 = 0,
.deltaq = 0,
.refpos = 0,
.refvel = 0,
.refa = 0,
.refaa = 0,
.maxvel = 20000,
.maxa = 10000,
.maxaa = 10000,
};
S_User_ParHandle_t pSUserM2 =
{
.preq1 = 0,
.deltaq = 0,
.refpos = 0,
.refvel = 0,
.refa = 0,
.refaa = 0,
.maxvel = 20000,
.maxa = 10000,
.maxaa = 10000,
};
SCurve_Handle_t pSCurveM1 =
{
.Ta = 0,
.Tv = 0,
.Td = 0,
.Tj1 = 0,
.Tj2 = 0,
.q0 = 0,
.q1 = 0,
.v0 = 0,
.v1 = 0,
.vlim = 0,
.amax = 0,
.amin = 0,
.alima = 0,
.alimd = 0,
.jmax = 0,
.jmin = 0,
.User_Par = &pSUserM1,
};
SCurve_Handle_t pSCurveM2 =
{
.Ta = 0,
.Tv = 0,
.Td = 0,
.Tj1 = 0,
.Tj2 = 0,
.q0 = 0,
.q1 = 0,
.v0 = 0,
.v1 = 0,
.vlim = 0,
.amax = 0,
.amin = 0,
.alima = 0,
.alimd = 0,
.jmax = 0,
.jmin = 0,
.User_Par = &pSUserM2,
};
int sign(float q0, float q1)
{
int a = 0;
if (q1 - q0 > 0)
a = 1;
else
a = -1;
return a;
}
float Fq0,Fq1,Fv0,Fv1;
/*------------------------------------------------
Function:S曲线参数计算
Input :起始位置，终止位置，起始速度，终止速度，最大速度、最大加速度、最大加加速度
Output :No
Explain :No
------------------------------------------------*/
SCurve_Handle_t STrajectoryPara( SCurve_Handle_t *Input_data,float q0, float q1, float v0, float v1, float vmax, float amax, float jmax)
{
float vmin, amin, jmin;
int sigma = 0;
float Tj = 0, delta = 0;
vmin = -vmax;
amin = -amax;
jmin = -jmax;
// 利用公式（3.31）（3.32）转化得到实际的q_0、q_1、v_max、a_max
sigma = sign(q0, q1);
Input_data->User_Par->deltaq = sigma;
Input_data->q0 = sigma * q0;
Input_data->q1 = sigma * q1;
Input_data->v0 = sigma * v0;
Input_data->v1 = sigma * v1;
Fq0 = sigma * q0;
Fq1 = sigma * q1;
Fv0 = sigma * v0;
Fv1 = sigma * v1;
vmax = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
vmin = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
Input_data->amax = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
Input_data->amin = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
Input_data->jmax = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
Input_data->jmin = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;
// 判断是否达到最大速度
if ((vmax - Input_data->v0)*Input_data->jmax < pow(Input_data->amax, 2)) {
// 达不到amax
Input_data->Tj1 = sqrt((vmax - Input_data->v0)/Input_data->jmax);
Input_data->Ta = 2*Input_data->Tj1;
Input_data->alima = Input_data->jmax * Input_data->Tj1;
} else {
// 能够达到amax
Input_data->Tj1 = Input_data->amax/Input_data->jmax;
Input_data->Ta = Input_data->Tj1 + (vmax - Input_data->v0 )/Input_data->amax;
Input_data->alima = Input_data->amax;
}
if ((vmax - Input_data->v1)*Input_data->jmax < pow(Input_data->amax, 2)) {
// 达不到amin
Input_data->Tj2 = sqrt((vmax - Input_data->v1)/Input_data->jmax);
Input_data->Td = 2 * Input_data->Tj2;
Input_data->alimd = -Input_data->jmax * Input_data->Tj2;
} else {
// 能够达到amin
Input_data->Tj2 = Input_data->amax/Input_data->jmax;
Input_data->Td = Input_data->Tj2 + (vmax - Input_data->v1)/Input_data->amax;
Input_data->alimd = -Input_data->amax;
}
// 根据（3.25）计算匀速段时间
Input_data->Tv = (Input_data->q1 - Input_data->q0)/vmax - (Input_data->Ta/2)*(1 + Input_data->v0/vmax)
- (Input_data->Td/2)*(1 + Input_data->v1/vmax);
// 对Tv进行讨论
if (Input_data->Tv > 0)
// 能够达到给定的最大速度vmax， 即存在匀速阶段
Input_data->vlim = vmax;
else {
// 达不到最大速度，即匀速阶段Tv=0
// 假设最大加速度和最小加速度均能达到
Input_data->Tv = 0;
Tj = Input_data->amax / Input_data->jmax;
Input_data->Tj1 = Tj;
Input_data->Tj2 = Tj;
delta = (pow(Input_data->amax, 4)/pow(Input_data->jmax, 2)) + 2*(pow(Input_data->v0, 2) + pow(Input_data->v1, 2))
+ Input_data->amax*(4*(Input_data->q1 - Input_data->q0) - 2*(Input_data->amax/Input_data->jmax)*(Input_data->v0 + Input_data->v1));
Input_data->Ta = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v0 + sqrt(delta)) / (2*Input_data->amax);
Input_data->Td = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v1 + sqrt(delta)) / (2*Input_data->amax);
// 对Ta和Td进行讨论
if (Input_data->Ta < 0 || Input_data->Td < 0) {
if (Input_data->Ta < 0) {
// 没有加速段，只有减速段
Input_data->Ta = 0;
Input_data->Tj1 = 0;
Input_data->Td = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj2 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
+ pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = 0;
Input_data->alimd = -Input_data->jmax * Input_data->Tj2;
Input_data->vlim = v0;
} else if(Input_data->Td < 0) {
// 没有减速段，只有加速段
Input_data->Td = 0;
Input_data->Tj2 = 0;
Input_data->Ta = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj1 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
- pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = Input_data->jmax * Input_data->Tj1;
Input_data->alimd = 0;
Input_data->vlim = Input_data->jmax * Input_data->Tj1;
}
} else if(Input_data->Ta >= 2*Tj && Input_data->Td >= 2*Tj) {
// 加速段和减速段都能达到最大速度
Input_data->alima = Input_data->amax;
Input_data->alimd = -Input_data->amax;
Input_data->vlim = v0 + Input_data->alima*(Input_data->Ta - Tj);
} else {
// 加速段和减速阶段至少有一段不能达到最大加速度
float lambda = 0.99; // 系统取0<lambda<1
while (Input_data->Ta < 2*Tj || Input_data->Td < 2*Tj) {
Input_data->amax = lambda * Input_data->amax;
Input_data->Tv = 0;
Tj = Input_data->amax / Input_data->jmax;
Input_data->Tj1 = Tj;
Input_data->Tj2 = Tj;
delta = (pow(Input_data->amax, 4)/pow(Input_data->jmax, 2)) + 2*(pow(Input_data->v0, 2) + pow(Input_data->v1, 2))
+ Input_data->amax*(4*(Input_data->q1 - Input_data->q0) - 2*(Input_data->amax/Input_data->jmax)*(Input_data->v0 + Input_data->v1));
Input_data->Ta = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v0 + sqrt(delta)) / (2*Input_data->amax);
Input_data->Td = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v1 + sqrt(delta)) / (2*Input_data->amax);
if (Input_data->Ta < 0 || Input_data->Td < 0) {
if (Input_data->Ta < 0) {
// 没有加速段，只有减速段
Input_data->Ta = 0;
Input_data->Tj1 = 0;
Input_data->Td = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj2 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
+ pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = 0;
Input_data->alimd = -Input_data->jmax * Input_data->Tj2;
Input_data->vlim = v0;
} else if(Input_data->Td < 0) {
// 没有减速段，只有加速段
Input_data->Td = 0;
Input_data->Tj2 = 0;
Input_data->Ta = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj1 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
- pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = Input_data->jmax * Input_data->Tj1;
Input_data->alimd = 0;
Input_data->vlim = Input_data->jmax * Input_data->Tj1;
}
} else if(Input_data->Ta >= 2*Tj && Input_data->Td >= 2*Tj) {
// 加速段和减速段都能达到最大速度
Input_data->alima = Input_data->amax;
Input_data->alimd = -Input_data->amax;
Input_data->vlim = v0 + Input_data->alima*(Input_data->Ta - Tj);
}
}
}
}
return ( *Input_data );
}
// 计算位置
float S_position(float t, SCurve_Handle_t *Input_data)
{
float T = 0, q = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float q0 = Input_data->q0;
float q1 = Input_data->q1;
float v0 = Input_data->v0;
float v1 = Input_data->v1;
float vlim = Input_data->vlim;
float alima = Input_data->alima;
float alimd = Input_data->alimd;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
q = q0 + v0*t + jmax*pow(t, 3)/6;
else if (t >= Tj1 && t < Ta - Tj1)
q = q0 + v0*t +(alima/6)*(3*pow(t, 2) - 3*Tj1*t + pow(Tj1, 2));
else if (t >= Ta - Tj1 && t < Ta)
q = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*(pow(Ta - t, 3)/6);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
q = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(pow(t - T + Td, 3)/6);
else if (t >= T - Td + Tj2 && t < T - Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*pow(t - T + Td, 2) - 3*Tj2*(t - T + Td) + pow(Tj2, 2));
else if (t >= T - Tj2 && t <= T)
q = q1 - v1*(T - t) - jmax*(pow(T - t, 3)/6);
// else if(t <= 3*T)
// q = q1;
return q;
}
// 计算速度
float S_velocity(float t, SCurve_Handle_t *Input_data)
{
float T = 0, qd = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float v0 = Input_data->v0;
float v1 = Input_data->v1;
float vlim = Input_data->vlim;
float alima = Input_data->alima;
float alimd = Input_data->alimd;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qd = v0 + jmax*(pow(t, 2)/2);
else if (t >= Tj1 && t < Ta - Tj1)
qd = v0 + alima*(t - Tj1/2);
else if (t >= Ta - Tj1 && t < Ta)
qd = vlim + jmin*(pow(Ta - t, 2)/2);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qd = vlim;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qd = vlim - jmax*(pow(t - T + Td, 2)/2);
else if (t >= T - Td + Tj2 && t < T - Tj2)
qd = vlim + alimd*(t - T + Td - Tj2/2);
else if (t >= T - Tj2 && t <= T)
qd = v1 + jmax*(pow(t - T, 2)/2);
return qd;
}
// 计算加速度
float S_acceleration(float t, SCurve_Handle_t *Input_data)
{
float T = 0, qdd = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float alima = Input_data->alima;
float alimd = Input_data->alimd;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qdd = jmax * t;
else if (t >= Tj1 && t < Ta - Tj1)
qdd = alima;
else if (t >= Ta - Tj1 && t < Ta)
qdd = -jmin * (Ta - t);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qdd = 0;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qdd = -jmax * (t - T + Td);//此处修改
else if (t >= T - Td + Tj2 && t < T - Tj2)
qdd = alimd;
else if (t >= T - Tj2 && t <= T)
qdd = -jmax * (T - t);
return qdd;
}
// 计算加加速度
float S_jerk(float t, SCurve_Handle_t *Input_data)
{
float T = 0, qddd = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qddd = jmax;
else if (t >= Tj1 && t < Ta - Tj1)
qddd = 0;
else if (t >= Ta - Tj1 && t < Ta)
qddd = -jmax;//此处修改-jmax ----> -jmax
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qddd = 0;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qddd = -jmax;
else if (t >= T - Td + Tj2 && t < T - Tj2)
qddd = 0;
else if (t >= T - Tj2 && t <= T)
qddd = jmax;
return qddd;
}
2其次是.h文件
/* scurve.h */
#ifndef _S_PROFILE_H
#define _S_PROFILE_H
#include "math.h"
//#include "stdio.h"
//#include "stdint.h"
//#include "misc.h"
//#include "./usart/usart.h"
//#include "stdlib.h"
//#include "./key/key.h"
//#include "./scurve/scurve.h"
// https://blog.csdn.net/qq_26565435/article/details/94657852
typedef struct SCurve_Node_User_Par
{
float preq1; //上一次的终止位置
float deltaq; //q的变换量用于判断正反转
float refpos; //参考位置
float refvel; //参考速度
float refa; //参考加速度
float refaa; //参考加加速度
long maxvel; //最大速度
long maxa; //最大加速度
long maxaa; //最大加加速度
float total_T; //Ta + Td +Tv 总的规划时间
float step_T; //当前规划时间
float step; //规划次数
}S_User_ParHandle_t;
typedef struct SCurve_Node
{
float Ta; //加速阶段时间
float Tv; //匀速阶段时间
float Td; //减速阶段时间
float Tj1;//加速阶段用时
float Tj2;//减速阶段用时
float q0; //起始位置
float q1; //终止位置
float v0; //起始速度
float v1; //终止速度
float vlim; //最大速度限制
float amax; //最大加速度
float amin; //最大减速度
float alima; //最大加速度限制
float alimd; //最大减速度限制
float jmax; //最大加加速度
float jmin; //最大减减速度
S_User_ParHandle_t *User_Par; //用户自定义变量结构体指针
}SCurve_Handle_t;
extern S_User_ParHandle_t pSUserM1;
extern S_User_ParHandle_t pSUserM2;
int sign(float q0, float q1);
// S曲线参数计算
//void STrajectoryPara(SCurve_Handle_t Input_data, float q0, float q1, float v0, float v1, float vmax, float amax, float jmax);
SCurve_Handle_t STrajectoryPara( SCurve_Handle_t *Input_data,float q0, float q1, float v0, float v1, float vmax, float amax, float jmax);
// 计算位置
float S_position(float t, SCurve_Handle_t *Input_data);
// 计算速度
float S_velocity(float t, SCurve_Handle_t *Input_data);
// 计算加速度
float S_acceleration(float t, SCurve_Handle_t *Input_data);
// 计算加加速度
float S_jerk(float t, SCurve_Handle_t *Input_data);
#endif
首先了解.c文件和.h文件内容，查看文首致谢两篇文章。博主讲解的原理很清除。
3具体参数说明以及函数调用规则
曲线规划函数STrajectoryPara只需要在目标位置更改以后计算一次。
pInputdata->User_Par->total_T 总时间T
pHandle->pInputdata->User_Par->step_T 当前step下的时间
pInputdata->User_Par->step 当前step
pHandle->SamplingTime 执行周期
pHandle->PositionCtrlStatus 切换状态
oid TC_FollowCommand(PosCtrl_Handle_t *pHandle, float Angle)
{
float q0 = 0,q1 = 0,v1= 0;
q1 = Angle;//赋值终止位置
if(pHandle->pInputdata->User_Par->preq1 != q1)//如果两次位置不一致则需要进行一次计算规划
{
// RED_LED1_ON;
//START 0US
pHandle->pInputdata->User_Par->preq1 = q1;//本次的位置给上一次规划的位置
pHandle->PositionCtrlStatus = TC_READY_FOR_COMMAND;//位置环状态，准备好控制命令
q0 = (float)(SPD_GetMecAngle(STC_GetSpeedSensor(pHandle->pSTC))) / RADTOS16;//获取当前编码器位置，赋值给本次规划的初始位置
STrajectoryPara(pHandle->pInputdata,pHandle->pInputdata->User_Par->refpos,q1,v1,v1,(float)pHandle->pInputdata->User_Par->maxvel,(float)pHandle->pInputdata->User_Par->maxa,(float)pHandle->pInputdata->User_Par->maxaa);
pHandle->pInputdata->User_Par->total_T = pHandle->pInputdata->Ta+pHandle->pInputdata->Td +pHandle->pInputdata->Tv;//计算总时间
pHandle->pInputdata->User_Par->step_T = 0; //规划完成 不要进行规划 该三个参数清零是在目标位置到达，这这里清零是为了重新规划的时候重新开始
pHandle->pInputdata->User_Par->step = 0;//清零 用于下次规划计次数
// pHandle->PIDPosRegulator->hErr = 0; //清除位置环误差
// pHandle->PIDPosRegulator->hOutput = 0; //清除位置环输出值 8.8US
//END 8.8US
// RED_LED1_OFF;
}
if (pHandle->PositionCtrlStatus == TC_READY_FOR_COMMAND)//如果状态等于准备好命令
{
pHandle->PositionCtrlStatus = TC_FOLLOWING_ON_GOING;//切换状态3，每改变一次位置值规划一次，该部分只运行一次
//todo:此处与402状态机关联做处理
}
return;
}分别调用 S_position计算位置、S_velocity计算速度、S_acceleration计算加速度、S_jerk计算加加速度。
if(pHandle->pInputdata->User_Par->deltaq < 0)用于提供反转规划。
step下的时间大于总时间说明此次规划规划完成。
pHandle->PositionCtrlStatus 切换状态。
void TC_TrapezoidalVelocityPlanning(PosCtrl_Handle_t *pHandle) // float V0, float Vt, float Vav,float S,float A,float D)
{
if(pHandle->PositionCtrlStatus == TC_FOLLOWING_ON_GOING)//如果function.c中将状态更改为 following on going
{
// RED_LED1_ON;
//START 0US
pHandle->pInputdata->User_Par->refpos = S_position(pHandle->pInputdata->User_Par->step_T ,pHandle->pInputdata);//计算参考位置
pHandle->pInputdata->User_Par->refvel = S_velocity(pHandle->pInputdata->User_Par->step_T,pHandle->pInputdata);//计算参考速度
pHandle->pInputdata->User_Par->refa = S_acceleration(pHandle->pInputdata->User_Par->step_T,pHandle->pInputdata);
pHandle->pInputdata->User_Par->refaa = S_jerk(pHandle->pInputdata->User_Par->step_T,pHandle->pInputdata);
//START 2.56US
// RED_LED1_OFF;
if(pHandle->pInputdata->User_Par->deltaq < 0)
{
pHandle->pInputdata->User_Par->refpos = -pHandle->pInputdata->User_Par->refpos ;
pHandle->pInputdata->User_Par->refvel = -pHandle->pInputdata->User_Par->refvel;
pHandle->pInputdata->User_Par->refa = -pHandle->pInputdata->User_Par->refa;
pHandle->pInputdata->User_Par->refaa = -pHandle->pInputdata->User_Par->refaa;
}
pHandle->pInputdata->User_Par->step++;//计次数
pHandle->pInputdata->User_Par->step_T = pHandle->pInputdata->User_Par->step*pHandle->SamplingTime;//计时间 1ms 0.001
if(pHandle->pInputdata->User_Par->step_T > pHandle->pInputdata->User_Par->total_T)//如果时间大于总时间
{
pHandle->pInputdata->User_Par->step_T = 0; //规划完成 不要进行规划 该三个参数清零是在目标位置到达，这这里清零是为了重新规划的时候重新开始
pHandle->pInputdata->User_Par->step = 0;//清零 用于下次规划计次数
pHandle->PositionCtrlStatus = TC_TARGET_POSITION_REACHED;
}
}
else if(pHandle->PositionCtrlStatus == TC_TARGET_POSITION_REACHED)//位置到达
{
// pHandle->Omega = pHandle->pInputdata->User_Par->refvel;
// pHandle->Theta = pHandle->pInputdata->User_Par->refpos;
//todo:此处与402状态机关联做处理
}
}将计算的出来的 pInputdata->User_Par->refpos进行标准化处理。
与实际位置做差作为位置环输入参数，位置环计算得到速度参考为S型。
void TC_PositionRegulation(PosCtrl_Handle_t *pHandle)
{
// int32_t wMecAngleRef;
// int32_t wMecAngle;
// int32_t wError;
// int32_t hTorqueRef_Pos;
if (pHandle->PositionControlRegulation == ENABLE)
{
if ( pHandle->PositionCtrlStatus == TC_MOVEMENT_ON_GOING )
{
TC_MoveExecution(pHandle);
}
if ( pHandle->PositionCtrlStatus == TC_FOLLOWING_ON_GOING )//等待function.c中规划算法计算完成，将状态改为 following on going 状态
{
TC_FollowExecution(pHandle);
}
wMecAngleRef = (int32_t)(pHandle->pInputdata->User_Par->refpos * RADTOS16);//参考位置
wMecAngle = SPD_GetMecAngle(STC_GetSpeedSensor(pHandle->pSTC));//实际位置
wError = wMecAngleRef - wMecAngle;//参考位置和实际位置的偏差
hSpeedRef_Pos = PID_Controller(pHandle->PIDPosRegulator, wError);//位置误差给到位置环计算得到速度参考值
if(hSpeedRef_Pos != 0)
{
wErrormotor1 = hSpeedRef_Pos;
}
// STC_SetControlMode( pHandle->pSTC, STC_TORQUE_MODE );
// STC_ExecRamp( pHandle->pSTC, hSpeedRef_Pos, 0 );
STC_SetControlMode( pHandle->pSTC, STC_POSITION_MODE );//STC_SPEED_MODE STC_TORQUE_MODE
STC_ExecRamp( pHandle->pSTC, hSpeedRef_Pos, 0 );
}
}总体来说：位置环PI输入参数就一个那就是实际位置和目标位置的偏差，紧抓此信息就能将S型算法融入你自己的工程中。
4定义接口：
SCurve_Handle_t *pSCurve[2]; //定义两个SCurve_Handle_t类型的指针
S_User_ParHandle_t *pSUser[2]; //定义两个S_User_ParHandle_t类型的指针
S_User_ParHandle_t pSUserM1 =
{
.preq1 = 0,
.deltaq = 0,
.refpos = 0,
.refvel = 0,
.refa = 0,
.refaa = 0,
.maxvel = 20000,
.maxa = 10000,
.maxaa = 10000,
};
S_User_ParHandle_t pSUserM2 =
{
.preq1 = 0,
.deltaq = 0,
.refpos = 0,
.refvel = 0,
.refa = 0,
.refaa = 0,
.maxvel = 20000,
.maxa = 10000,
.maxaa = 10000,
};
SCurve_Handle_t pSCurveM1 =
{
.Ta = 0,
.Tv = 0,
.Td = 0,
.Tj1 = 0,
.Tj2 = 0,
.q0 = 0,
.q1 = 0,
.v0 = 0,
.v1 = 0,
.vlim = 0,
.amax = 0,
.amin = 0,
.alima = 0,
.alimd = 0,
.jmax = 0,
.jmin = 0,
.User_Par = &pSUserM1,
};
SCurve_Handle_t pSCurveM2 =
{
.Ta = 0,
.Tv = 0,
.Td = 0,
.Tj1 = 0,
.Tj2 = 0,
.q0 = 0,
.q1 = 0,
.v0 = 0,
.v1 = 0,
.vlim = 0,
.amax = 0,
.amin = 0,
.alima = 0,
.alimd = 0,
.jmax = 0,
.jmin = 0,
.User_Par = &pSUserM2,
};5初始化指针
pSCurve[M1] = &pSCurveM1;
pSUser[M1] = &pSUserM1; pSCurve[M2] = &pSCurveM2;
pSUser[M2] = &pSUserM2;将定义的指针指到使用的地方，容易管理参数。看个人需要。
PosCtrl_Handle_t pPosCtrlM1 =
{
.SamplingTime = 1.0f/MEDIUM_FREQUENCY_TASK_RATE,
.SysTickPeriod = 1.0f/SYS_TICK_FREQUENCY,
.AlignmentCfg = TC_ABSOLUTE_ALIGNMENT_NOT_SUPPORTED/*TC_ABSOLUTE_ALIGNMENT_SUPPORTED*/,
.pTrapezoidal = &pTrapezoidalM1,
.pSMC_Position_Struct = &pSMC_Position_StructM1,
.pInputdata = &pSCurveM1,
};
PosCtrl_Handle_t pPosCtrlM2 =
{
.SamplingTime = 1.0f/MEDIUM_FREQUENCY_TASK_RATE2,
.SysTickPeriod = 1.0f/SYS_TICK_FREQUENCY,
.AlignmentCfg = TC_ABSOLUTE_ALIGNMENT_NOT_SUPPORTED/*TC_ABSOLUTE_ALIGNMENT_SUPPORTED*/,
.pTrapezoidal = &pTrapezoidalM2,
.pSMC_Position_Struct = &pSMC_Position_StructM2,
.pInputdata = &pSCurveM2,
};6开放接口
index_pObjDictry ObjDict_Index6081[]=
{
{0x6081,0,RW,UNSIGNED8,SIZE_UINT8,PDO_BAN,(void*)&ParInNum2},
{0x6081,1,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pSUserM1.maxvel}, //M1位置环最大速度
{0x6081,2,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pSUserM2.maxvel}, //M2位置环最大速度
};
index_pObjDictry ObjDict_Index607A[]=
{
{0x607A,0,RO,INTEGER8,SIZE_INTEGER8,PDO_BAN,(void*)&ParInNum2},
{0x607A,1,RW,INTEGER32,SIZE_INTEGER32,PDO_ALLOW,(void*)&pCtrlPar[M1].SetPulseMotor}, //M1目标位置
{0x607A,2,RW,INTEGER32,SIZE_INTEGER32,PDO_ALLOW,(void*)&pCtrlPar[M2].SetPulseMotor}, //M2目标位置
};
unsigned int obj6083_Profile_Accel = 0;//轮廓加速度 该值在402协议当中定义位加速度，在中菱和和利时的驱动器当中该值为时间
index_pObjDictry ObjDict_Index6083[]=
{
{0x6083,0,RW,UNSIGNED8,SIZE_UINT8,PDO_BAN,(void*)&ParInNum4},
{0x6083,1,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pCtrlPar[M1].hDurationms}, //M1速度环加速度时间 默认值为200
{0x6083,2,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pCtrlPar[M2].hDurationms}, //M2速度环加速度时间 默认值为200
{0x6083,3,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pSUserM1.maxa}, //M1位置环加速度
{0x6083,4,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pSUserM2.maxa}, //M2位置环加速度
// {0x6083,5,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&0}, //M1位置环加速度时间 默认值为0 在位置环不起作用
// {0x6083,6,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&0}, //M2位置环加速度时间
};
/**/
unsigned int obj6084_Profile_Decel = 0;//轮廓减速度
index_pObjDictry ObjDict_Index6084[]=
{
//轮廓(Profile)减速度(参见CANopen402 P83)
{0x6084,0,RW,UNSIGNED8,SIZE_UINT8,PDO_BAN,(void*)&ParInNum4},
{0x6084,1,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pCtrlPar[M1].hDurationms}, //M1速度环减速度时间 默认值为200
{0x6084,2,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pCtrlPar[M2].hDurationms}, //M2速度环减速度时间 默认值为200，为了兼容中菱驱动器
{0x6084,3,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pSUserM1.maxa}, //M1位置环减速度
{0x6084,4,RW,UNSIGNED32,SIZE_UINT32,PDO_ALLOW,(void*)&pSUserM2.maxa}, //M2位置环减速度
};7最终完成曲线图：
红色代表参考位置（labview最大显示范围-32768-32767，导致图像变成这个样子）
蓝色代表实际位置
黄色代表实际速度
可见实际速度没有跳变，实际电机运行比较平滑。
8大概的软件执行框架我画了一下！
9个人在开发过程中的问题：
1.本来考虑读取当前位置作为曲线规划位置参数的输入，但是当前位置和参考位置存在误差会导致曲线规划速度参考不连续
如图手绘图，发现手绘图说明问题不太清晰，该代码比较麻烦，后来还是将代码改回错误的情况打印数据进行说明。
局部放大
局部放大
红色为参考位置曲线
绿色为实际位置曲线
蓝色为加速度曲线
黄色为实际速度曲线
结论：参考位置存在跳变，实际速度不平滑存在抖动，加速去曲线不连续。
2.算法中的一些小错误
图中黄色代表加加速度，在从左至右第一个方框中标注出来的黄色部分为加加速度出错位置，第二个方框中为加速度出错位置，第三个方框中为加加速度出错位置，第四个方框中为加速度出错位置。
3.仔细考虑该函数STrajectoryPara入口参数
STrajectoryPara(pHandle->pInputdata,pHandle->pInputdata->User_Par->refpos,q1,v1,v1,(float)pHandle->pInputdata->User_Par->maxvel,(float)pHandle->pInputdata->User_Par->maxa,(float)pHandle->pInputdata->User_Par->maxaa);与之后接口中的单位有关系，比如你加速度的单位是什么？r/s2 p/s2 (r-转，p-编码器脉冲)查看了汇川的IS620P伺服驱动手册、明志以及中菱。
汇川
鸣志
中菱
参考402协议，其中描述加速度的单位由用户自定义给出，就导致了我参考的汇川、中菱、明志三个厂家的加速度单位不统一。
最终将加速度单位处理为与编码器挂钩为：counts/s2 也就是p/s2。
267
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
