关于4轴X型串级PID内环整定问题

U16 ym;

double PID_x=0,PID_y=0,PID_z=0;  //PID最终输出量

//X轴串级PID



float out_pid_x=0;                                                                   //X输出量

float out_x_kp=0,out_x_ki=0,Error_x_out;           //39 0.05                 X外环PI  积分

float in_x_kp=1.2,in_x_ki=0.1,in_x_kd=0,Error_x_in;                          //        X内环PID        积分 0.1 0.04 11

                                

//Y轴串级PID



float out_pid_y=0;                                                                //内环Y输出量

float out_y_kp=39,out_y_ki=0.05,Error_y_out=0;                //Y外环PI

float in_y_kp=0.3,in_y_ki=0.05,in_y_kd=11;                //Y内环PID



//Z轴自转PD



float z_kp=5.0,z_kd=4.0;



char x_set,y_set,z_set,xe,ye,ze;

double x_jiaodu,x_jiaosudu,y_jiaodu,y_jiaosudu,z_jiaodu,last_x_jiaosudu,last_y_jiaosudu;

int speed1,speed2,speed3,speed4;

void PID_Update() interrupt 1 using 1

{        

                last_x_jiaosudu=x_jiaosudu;

                last_y_jiaosudu=y_jiaosudu;

                x_jiaodu        =(float)stcAngle.Angle[0]/32768*180;

                x_jiaosudu        =(float)stcGyro.w[0]/32768*2000;

                y_jiaodu        =(float)stcAngle.Angle[1]/32768*180;

                y_jiaosudu        =(float)stcGyro.w[1]/32768*2000;

                z_jiaodu        =(float)stcAngle.Angle[2]/32768*180;

        xe=x_set-x_jiaodu;

        if(ym>40)

        {

                Error_x_out+=xe;                        //油门大于40时x外环积分

        }

        else

        {

                Error_x_out=0;                                //小于40积分清零，不积分。

        }

        if(Error_x_out<(-420))

        {

                Error_x_out=(-420);

        }

        if(Error_x_out>420)

        {

                Error_x_out=420;        

        }                                                        //积分限制

        out_pid_x=out_x_kp*xe+Error_x_out*out_x_ki;                        //计算x外环输出量

        if(ym>40)

        {

                Error_x_in+=out_pid_x-x_jiaosudu;                                   //油门大于100时内环x积分

        }

        else

        {

                Error_x_in=0;                                                                          //小于100时清除积分，不积分

        }

        if(Error_x_in<(-420))

        {

                Error_x_in=(-420);

        }

        if(Error_x_in>420)

        {

                Error_x_in=420;

        }                                                                                   //积分限制

        PID_x=(out_pid_x-x_jiaosudu)*in_x_kp+(Error_x_in*in_x_ki)+(x_jiaosudu-last_x_jiaosudu)*in_x_kd;

        if(PID_x>840)        

        {

                PID_x=840;

        }

        if(PID_x<-840)

        {

                PID_x=-840;

        }

        PWM1=360+ym+PID_x;

        PWM2=360+ym-PID_x;

        PWM3=360+ym+PID_x;

        PWM4=360+ym-PID_x;

        if(PWM1<360)  PWM1=360;

        if(PWM1>1200) PWM1=1200;

        if(PWM2<360)  PWM2=360;

        if(PWM2>1200) PWM2=1200;

        if(PWM3<360)  PWM3=360;

        if(PWM3>1200) PWM3=1200;

        if(PWM4<360)  PWM4=360;

        if(PWM4>1200) PWM4=1200;

        pwm(PWM1,PWM2,PWM3,PWM4);

}