S曲线C语言实现，利用robomodule+STM32F429+直流伺服电机进行简单验证

void CAN1_Configuration(void) { 
    CAN_InitTypeDef can; CAN_FilterInitTypeDef can_filter; GPIO_InitTypeDef gpio; NVIC_InitTypeDef nvic; //使能相关时钟 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);//使能gpio时钟  RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);//使能CAN1时钟 //引脚复用映射配置 Connect CAN pins to AF9 GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_CAN1); GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_CAN1); //初始化GPIO gpio.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_8; gpio.GPIO_Mode = GPIO_Mode_AF;//复用功能 GPIO_Init(GPIOB, &gpio);//初始化PB8, PB9 nvic.NVIC_IRQChannel = CAN1_RX0_IRQn; nvic.NVIC_IRQChannelPreemptionPriority = 2;//主优先级为2 nvic.NVIC_IRQChannelSubPriority = 1;// 次优先级为1 nvic.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&nvic); nvic.NVIC_IRQChannel = CAN1_TX_IRQn; nvic.NVIC_IRQChannelPreemptionPriority = 1; nvic.NVIC_IRQChannelSubPriority = 1; nvic.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&nvic); //CAN寄存器初始化 CAN_DeInit(CAN1); CAN_StructInit(&can); //CAN单元设置 can.CAN_TTCM = DISABLE;//非时间触发通信模式  can.CAN_ABOM = DISABLE;//软件自动离线管理 can.CAN_AWUM = DISABLE;//睡眠模式通过软件唤醒(清除CAN->MCR的SLEEP位) can.CAN_NART = DISABLE;//报文自动传送 %%%%%%%%%% can.CAN_RFLM = DISABLE;//报文不锁定,新的覆盖旧的  can.CAN_TXFP = ENABLE;//%%%%%%%%%%% can.CAN_Mode = CAN_Mode_Normal;//常规模式 can.CAN_SJW = CAN_SJW_1tq;//再同步跳转宽度 can.CAN_BS1 = CAN_BS1_5tq;//时间段1占用5个时间单元数 can.CAN_BS2 = CAN_BS2_3tq;//时间段2占用3个时间单元数 can.CAN_Prescaler = 5; //CAN BaudRate 45/(1+5+3)/5=1Mbps 分频系数 CAN_Init(CAN1, &can);// 初始化CAN1 //配置过滤器，可配置过滤器来接收固定的几个id can_filter.CAN_FilterNumber = 0; //过滤器0 can_filter.CAN_FilterMode = CAN_FilterMode_IdMask; can_filter.CAN_FilterScale = CAN_FilterScale_32bit;//32位 can_filter.CAN_FilterIdHigh = 0x0000;32位ID can_filter.CAN_FilterIdLow = 0x0000; can_filter.CAN_FilterMaskIdHigh = 0x0000;//32位MASK can_filter.CAN_FilterMaskIdLow = 0x0000; can_filter.CAN_FilterFIFOAssignment = 0;//过滤器0关联到FIFO0 can_filter.CAN_FilterActivation=ENABLE;//激活过滤器0 CAN_FilterInit(&can_filter);//滤波器初始化 CAN_ITConfig(CAN1,CAN_IT_FMP0,ENABLE); CAN_ITConfig(CAN1,CAN_IT_TME,ENABLE); } 

接下来是S曲线的C语言代码，准备知识及相应的matlab代码见机器人学回炉重造（6）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划。

/* scurve.h */ #ifndef _SCURVE_H_ #define _SCURVE_H_ typedef struct SCurve_Node *PtrToSCurve; 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; }; typedef PtrToSCurve SCurve_para; int sign(float q0, float q1); // S曲线参数计算 void STrajectoryPara(SCurve_para Input_data, float q0, float q1, float v0, float v1, float vmax, float amax, float jmax); // 计算位置 float S_position(float t, SCurve_para Input_data); // 计算速度 float S_velocity(float t, SCurve_para Input_data); // 计算加速度 float S_acceleration(float t, SCurve_para Input_data); // 计算加加速度 float S_jerk(float t, SCurve_para Input_data); #endif 

/* scurve.c */ #include "Headfile.h" int sign(float q0, float q1) { 
    int a = 0; if (q1 - q0 > 0) a = 1; else a = -1; return a; } // S曲线参数计算 void STrajectoryPara(SCurve_para 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->q0 = sigma * q0; Input_data->q1 = sigma * q1; Input_data->v0 = sigma * v0; 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 
    
    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 
    ) 
    ; 
    } 
    } 
    } 
    } 
    } 
    // 计算位置 
    float 
    S_position 
    ( 
    float t 
    , SCurve_para 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 
    ) 
    ; 
    return q 
    ; 
    } 
    // 计算速度 
    float 
    S_velocity 
    ( 
    float t 
    , SCurve_para 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_para 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_para 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 
    = 
    -jmin 
    ; 
    // 匀速段 
    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 
    ; 
    } 
   

/* Headfile.h */ #ifndef _Headfile_H #define _Headfile_H #include "stm32f4xx.h" #include "./can/can1.h" #include "./systick/systick.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" #endif