S_S曲线单片机_s曲线_

/*----------------------------------------------------------------------------*/ //版权信息： 无极电子工作室 QQ:1921826084 技术支持群:299042155 //文件名： main.c //当前版本： 1.0 //单片机型号： STM32 //开发环境： Keil //晶振频率： 8M //作者： paul //功能： //修订记录: //2015-09-28----创建 //本算法供学习使用，切勿用于商业，本工作室保留所有权利 /*----------------------------------------------------------------------------*/ #include "s_curve.h" //s曲线加速度各段参数定义 //起始速度 const float f0 = 0; //加加速度与减减速度， float faa = 0; float frr = 0; //加速段三个时间 const float taa = 0.2; const float tua = 0.1; const float tra = 0.2; //匀速段 const float tuu = 0.1; //减速段 const float tar = 0.2; const float tur = 0.1; const float trr = 0.2; unsigned int s_curve_table[TABLE_LEN]={0}; //根据参数以及当前时间点来确定加速度与频率 static int S_curve_func( S_curve_params_struct *s_params, float t, float *freq, float *acc ); void S_curve_gen(void) { unsigned int i,tint; float ti; float freq,freq_pre; float acc,acc_pre; float fi; S_curve_params_struct s_params; //给参数结构体赋值 s_params.f0 = f0; s_params.taa = taa; s_params.tua = tua; s_params.tra = tra; s_params.tuu = tuu; s_params.tar = tar; s_params.tur = tur; s_params.trr = trr; //根据约束条件求出加加速段与减减速段斜率 s_params.faa = 2.0/(s_params.taa*(s_params.taa+s_params.tra+2*s_params.tua)); s_params.frr = 2.0/(s_params.trr*(s_params.tar+s_params.trr+2*s_params.tur)); for(i = 0;i < TABLE_LEN;i++) { //求出每个时间点对应的频率以及加速度 fi = i*(taa+tua+tra+tuu+tar+tur+trr)/TABLE_LEN; S_curve_func(&s_params,fi,&freq,&acc); //根据最大与最小装载值确定定时器实际值 ti = TMAX - (TMAX - TMIN)*freq; //转换为整数值 tint = (unsigned int)ti; //存入s曲线表 s_curve_table[i] = tint; } } //根据7段加速度曲线计算对应的频率与加速度值 static int S_curve_func( S_curve_params_struct *s_params, float t, float *freq, float *acc ) { //辅助常数项 float A,B,C,D,E,F; //表达式中间值 float f1,f2,f3,f4,f5; //加速段，匀速段，减速段时间 float Ta,Tu,Tr; //减加速与加减速段斜率 float fra,far; //起始频率与加加速频率，减减速频率 float f0,faa,frr; float taa,tua,tra,tuu,tar,tur,trr; faa = s_params->faa; frr = s_params->frr; taa = s_params->taa; tua = s_params->tua; tra = s_params->tra; tuu = s_params->tuu; tar = s_params->tar; tur = s_params->tur; trr = s_params->trr; f0 = s_params->f0; fra = faa*taa/tra; far = frr*trr/tar; Ta = taa + tua + tra; Tu = tuu; Tr = tar + tur + trr; A = f0; B = f0 - 0.5*faa*taa*taa; C = f0 + 0.5*faa*taa*taa + faa*taa*tua + 0.5*fra*(taa+tua)*(taa+tua) - fra*Ta*(taa+tua); f1 = f0 + 0.5*faa*taa*taa; f2 = f0 + 0.5*faa*taa*taa + faa*taa*tua; f3 = 0.5*fra*Ta*Ta + C; D = f3 - 0.5*far*(Ta+Tu)*(Ta+Tu); f4 = -far*0.5*(Ta+Tu+tar)*(Ta+Tu+tar) + far*(Ta+Tu)*(Ta+Tu+tar) + D; E = f4 + far*tar*(Ta+Tu+tar); f5 = -far*tar*(Ta+Tu+Tr-trr) + E; F = f5 + frr*(Ta+Tu+Tr)*(Ta+Tu+Tr-trr) - 0.5*frr*(Ta+Tu+Tr-trr)*(Ta+Tu+Tr-trr); //如果时间点在全行程规定的时间段内 if((t>=0) && (t<=Ta+Tu+Tr)) { //加加速段 if((t>=0) && (t<=taa)) { *freq = 0.5*faa*t*t + A; *acc = faa*t; } //匀加速段 else if((t>=taa) && (t<=taa+tua)) { *freq = faa*taa*t + B; *acc = faa*taa; } //加减速段 else if((t>=taa+tua) && (t<=taa+tua+tra)) { *freq = -0.5*fra*t*t + fra*Ta*t + C; *acc = -fra*t + fra*Ta; } //匀速段 else if((t>=Ta) && (t<=Ta+tuu)) { *freq = f3; *acc = 0; } //加减速段 else if((t>=Ta+Tu) && (t<=Ta+Tu+tar)) { *freq = -0.5*far*t*t + far*(Ta+Tu)*t + D; *acc = -far*t + far*(Ta+Tu); } //匀减速 else if((t>=Ta+Tu+tar) && (t<=Ta+Tu+tar+tur)) { *freq = -far*tar*t + E; *acc = -far*tar; } //减减速 else if((t>=Ta+Tu+Tr-trr) && (t<=Ta+Tu+Tr)) { *freq = 0.5*frr*t*t - frr*(Ta+Tu+Tr)*t + F; *acc = frr*t - frr*(Ta+Tu+Tr); } } else { return -1; } return 0; }