瑞萨版双环PID

/******************************************************************************* ***********函数类型：双环PID适用编程环境：CS+参数个数：共15个分别为三种姿态每种姿态各有5个参数：外环（角度环）的P、I、D 及内环（角速度环）的P、D函数说明：1、这个双环PID是根据crazepony的串行PID的伪代码改的用于瑞萨的程序本双环PID 外环是角度环有P、I、D三个参数外环PID输出公式为：外环PIDout = P * 角度差 + I * 角度差积分 + D * 角速度（其中角度差=期望角度-实时角度；另外由于角度差的微分为角速度所以直接用6050读出的角速度参与计算）内环是角速度环有P、D两个参数内环PD输出公式为：内环PDout = P * 角速度差 + D * 角加速度（其中角速度差=外环PID输出-实时角速度；另外由于角速度差的微分为角加速度所以直接用6050读出的角加速度参与计算）2、期望角度（Q_Roll,Q_Pitch,Q_Yaw）由主程序mpu6050初始化后得出3、瑞萨编译器CS+中汉语字符乱码所以所有注释均由拼音给出******************************************************************************** *************//**** bian liang & chang liang ding yi *********//*******chang liang ding yi *********/double Q_Roll,Q_Pitch,Q_Yaw; //chu shi jiao du (qi wang jiao)/*******bian liang ding yi ********/double RA_Roll,RA_Pitch,RA_Yaw; //shi shi jiao du//double RA_Roll_old,RA_Pitch_old,RA_Yaw_old; //shang yi ci jiao du zhi double RS_Roll,RS_Pitch,RS_Yaw; //shi shi jiao su du//double RS_Roll_old,RS_Pitch_old,RS_Yaw_old; //shang yi ci jiao su du zhi double RJ_Roll,RJ_Pitch,RJ_Yaw; //shi shi jiao jia su du/*****WAI HUAN****///wai huan bian liangdouble W_Roll_p,W_Pitch_p,W_Yaw_p; //wai huan bi li bian liangdouble W_Roll_i,W_Pitch_i,W_Yaw_i; //wai huan ji fen bian liangdouble W_Roll_d,W_Pitch_d,W_Yaw_d; //wai huan wei fen bian liang//wai huan PID can shudouble P_W_Roll,P_W_Pitch,P_W_Yaw; //wai huan P can shudouble I_W_Roll,I_W_Pitch,I_W_Yaw; //wai huan I can shudouble D_W_Roll,D_W_Pitch,D_W_Yaw; //wai huan D can shu//wai huan PID shu chudouble W_PIDout_Roll,W_PIDout_Pitch,W_PIDout_Yaw; //wai huan PID shu chu /****** JIAO SU DU CHA ****/double S_C_Roll,S_C_Pitch,S_C_Yaw; //jiao su du cha = wai huan PID - dang qian jiao su du/*****NEI HUAN****///nei huan bian liangdouble N_Roll_p,N_Pitch_p,N_Yaw_p; //nei huan bi li bian liangdouble N_Roll_d,N_Pitch_d,N_Yaw_d; //nei huan wei fen bian liang//nei huan PID can shudouble P_N_Roll,P_N_Pitch,P_N_Yaw; //nei huan P can shudouble D_N_Roll,D_N_Pitch,D_N_Yaw; //wai huan D can shu//nei huan PD shu chudouble N_PDout_Roll,N_PDout_Pitch,N_PDout_Yaw; //nei huan PD shu chu/*********** han shu sheng ming **************/double ji_fen_i(double,double);void double_PID_control(){/****PID can shu tiao zheng*****///wai huanP_W_Roll=0;I_W_Roll=0;D_W_Roll=0;P_W_Pitch=0;I_W_Pitch=0;D_W_Pitch=0;P_W_Yaw=0; //wai huan P can shuI_W_Yaw=0; //wai huan I can shuD_W_Yaw=0; //wai huan D can shu//nei huanP_N_Roll=0;D_N_Roll=0;P_N_Pitch=0;D_N_Pitch=0;P_N_Yaw=0; //nei huan P can shuD_N_Yaw=0; //wai huan D can shu/*********wai huan ge fen shi ji shuan*******/// bi li_pW_Roll_p=Q_Roll-RA_Roll;W_Pitch_p=Q_Pitch-RA_Pitch;W_Yaw_p=Q_Yaw-RA_Yaw;//ji fen_iW_Roll_i=ji_fen_i(Q_Roll,RA_Roll);W_Pitch_i=ji_fen_i(Q_Pitch,RA_Pitch);W_Yaw_i=ji_fen_i(Q_Yaw,RA_Yaw);//wei fen_d//W_Roll_d=wei_fen_d(Q_Roll,RA_Roll,RA_Roll_old);//W_Pitch_d=wei_fen_dQ_Pitch,RA_Pitch,RA_Pitch_old);//W_Yaw_d=wei_fen_d(Q_Yaw,RA_Yaw,RA_Yaw_old);/*********wai huan PID shu chu***********/W_PIDout_Roll=(P_W_Roll*W_Roll_p+I_W_Roll*W_Roll_i+D_W_Roll*RS_Roll); //wai huan PID= P * JIAO DU CHA + I * JIAO DU CHA JI FEN + D * DANG QIAN JIAO SU DUW_PIDout_Pitch=(P_W_Pitch*W_Pitch_p+I_W_Pitch*W_Pitch_i+D_W_Pitch*RS_Pitch);W_PIDout_Yaw=(P_W_Yaw*W_Yaw_p+I_W_Yaw*W_Yaw_i+D_W_Yaw*RS_Yaw);/************ JIAO SU DU CHA **********************/S_C_Roll=(W_PIDout_Roll-RS_Roll);S_C_Pitch=(W_PIDout_Pitch-RS_Pitch);S_C_Yaw=(W_PIDout_Yaw-RS_Yaw);/********** nei huan ge fen shi ji suan********///bi li_pN_Roll_p=S_C_Roll; //nei huan bi li xiang = jiao sudu chaN_Pitch_p=S_C_Pitch;N_Yaw_p=S_C_Yaw;//wei fen_dN_Roll_d=RJ_Roll; //nei huan wei fen xiang = jiaojia su duN_Pitch_d=RJ_Pitch;N_Yaw_d=RJ_Yaw;/******************* nei huan PID shu chu************/N_PDout_Roll=(P_N_Roll*N_Roll_p+D_N_Roll*N_Roll_d);N_PDout_Pitch=(P_N_Pitch*N_Pitch_p+D_N_Pitch*N_Pitch_d);N_PDout_Yaw=(P_N_Yaw*N_Yaw_p+D_N_Yaw*N_Yaw_d);/******* END ***************/}/***********ji fen yun suan han shu*******/double ji_fen_i(double q,double r){double s=0;int i=0;for(i=0;i<100000;i++){s+=(q-r*r/100000);r+=((q-r)/100000);}return s;}。