S型曲线规划

s

#include "stdio.h"


typedef struct S_CTRL{
#define SSPD_BUF_LEN 100
    struct{
        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;
}