////////////////////////////////////////////////////////////////////////////////////////////////////
for(i=0;i<5;i++) { ad_valu[0][i]=ADC_Read(9); ad_valu[1][i]=ADC_Read(10); ad_valu[2][i]=ADC_Read(8); ad_valu[3][i]=ADC_Read(11); } for(i=0;i<4;i++) { for(j=0;j<4;j++) { for(k=0;k<4-j;k++) { if(ad_valu[i][k] > ad_valu[i][k+1]) { temp = ad_valu[i][k+1]; ad_valu[i][k+1] = ad_valu[i][k]; ad_valu[i][k] = temp; } } } } for(i=0;i<4;i++) { ad_sum[i] = ad_valu[i][1] + ad_valu[i][2] + ad_valu[i][3]; ad_valu1[i] = ad_sum[i] / 3; } for(i=0;i<4;i++) { ad_valu1_temp[i][4] = ad_valu1_temp[i][3]; ad_valu1_temp[i][3] = ad_valu1_temp[i][2]; ad_valu1_temp[i][2] = ad_valu1_temp[i][1]; ad_valu1_temp[i][1] = ad_valu1_temp[i][0]; ad_valu1_temp[i][0] = ad_valu1[i]; } for(i=0;i<4;i++) {// g_ValueOfAD[i] = (int16)(ad_valu1_temp[i][0]);// g_ValueOfAD[i] = (int16)(ad_valu1_temp[i][0]/10);// g_ValueOfAD[i] = g_ValueOfAD[i]*10; g_ValueOfAD[i] = ad_valu1[i]; } AD_0=(float)1000000*(g_ValueOfAD[0]-96)/(1370-96); AD_1=(float)1000000*(g_ValueOfAD[1]-93)/(1470-93); AD_2=(float)1000000*(g_ValueOfAD[2]-140)/(1340-114); AD_3=(float)1000000*(g_ValueOfAD[3]-91)/(1340-91); AD_to10=AD_0/10000; AD_to11=AD_1/10000; AD_to12=AD_2/10000; AD_to13=AD_3/10000; //////////////////////////////////////////////////////////////////////////////////////////////////////// AD0=ADC_Read(9);// AD1=ADC_Read(10);// AD2=ADC_Read(8);// AD3=ADC_Read(11);// // AD_to10=(float)100*(AD0-91)/(1320-91);// AD_to11=(float)100*(AD1-87)/(1220-87);// AD_to12=(float)100*(AD2-90)/(1100-90);// AD_to13=(float)100*(AD3-90)/(850-90);
if(AD_to10>99) AD_to10=99; if(AD_to10<1) AD_to10=1; if(AD_to11>99) AD_to11=99; if(AD_to11<1) AD_to11=1; if(AD_to12>99) AD_to12=99; if(AD_to12<1) AD_to12=1; if(AD_to13>99) AD_to13=99; if(AD_to13<1) AD_to13=1; AD_sum12 = (AD_to10 + AD_to11);AD_sum34 = (AD_to12 + AD_to13);AD_cha34 = myabs(AD_to12 - AD_to13);AD_cha12 = myabs(AD_to10 - AD_to11);
// g_fDirectionError = (float)(AD_to10 - AD_to11)/(AD_to10 + AD_to11+1);
JueDui = JueDuiZhi(AD_to12 - AD_to13); Weight_Shu = JueDui*0.06; Weight_Ping = (20 - Weight_Shu)/2; DianCi = (float)14.4*(Weight_Ping*(sqrt(AD_to10) - sqrt(AD_to11))+Weight_Shu*(sqrt(AD_to12) - sqrt(AD_to13)))/(Weight_Ping*(AD_to10 + AD_to11)+Weight_Shu*(AD_to12 + AD_to13)+1); //g_fDirectionError = DianCi; g_fDirectionError = 0.3*DianCi + 0.7*DianCiLast; DianCiLast = g_fDirectionError;// g_fDirectionError = (g_fDirectionError>=1?1:g_fDirectionError);// g_fDirectionError = (g_fDirectionError<=-1?-1:g_fDirectionError); g_fDirectionErrorTemp[4] = g_fDirectionErrorTemp[3]; g_fDirectionErrorTemp[3] = g_fDirectionErrorTemp[2]; g_fDirectionErrorTemp[2] = g_fDirectionErrorTemp[1]; g_fDirectionErrorTemp[1] = g_fDirectionErrorTemp[0]; g_fDirectionErrorTemp[0] = g_fDirectionError;
g_fDirectionError_dot = 65*(g_fDirectionErrorTemp[0]-g_fDirectionErrorTemp[3]); g_fDirectionError_dot = (g_fDirectionError_dot>150?150:g_fDirectionError_dot); g_fDirectionError_dot = (g_fDirectionError_dot<-150?-150:g_fDirectionError_dot);
// if(AD_to10+AD_to11>30)
//////////////////////////////////////////////////////////////////////////////////////////////////////// if(g_fDirectionError*100>140||g_fDirectionError*100<-140)// {ppp=6;ddd=36;ppp_s=2;} if(g_fDirectionError*100>120||g_fDirectionError*100<-120) {ppp=4.8;ddd=72;ppp_s=1.8;} else if(g_fDirectionError*100>100||g_fDirectionError*100<-100) {ppp=4.7;ddd=72;ppp_s=1.8;} else if(g_fDirectionError*100>80||g_fDirectionError*100<-80) {ppp=4.6;ddd=71;ppp_s=1.8;} else if(g_fDirectionError*100>60||g_fDirectionError*100<-60) {ppp=4.5;ddd=70;ppp_s=1.7;} else if(g_fDirectionError*100>40||g_fDirectionError*100<-40) {ppp=4.5;ddd=70;ppp_s=1.6;} else if(g_fDirectionError*100>20||g_fDirectionError*100<-20) {ppp=4.4;ddd=69;ppp_s=1.6;} else {ppp=4.4;ddd=69;ppp_s=1.3;}///////////////////////////////////////////////////////////////////////////////////////////////////////
if(AD_sum12>75){ g_fSpeedFilter = g_fDirectionError*(ppp-0.2) + g_fDirectionError_dot* (65)/100; g_fSpeedFilterLast = g_fSpeedFilter; loss = 0; }//ppp_s=1.55,ddd=35//else loss = 1;
if(loss == 1){ if(g_fSpeedFilterLast>0) { g_fSpeedFilter = 5.7; } else { g_fSpeedFilter = -5.7; }} YangOld = Yang; YangNew = g_fSpeedFilter; if(YangNew>=YangOld) Yang = ((YangNew-YangOld)>0.21?(YangOld+0.21):YangNew); else Yang = ((YangNew-YangOld)<-0.21?(YangOld-0.21):YangNew);
暂无评论