【无标题】速度环中限制加速度与加加速度的积分处理

//第一个控制周期

if (First_W == false)
        {
            targetspeed_last[5] = targetspeed[5];//
            target_a_last[5] = target_a_now[5];//
            First_W = true;
        }

    // before  process

    //计算当前加速度与加加速度
        target_a_now[5]=(targetspeed[5] - targetspeed_last[5]) / dt;
         target_jerk_now[5] = (target_a_now[5] - target_a_last[5]) / dt;

        double temp_Ro_a_Jerk_max = 60.0;//
        if (target_jerk_now[5] > temp_Ro_a_Jerk_max)
            target_jerk_now[5] = temp_Ro_a_Jerk_max;
        if (target_jerk_now[5] < -temp_Ro_a_Jerk_max)
            target_jerk_now[5] = -temp_Ro_a_Jerk_max;
        target_a_now[5] = target_a_last[5] + target_jerk_now[5] * dt;

        double temp_W_acc_max = 1.5; // 

        if (target_a_now[5] > temp_W_acc_max)
            target_a_now[5] = temp_W_acc_max;
        if (target_a_now[5] < -temp_W_acc_max)
            target_a_now[5] = -temp_W_acc_max;
        targetspeed[5] = targetspeed_last[5] + target_a_now[5] * dt;

      

 // after  process

        targetspeed_last[5] = targetspeed[5];
        target_a_last[5] = target_a_now[5];