|
|
采用三個電感循跡和超聲波避障
制作出來的實物圖如下：
單片機(jī)源程序如下:
- #include "led.h"
- #include "delay.h"
- #include "key.h"
- #include "sys.h"
- #include "usart.h"
- #include "timer.h"
- #include "moto.h"
- #include "pwm.h"
- #include "lcd.h"
- #define Trig PGout(1) //PC1 TRIG
- //函數(shù)聲明
- void UltrasonicWave_Configuration(void);
- void UltrasonicWave_StartMeasure(void);
- void TIM5_Int_Init(u16 arr,u16 psc);
- extern int Sensor_Left,Sensor_Middle,Sensor_Right;//電磁巡線相關(guān)
- extern float Velocity_KP,Velocity_KI,Sensor,sum; //速度控制PID參數(shù)
- extern float Velocity,Velocity_Set,Turn,Angle_Set;
- extern int Motor_A,Motor_B,Servo,Target_A,Target_B; //電機(jī)控制相關(guān)
- extern int Encoder_Left,Encoder_Right; //左右編碼器的脈沖計數(shù)
- extern u8 TIM5CH1_CAPTURE_STA; //輸入捕獲狀態(tài)
- extern u16 TIM5CH1_CAPTURE_VAL; //輸入捕獲值
- //void TIM3_Int_Init(u16 arr,u16 psc);
- void UltrasonicWave_Configuration(void)
- {
- GPIO_InitTypeDef GPIO_InitStructure;
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOG, ENABLE);
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOG, &GPIO_InitStructure);
- }
- void UltrasonicWave_StartMeasure(void)
- {
- Trig = 1;
- delay_us(20); //??20US
- Trig = 0;
-
- }
- int main(void)
- {
- u32 temp=0;
- float UltrasonicWave_Distance=0; //計算出的距離
- delay_init(); //延時函數(shù)初始化
- Encoder_Init_TIM2(); //=====編碼器接口
- delay_ms(100);
- Encoder_Init_TIM3(); //=====初始化編碼器
- delay_ms(100);
- Motor_PWM_Init(7199,0); //=====初始化PWM 10KHZ，用于驅(qū)動電機(jī)
- delay_ms(100);
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應(yīng)優(yōu)先級
- UltrasonicWave_Configuration();
- TIM5_Cap_Init(0XFFFF,72-1); //以1Mhz的頻率計數(shù)
- ele_Init(); //=====電磁傳感器初始化
- delay_ms(100);
-
- LED_Init(); //LED端口初始化
- LCD_Init();
-
- POINT_COLOR=RED;
- delay_ms(100);
- //TIM5_Int_Init(5000,71);//10Khz的計數(shù)頻率，計數(shù)到5000為500ms
- delay_ms(100);
- while(1)
- {
- UltrasonicWave_StartMeasure();//開始測距
- if(TIM5CH1_CAPTURE_STA&0X80)//成功捕獲到了一次上升沿
- {
- temp=TIM5CH1_CAPTURE_STA&0X3F;
- temp*=65536;//溢出時間總和
- temp+=TIM5CH1_CAPTURE_VAL;//得到總的高電平時間
-
- UltrasonicWave_Distance=(float)temp*170/10000;//距離計算公式：距離=高電平持續(xù)時間*聲速/2
- //即 距離= 高電平持續(xù)時間*340*100/（2*1000 000） = 高電平持續(xù)時間*170/10000
- //printf("Distance:%f cm\r\n",UltrasonicWave_Distance);//打印總的高點平時間
- TIM5CH1_CAPTURE_STA=0;//開啟下一次捕獲
- }
- LCD_ShowString(10,40,200,16,16,"TFTLCD TEST");
- LCD_ShowxNum(10,60,Sensor_Left,4,16,0);
- LCD_ShowxNum(10,80,Sensor_Middle,4,16,0);
- LCD_ShowxNum(10,100,Sensor_Right,4,16,0);
- LCD_ShowxNum(10,120,Turn,4,16,0);
- LCD_ShowxNum(10,140,Sensor,4,16,0);
- LCD_ShowxNum(10,160,UltrasonicWave_Distance,4,16,0);
- Sensor_Left=Get_Adc(11); //采集左邊電感的數(shù)據(jù)
- Sensor_Right=Get_Adc(13); //采集右邊電感的數(shù)據(jù)
- Sensor_Middle=Get_Adc(12); //采集中間電感的數(shù)據(jù)
- sum=Sensor_Left-Sensor_Right; //歸一化處理
- Sensor=150*sum/(Sensor_Left+Sensor_Middle+Sensor_Right+1); //求偏差
- Sensor=Sensor>120?120:Sensor;
- Sensor=Sensor<-120?-120:Sensor;
- Get_RC();//pid計算輸出
- if(UltrasonicWave_Distance>15)
- {
- PWMA1=4000+Turn,PWMA2=0;
- PWMB1=0,PWMB2=3200+Turn;
- }
- else
- {
- PWMA1=7200+Turn,PWMA2=0;
- PWMB1=0,PWMB2=0+Turn;
- }
- //LED0=!LED0;
- //delay_ms(200);//測試主程序是否卡死
-
- }
- }
- u8 TIM5CH1_CAPTURE_STA=0; //輸入捕獲狀態(tài)
- u16 TIM5CH1_CAPTURE_VAL; //輸入捕獲值
-
- //定時器5中斷服務(wù)程序
- void TIM5_IRQHandler(void)
- {
- if((TIM5CH1_CAPTURE_STA&0X80)==0)//還未成功捕獲
- {
- if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET)//檢查TIM5更新中斷發(fā)生與否
-
- {
- if(TIM5CH1_CAPTURE_STA&0X40)//已經(jīng)捕獲到高電平了
- {
- if((TIM5CH1_CAPTURE_STA&0X3F)==0X3F)//高電平太長了
- {
- TIM5CH1_CAPTURE_STA|=0X80;//標(biāo)記成功捕獲了一次
- TIM5CH1_CAPTURE_VAL=0XFFFF;
- }
- else TIM5CH1_CAPTURE_STA++;
- }
- }
- if (TIM_GetITStatus(TIM5, TIM_IT_CC1) != RESET)//捕獲1發(fā)生捕獲事件
- {
- if(TIM5CH1_CAPTURE_STA&0X40) //捕獲到一個下降沿
- {
- TIM5CH1_CAPTURE_STA|=0X80; //標(biāo)記成功捕獲到一次高電平脈寬
- TIM5CH1_CAPTURE_VAL=TIM_GetCapture1(TIM5);
- TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Rising); //CC1P=0 設(shè)置為上升沿捕獲
- }
- else //還未開始,第一次捕獲上升沿
- {
- TIM5CH1_CAPTURE_STA=0; //清空
- TIM5CH1_CAPTURE_VAL=0;
- TIM_SetCounter(TIM5,0);
- TIM5CH1_CAPTURE_STA|=0X40; //標(biāo)記捕獲到了上升沿
- TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Falling); //CC1P=1 設(shè)置為下降沿捕獲
- }
- }
- }
-
- TIM_ClearITPendingBit(TIM5, TIM_IT_CC1|TIM_IT_Update); //清除中斷標(biāo)志位
-
- }
- //定時器3中斷服務(wù)程序
- //void TIM5_IRQHandler(void) //TIM3中斷
- //{
- // if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) //檢查TIM3更新中斷發(fā)生與否
- // {
- //
- // TIM_ClearITPendingBit(TIM5, TIM_IT_Update ); //清除TIMx更新中斷標(biāo)志
- //// if(Flag_Target==1) '
- //
- //
- // //LED1=!LED1;//測試中斷
- // // if(Flag_Target==0)
- //// {
- // // Encoder_Left=Read_Encoder(2); //===讀取編碼器的值 //為了保證M法測速的時間基準(zhǔn)，首先讀取編碼器數(shù)據(jù)
- // // Encoder_Right=-Read_Encoder(3); //===讀取編碼器的值
- // Get_RC();
- // //Kinematic_Analysis(Velocity,Turn); //小車運動學(xué)分析
- ////Motor_A=Incremental_PI_A(Encoder_Left,Target_A); //===速度閉環(huán)控制計算電機(jī)A最終PWM
- // // Motor_B=Incremental_PI_B(Encoder_Right,Target_B); //===速度閉環(huán)控制計算電機(jī)B最終PWM
- // // Xianfu_Pwm(); //===PWM限幅
- // //Set_Pwm(Motor_A,Motor_B,Servo); //===賦值給PWM寄存器
- //// }
- // }
- //}
所有資料51hei提供下載:
xunji5.16.7z
(234.08 KB, 下載次數(shù): 37)
2021-5-21 17:40 上傳
點擊文件名下載附件
|
評分
-
查看全部評分
|
