標題: stm32f103ze電磁循跡超聲波避障停車源程序 [打印本頁]
作者: 狄利克雷    時間: 2021-5-21 16:48
標題: stm32f103ze電磁循跡超聲波避障停車源程序
采用三個電感循跡和超聲波避障
制作出來的實物圖如下：


單片機源程序如下:
  1. #include "led.h"
  2. #include "delay.h"
  3. #include "key.h"
  4. #include "sys.h"
  5. #include "usart.h"
  6. #include "timer.h"
  7. #include "moto.h"
  8. #include "pwm.h"
  9. #include "lcd.h"

  11. #define Trig PGout(1)                //PC1        TRIG
  12. //函數(shù)聲明
  13. void UltrasonicWave_Configuration(void);
  14. void UltrasonicWave_StartMeasure(void);
  15. void TIM5_Int_Init(u16 arr,u16 psc);
  16. extern int Sensor_Left,Sensor_Middle,Sensor_Right;//電磁巡線相關(guān)
  17. extern float Velocity_KP,Velocity_KI,Sensor,sum;               //速度控制PID參數(shù)
  18. extern float Velocity,Velocity_Set,Turn,Angle_Set;
  19. extern int Motor_A,Motor_B,Servo,Target_A,Target_B;  //電機控制相關(guān)
  20. extern int Encoder_Left,Encoder_Right;             //左右編碼器的脈沖計數(shù)

  22. extern u8  TIM5CH1_CAPTURE_STA;                //輸入捕獲狀態(tài)                                                   
  23. extern u16        TIM5CH1_CAPTURE_VAL;        //輸入捕獲值        

  25. //void TIM3_Int_Init(u16 arr,u16 psc);

  27. void UltrasonicWave_Configuration(void)
  28. {
  29.         GPIO_InitTypeDef GPIO_InitStructure;
  30.         
  31.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOG, ENABLE);

  33.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;                                         
  34.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;                  
  35.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                 
  36.         GPIO_Init(GPIOG, &GPIO_InitStructure);                       
  37. }
  38. void UltrasonicWave_StartMeasure(void)
  39. {
  40.         Trig = 1;
  41.         delay_us(20);                                      //??20US
  42.         Trig = 0;
  43.         
  44. }
  45. int main(void)
  46. {               
  47.   u32 temp=0;
  48.         float UltrasonicWave_Distance=0;      //計算出的距離
  49.         delay_init();                     //延時函數(shù)初始化         
  50.         Encoder_Init_TIM2();            //=====編碼器接口
  51.         delay_ms(100);
  52.         Encoder_Init_TIM3();            //=====初始化編碼器
  53.   delay_ms(100);         
  54.   Motor_PWM_Init(7199,0);                                  //=====初始化PWM 10KHZ，用于驅(qū)動電機
  55.         delay_ms(100);
  56.          NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應(yīng)優(yōu)先級
  57.         UltrasonicWave_Configuration();
  58.         TIM5_Cap_Init(0XFFFF,72-1);        //以1Mhz的頻率計數(shù)
  59.         ele_Init();  //=====電磁傳感器初始化
  60.         delay_ms(100);
  61.         
  62.          LED_Init();                             //LED端口初始化
  63.         LCD_Init();
  64.                  
  65.         POINT_COLOR=RED;
  66.         delay_ms(100);
  67.         //TIM5_Int_Init(5000,71);//10Khz的計數(shù)頻率，計數(shù)到5000為500ms
  68. delay_ms(100);         
  69.            while(1)
  70.         {
  71.                 UltrasonicWave_StartMeasure();//開始測距
  72.                 if(TIM5CH1_CAPTURE_STA&0X80)//成功捕獲到了一次上升沿
  73.                 {
  74.                         temp=TIM5CH1_CAPTURE_STA&0X3F;
  75.                         temp*=65536;//溢出時間總和
  76.                         temp+=TIM5CH1_CAPTURE_VAL;//得到總的高電平時間
  77.                         
  78.                         UltrasonicWave_Distance=(float)temp*170/10000;//距離計算公式：距離=高電平持續(xù)時間*聲速/2   
  79.                                                                                                                   //即 距離= 高電平持續(xù)時間*340*100/（2*1000 000） = 高電平持續(xù)時間*170/10000
  80.                         //printf("Distance:%f cm\r\n",UltrasonicWave_Distance);//打印總的高點平時間
  81.                         TIM5CH1_CAPTURE_STA=0;//開啟下一次捕獲
  82.                 }
  83.                 LCD_ShowString(10,40,200,16,16,"TFTLCD TEST");
  84.                 LCD_ShowxNum(10,60,Sensor_Left,4,16,0);
  85.                 LCD_ShowxNum(10,80,Sensor_Middle,4,16,0);
  86.                 LCD_ShowxNum(10,100,Sensor_Right,4,16,0);
  87.                 LCD_ShowxNum(10,120,Turn,4,16,0);
  88.                 LCD_ShowxNum(10,140,Sensor,4,16,0);
  89.                 LCD_ShowxNum(10,160,UltrasonicWave_Distance,4,16,0);
  90.     Sensor_Left=Get_Adc(11);                //采集左邊電感的數(shù)據(jù)
  91.                 Sensor_Right=Get_Adc(13);               //采集右邊電感的數(shù)據(jù)
  92.                 Sensor_Middle=Get_Adc(12);              //采集中間電感的數(shù)據(jù)
  93.                 sum=Sensor_Left-Sensor_Right;  //歸一化處理
  94.                 Sensor=150*sum/(Sensor_Left+Sensor_Middle+Sensor_Right+1);   //求偏差
  95.                 Sensor=Sensor>120?120:Sensor;
  96.                 Sensor=Sensor<-120?-120:Sensor;
  97.                 Get_RC();//pid計算輸出
  98.                 if(UltrasonicWave_Distance>15)
  99.                 {
  100.                                 PWMA1=4000+Turn,PWMA2=0;        
  101.                                 PWMB1=0,PWMB2=3200+Turn;
  102.                 }
  103.                 else
  104.                 {
  105.                     PWMA1=7200+Turn,PWMA2=0;        
  106.                                 PWMB1=0,PWMB2=0+Turn;
  107.                 }
  108.                 //LED0=!LED0;
  109.                 //delay_ms(200);//測試主程序是否卡死
  110.         
  111.         }         
  112. }         
  113. u8  TIM5CH1_CAPTURE_STA=0;        //輸入捕獲狀態(tài)                                                   
  114. u16        TIM5CH1_CAPTURE_VAL;        //輸入捕獲值

  116. //定時器5中斷服務(wù)程序         
  117. void TIM5_IRQHandler(void)
  118. {

  120.          if((TIM5CH1_CAPTURE_STA&0X80)==0)//還未成功捕獲        
  121.         {         
  122.                 if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET)//檢查TIM5更新中斷發(fā)生與否
  123.                  
  124.                 {            
  125.                         if(TIM5CH1_CAPTURE_STA&0X40)//已經(jīng)捕獲到高電平了
  126.                         {
  127.                                 if((TIM5CH1_CAPTURE_STA&0X3F)==0X3F)//高電平太長了
  128.                                 {
  129.                                         TIM5CH1_CAPTURE_STA|=0X80;//標記成功捕獲了一次
  130.                                         TIM5CH1_CAPTURE_VAL=0XFFFF;
  131.                                 }
  132.                                 else TIM5CH1_CAPTURE_STA++;
  133.                         }         
  134.                 }
  135.                 if (TIM_GetITStatus(TIM5, TIM_IT_CC1) != RESET)//捕獲1發(fā)生捕獲事件
  136.                 {        
  137.                         if(TIM5CH1_CAPTURE_STA&0X40)                //捕獲到一個下降沿                 
  138.                         {                                 
  139.                                 TIM5CH1_CAPTURE_STA|=0X80;                //標記成功捕獲到一次高電平脈寬
  140.                                 TIM5CH1_CAPTURE_VAL=TIM_GetCapture1(TIM5);
  141.                                    TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Rising); //CC1P=0 設(shè)置為上升沿捕獲
  142.                         }
  143.                         else                                                                  //還未開始,第一次捕獲上升沿
  144.                         {
  145.                                 TIM5CH1_CAPTURE_STA=0;                        //清空
  146.                                 TIM5CH1_CAPTURE_VAL=0;
  147.                                  TIM_SetCounter(TIM5,0);
  148.                                 TIM5CH1_CAPTURE_STA|=0X40;                //標記捕獲到了上升沿
  149.                                    TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Falling);                //CC1P=1 設(shè)置為下降沿捕獲
  150.                         }                    
  151.                 }                                                                                    
  152.          }

  154.     TIM_ClearITPendingBit(TIM5, TIM_IT_CC1|TIM_IT_Update); //清除中斷標志位

  156. }

  158. //定時器3中斷服務(wù)程序
  159. //void TIM5_IRQHandler(void)   //TIM3中斷
  160. //{
  161. //        if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET)  //檢查TIM3更新中斷發(fā)生與否
  162. //                {
  163. //                        
  164. //                        TIM_ClearITPendingBit(TIM5, TIM_IT_Update  );  //清除TIMx更新中斷標志
  165. ////  if(Flag_Target==1)  '
  166. //                          
  167. //                                                                        
  168. //                        //LED1=!LED1;//測試中斷
  169. //                        //         if(Flag_Target==0)
  170. ////         {
  171. //                                        //        Encoder_Left=Read_Encoder(2);                                       //===讀取編碼器的值                                                         //為了保證M法測速的時間基準，首先讀取編碼器數(shù)據(jù)
  172. //                                        //        Encoder_Right=-Read_Encoder(3);                                      //===讀取編碼器的值
  173. //                                                Get_RC();
  174. //                                                //Kinematic_Analysis(Velocity,Turn);                                                                                                                             //小車運動學(xué)分析   
  175. ////Motor_A=Incremental_PI_A(Encoder_Left,Target_A);                   //===速度閉環(huán)控制計算電機A最終PWM
  176. //                                //        Motor_B=Incremental_PI_B(Encoder_Right,Target_B);                  //===速度閉環(huán)控制計算電機B最終PWM
  177. //                                //                Xianfu_Pwm();                                                      //===PWM限幅
  178. //                                  //Set_Pwm(Motor_A,Motor_B,Servo);                                          //===賦值給PWM寄存器                  
  179. ////         }
  180. //                }
  181. //}
復(fù)制代碼

所有資料51hei提供下載:
xunji5.16.7z (234.08 KB, 下載次數(shù): 37)





歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1