s

#include "stdio.h"typedef struct S_CTRL{
#define SSPD_BUF_LEN 100struct{float aMax;float aMin;float vMax;float J;/* 加加速度 */int t[7];int T[7];int tMax;}in;struct{float accBuf[SSPD_BUF_LEN];float decBuf[SSPD_BUF_LEN];long  S[7];long  V[7];}out;
}S_CTRL;S_CTRL sCR = {{.aMax = 1400000.0, /* R/Min^2 */.aMin = -1400000.0,/* R/Min^2 */.vMax = 3000.0,    /*   RPM   */.J = 30.0,        /* R/Min^3 */{0},{40,20,40,0,40,20,40},.tMax = 200}};#define _T(sCTRL,i) ((sCTRL).in.T[(i)-1])
#define s (sCR)
int main(){float v = 0;float pos = 0;float tao = 0;int t1 = 40;int t2 = 60;int t3 = 100;sCR.in.t[0] = sCR.in.T[0];sCR.in.t[1] = sCR.in.T[1]+sCR.in.t[0];sCR.in.t[2] = sCR.in.T[2]+sCR.in.t[1];sCR.in.t[3] = sCR.in.T[3]+sCR.in.t[2];sCR.in.t[4] = sCR.in.T[4]+sCR.in.t[3];sCR.in.t[5] = sCR.in.T[5]+sCR.in.t[4];sCR.in.t[6] = sCR.in.T[6]+sCR.in.t[5];//    sCR.in.T[0] = sCR.in.t[0]-0;
//    sCR.in.T[1] = sCR.in.t[1]-sCR.in.t[0];
//    sCR.in.T[2] = sCR.in.t[2]-sCR.in.t[1];
//    sCR.in.T[3] = sCR.in.t[3]-sCR.in.t[2];
//    sCR.in.T[4] = sCR.in.t[4]-sCR.in.t[3];
//    sCR.in.T[5] = sCR.in.t[5]-sCR.in.t[4];
//    sCR.in.T[6] = sCR.in.t[6]-sCR.in.t[5];for(int i=0;i<7;i++){printf("t[%d] = %d\n",i,sCR.in.t[i]);}float J_tmp = 0;J_tmp = -(sCR.in.J);int flag = 0;for(int time = 0;time<sCR.in.tMax;time++){if(time<sCR.in.t[0]){/* T1 */tao = time;v = (sCR.in.J)*(tao)*(tao)/2;pos = (sCR.in.J)*tao*tao*tao/6;flag = 1;}else if(time<sCR.in.t[1]){/* T2 第二段速度文档中公式是错的 */if(time == sCR.in.t[0]){sCR.out.S[0] = (sCR.in.J)*tao*tao*tao/6;sCR.out.V[0] = (sCR.in.J)*(tao)*(tao)/2;}tao = time-t1;v = (sCR.in.J)*_T(s,1)*(_T(s,1)/2+tao);pos = sCR.out.S[0] + (sCR.out.V[0])*tao + (sCR.in.J)*_T(s,1)*tao*tao/2;flag = 2;}else if(time<sCR.in.t[2]){/* T3 */if(time == sCR.in.t[1]){sCR.out.S[1] = sCR.out.S[0] + (sCR.out.V[0])*tao + (sCR.in.J)*_T(s,1)*tao*tao/2;sCR.out.V[1] = (sCR.in.J)*_T(s,1)*(_T(s,1)/2+tao);}tao = time-t2;v = (sCR.in.J)*(_T(s,1)*_T(s,2)+_T(s,1)*_T(s,1)/2+_T(s,1)*tao-tao*tao/2.0);pos = sCR.out.S[1]+sCR.out.V[1]*tao+(sCR.in.J)*_T(s,1)*tao*tao/2-(sCR.in.J)*tao*tao*tao/6;flag = 3;}else if(time<sCR.in.t[3]){/* T4 *///nothing..if(time == sCR.in.t[2]){sCR.out.S[2] =  sCR.out.S[1]+sCR.out.V[1]*tao+(sCR.in.J)*_T(s,1)*tao*tao/2-(sCR.in.J)*tao*tao*tao/6;//上个位置公式的末尾速度sCR.out.V[2] = (sCR.in.J)*(_T(s,1)*_T(s,2)+_T(s,1)*_T(s,1)/2+_T(s,1)*tao-tao*tao/2.0);//上个速度公式的末尾速度}v = (sCR.in.J)*_T(s,1)*(_T(s,1)+_T(s,2));pos = sCR.out.S[2] + sCR.out.V[2]*tao;}else if(time<sCR.in.t[4]){/* T5 */if(time == sCR.in.t[3]){/* T4 没用到，所以这里用T3的 */sCR.out.S[3] = sCR.out.S[1]+sCR.out.V[1]*tao+(sCR.in.J)*_T(s,1)*tao*tao/2-(sCR.in.J)*tao*tao*tao/6;sCR.out.V[3] = (sCR.in.J)*(_T(s,1)*_T(s,2)+_T(s,1)*_T(s,1)/2+_T(s,1)*tao-tao*tao/2.0);}tao = time-sCR.in.t[3];v = (sCR.in.J)*(_T(s,1)*_T(s,1)+_T(s,1)*_T(s,2)-tao*tao/2);pos = sCR.out.S[3]+sCR.out.V[3]*tao-(sCR.in.J)*tao*tao*tao/6;}else if(time<sCR.in.t[5]){/* T6 */if(time == sCR.in.t[4]){sCR.out.S[4] = sCR.out.S[3]+sCR.out.V[3]*tao-(sCR.in.J)*tao*tao*tao/6;sCR.out.V[4] = (sCR.in.J)*(_T(s,1)*_T(s,1)+_T(s,1)*_T(s,2)-tao*tao/2);}tao = time-sCR.in.t[4];v = (sCR.in.J)*_T(s,1)*(_T(s,2)+_T(s,1)/2-tao);pos = sCR.out.S[4] + sCR.out.V[4]*tao-(sCR.in.J)*_T(s,1)*tao*tao/2;}else if(time<sCR.in.t[6]){/* T7 最后一段速度文档中公式是错的 */if(time == sCR.in.t[5]){sCR.out.S[5] = sCR.out.S[4] + sCR.out.V[4]*tao-(sCR.in.J)*_T(s,1)*tao*tao/2;sCR.out.V[5] = (sCR.in.J)*_T(s,1)*(_T(s,2)+_T(s,1)/2-tao);}tao = time-sCR.in.t[5];
//            v = (sCR.in.J)*(T1*T2+T1*T1/2-T1*tao+tao*tao/2);v = (sCR.in.J)*(_T(s,1)*_T(s,2)+_T(s,1)*_T(s,1)/2-(_T(s,1)*_T(s,6))-_T(s,1)*tao+tao*tao/2);pos = sCR.out.S[5]+sCR.out.V[5]*tao-(sCR.in.J)*_T(s,1)*tao*tao/2+(sCR.in.J)*tao*tao*tao/6;}//        sCR.out.accBuf[time] = v;
//        printf("v[%d]%d=%f\n",time,flag,v);
//        printf("%d->%f\n",time,v);
//        printf("%f\n",v);printf("%f\n",pos);}return 0;
}