【正文】 *******/ acc[0]=ADValue1。 //vol[5][0]代表了加速度計的采集的AD值 acc[0]=acc[0]/*3300*2。 //把測回來的電壓值變?yōu)閙v為單位的 acc[0]=(acc[0]1720)/800。 //1440mv代表的是0度對應的電壓值，而800mV為1個g if(acc[0]1) acc[0]=1。 else if(acc[0]1) acc[0]=1。 acc[0]=180/*asin(acc[0])+。//把acc測到的角度值計算出來，后面加的數(shù)為誤差 /****************以下為陀螺儀的角速度換算****************/ //w[0]=(vol[7][0]10282)/。 w[0]=(ADValue11450)/*3300*2/。 //w[0]=(ADValue18747)*。 w[0]=w[0]/。 if(w[0]1amp。amp。w[0]1) w[0]=0。 if(w[0]300) w[0]=300。 if(w[0]300) w[0]=300。 Kalman_Filter(acc[0],w[0])。 //用于計算角度 //*******************零點矯正 if(lingdian_count0) { lingdian_count。 } if(lingdian_count=200amp。amp。lingdian_count0)//10個周期后，開始累加 { lingdian_sum=angle[0]+lingdian_sum。 beepon()。//累加的時候，蜂鳴器響 testzhouqi++。 } if(lingdian_count==0)//200個周期后算出矯正的數(shù)值 { lingdian_jiaozheng=。 } /* if(fabs(d_y[0]piancha_zhongzhi)8amp。amp。!swit[15]) start_count=11。*/ /************************速度采集**************************/ speed_R=DMA_count_get(DMA_CH_0)。//右輪速度 DMA_count_reset(DMA_CH_0)。//重置 speed_L=DMA_count_get(DMA_CH_1)。//左輪速度 DMA_count_reset(DMA_CH_1)。//重置 L=Lspeed_L。 R=Rspeed_R。 //側(cè)翻超速保護 if(swit[13]==0) { if((speed_L90||speed_R90)amp。amp。fabs(d_y[0]piancha_zhongzhi)4) { sudu_overbeep=100。 start_count=0。 } } if(sudu_overbeep0) sudu_overbeep。 if((vol[3][0]+vol[2][0])5000) { goout=goout+speed_R+speed_L。 while(goout1000) { PWMstart=0。 forward_flag=0。 //startbeepflag=1。 //beepon()。 vcon_2motor(0,0)。 } } else goout=0。 // Lcountspeed+=sudu_L[0]。//記錄左輪的路程 // Rcountspeed+=sudu[0]。 //記錄右輪的路程 Lcountspeed+=speed_L。 Rcountspeed+=speed_R。 /********************轉(zhuǎn)彎PWM計算公式*************************/ piancha_zhongzhi=。 // turn_pwm=180*(d_y[0]piancha_zhongzhi)+*(vol[9][0]14440)。 //er=er+*(d_y[0]piancha_zhongzhier)。 turn_pwm=204*(d_y[0]piancha_zhongzhi)+*(vol[9][0]14500)。 /* if(vol[2][0]=4500 amp。amp。 (vol[2][0]+vol[3][0])20000 amp。amp。 vol[2][0]vol[3][0]) { turn_pwm=2300。 turn_beep=1。 } else if(vol[3][0]=4500 amp。amp。 (vol[2][0]+vol[3][0])20000 amp。amp。 vol[3][0]vol[2][0]) { turn_pwm=2300。 turn_beep=1。 } else { turn_beep=0。 turn_pwm=204*(d_y[0]piancha_zhongzhi)+*(vol[9][0]14500)。 }*/ /********************靜止直立的PWM計算公式********************/ //直道的靜止PWM if((fabs(dy_ave)1 || wan_cd_time0)amp。amp。 start_count=43) { if(swit[14]==0) wan=100。 else if(swit[15]==0) wan=100。 else wan=0。 utpwm=80*(angle[0]+lingdian_jiaozheng)+5*angle_dot[0]+wan。 //utpwm=380*angle[0]+5*angle_dot[0]+*abs(vol[9][0]16400)。 //bflag=1。 } else { utpwm=80*(angle[0]+lingdian_jiaozheng)+5*angle_dot[0]。 //bflag=0。 } //開車標志為1則小車開始行走 if(PWMstart==1) { if(forward_flag==0)//前進標志為0,小車不動 turn_pwm=0。 if(swit[12]==0) { if(turn_pwm0) vcon_2motor(utpwm+RValueturn_pwm,utpwm+RValue+turn_pwm)。 else vcon_2motor(utpwm+RValueturn_pwm,utpwm+RValue+turn_pwm)。 } else vcon_2motor(utpwm+RValueturn_pwm,utpwm+RValue+turn_pwm)。 //vcon_2motor(utpwm+RValue+5*((speed_L+speed_R)(sudu_rec)/10)turn_pwm,utpwm+RValue+5*((speed_L+speed_R)(sudu_rec)/10)+turn_pwm)。 deltapwm=newpwmoldpwm。 RValue=(int)((speed_period+1)/car_motion_period * deltapwm+oldpwm)。//速度控制 speed_period++。 if(speed_period= car_motion_period) { speed_period=0。//周期大于car_motion_period時,清零 if(forward_flag==1){//接收到前進信號 //sudu_rec=600。 if( ((q_zheng=6) amp。amp。 (h_fu=6)) || ((q_fu=6) amp。amp。 (h_zheng=6)) ) { pre=now。 now=(L+R)/2。 if((nowpre)1584 amp。amp。 (nowpre)10320) 。//tubian=4。 } if(tubian0) tubian。 if(fabs(dy_ave)1) { wan_cd_time=5。 //tubian=2。 //sudu_rec=1200。 } if(wan_cd_time0) wan_cd_time。 if(wan_cd_time==2) 。//tubian=2。 /* if(wan_cd_time==1 amp。amp。 start_count=23) { //sudu_rec=1100。 sudu_rec=(int)((Rcountspeed+Lcountspeed)/2)。 }*/ //vcon_R((int)(vstart))。 vstart=(sudu_rec)。 vcon_start((int)(vstart))。 } else if(forward_flag==0||stop_flag==1){//沒有接收到前進信號或者接收到靜止信號 vcon_R(0)。 } LValue=(Rcountspeed+Lcountspeed)/2。 Rcountspeed=0。 Lcountspeed=0。 } } //開車標志為0則小車停止 else //vcon_2motor(0,0)。 { //電機1 ftm0_pwm_change_duty(4,0)。//控制占空比,占空比為0 0~10000 ftm0_pwm_change_duty(6,0)。//控制占空比,占空比為0 0~10000 //電機2 ftm0_pwm_change_duty(5,0)。//控制占空比,占空比為0 0~10000 ftm0_pwm_change_duty(7,0)。//控制占空比,占空比為0 0~10000 } /**********************發(fā)送數(shù)據(jù)到上位機*********************/ //if (send_flag==1) if((!swit[15])amp。amp。send_flag==1) { i++。 if(i==3) 。//send_angle_func((double)(angle[0]+lingdian_jiaozheng))。//發(fā)送角度 //send_angle_func((double))。//發(fā)送角度 else if(i==6) 。//send_angle_dot_func((double)lingdian_sum)。//發(fā)送角速度 //send_angle_dot_func((double))。//發(fā)送角速度 else if(i==9) 。//send_Lsudu((int)testzhouqi)。//發(fā)送左輪速度// else if(i==12) 。//send_Rsudu((int)speed_R)。//發(fā)送右輪速度 else if(i==15) 。//send_acc((double)(max+min))。//發(fā)送加速度計數(shù)據(jù) //send_acc((double))。