標題: stm32循跡避障小車代碼 [打印本頁]
作者: 甘洋洋    時間: 2018-12-20 08:11
標題: stm32循跡避障小車代碼
  1. #include "PID.h"
  2. #include "stm32f10x.h"
  3. #include "motor.h"
  4. #include "delay.h"
  5. u16 ReadData = 0xffff;
  6. int BasicSpeed = 0;
  7. int LeftSpeed = 0;
  8. int RightSpeed = 0;
  9. int basespeed = 530;
  10. unsigned char bizhangflag1 = 0;
  11. unsigned char bizhangflag2 = 0;
  12. unsigned char bizhangflag3 = 0;
  13. unsigned char catchflag = 0;
  14. //紅外對管對應(yīng)于PF1~PF10
  15. void TIM4_IRQHandler(void)
  16. {
  17.         static int LastError = 0;
  18.         static int CurrentError = 0;
  19.         static unsigned int PIDCounter = 0;
  20.         unsigned char PIDflag = 0;
  21.         static int DiffError = 0;
  22. //        static int delay_time = 0;
  23. //        static int IntError = 0;
  24.         int Output = 0;
  25.         unsigned char cornerflag = 0;
  26.         if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET)
  27.         {
  28.         //        TIM_Cmd(TIM4, DISABLE);
  29.                 ReadData = GPIO_ReadInputData(GPIOF);       
  30.        
  31.                 /***************************************特殊情況******************************************************/

  33.                 /***************************************直角情況*******************************************/                                                               
  34.                 if(ReadData == 0xff81||ReadData==0xff03||ReadData==0xff01||ReadData==0xffc1||ReadData==0xff83||ReadData==0xff07||
  35.                    ReadData == 0xffe1||ReadData==0xffc3||ReadData==0xff87)
  36.                 {
  37.                         delay_ms(15);
  38.                         if(GPIO_ReadInputData(GPIOF)==0xffff)
  39.                         {                                                                                                                                                                                               
  40.                                  motor(0,0);
  41.                     delay_ms(10);
  42.                                 motor(-500,-500);
  43.                                 delay_ms(250);
  44.                                 motor(-500,500);
  45.                                 while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)
  46.                                          &&(GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f));
  47.                                 cornerflag = 1;
  48.                         }
  49.                 }
  50.                 if(ReadData ==0xf80f||ReadData ==0xf81f||ReadData==0xfc0f||ReadData==0xfc1f||ReadData==0xf83f||ReadData==0xfe0f||
  51.                    ReadData ==0xf87f||ReadData ==0xfc3f||ReadData==0xff0f)
  52.                 {
  53.                         delay_ms(10);
  54.                         if(GPIO_ReadInputData(GPIOF)==0xffff)
  55.                         {
  56.                                    motor(0,0);
  57.                                 delay_ms(10);
  58.                                 motor(-500,-500);
  59.                                 delay_ms(250);
  60.                                 motor(500,-500);
  61.                                 while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)&&
  62.                                             (GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f));
  63.                                 cornerflag = 1;
  64.                         }
  65.                 }       
  66.                 /***********************************虛線情況******************************/
  67.                 if(ReadData == 0xffff)
  68.                 {
  69.                         motor(540,550);
  70.                         delay_us(30);
  71.                         cornerflag = 1;
  72.                 }         
  73.                 /***********************************************************************************************/
  74.                 if(PIDCounter<10)                                  
  75.                 {
  76.                         PIDCounter++;
  77.                 }
  78.                 if(PIDCounter == 10)
  79.                 {
  80.                         PIDflag = 1;
  81.                         PIDCounter = 0;
  82.                 }
  83.                 if((PIDflag == 1)||(cornerflag == 1))
  84.                 {
  85.                         PIDflag=0;
  86.                         cornerflag = 0;
  87.                         LastError = CurrentError;
  88.                         switch(ReadData)   //使用Pf1~Pf10
  89.                         {
  90.                                 case 0xff9f:CurrentError = 0;break;//1111 111110011110
  91.                                 case 0xffbf:CurrentError = 1;break;//1111 111110111110
  92.                                 case 0xff3f:CurrentError = 3;break;//1111 111100111110                          
  93.                                 case 0xff7f:CurrentError = 4;break;//1111 111101111110
  94.                                 case 0xfe7f:CurrentError = 6;break;//1111 111001111110
  95.                                 case 0xfeff:CurrentError = 7;break;//1111 111011111110
  96.                                 case 0xfcff:CurrentError = 9;break;//1111 110011111110
  97.                                 case 0xfdff:CurrentError = 12;break;//1111 110111111110
  98.                                 case 0xf9ff:CurrentError = 14;break;//1111 100111111110
  99.                                 case 0xfbff:CurrentError = 16;break;//1111 101111111110       
  100.                                 case 0xffdf:CurrentError = -0;break;//1111 111111011110
  101.                                 case 0xffcf:CurrentError = -1;break;//1111 111111001110
  102.                                 case 0xffef:CurrentError = -3;break;//1111 111111101110
  103.                                 case 0xffe7:CurrentError = -6;break;//1111 111111100110
  104.                                 case 0xff77:CurrentError = -7;break;//1111 111101110110
  105.                                 case 0xfff3:CurrentError = -9;break;//1111 111111110010
  106.                                 case 0xfffb:CurrentError = -12;break;//1111 11111111010
  107.                                 case 0xfff9:CurrentError = -14;break;//1111 11111111000
  108.                                 case 0xfffc:CurrentError = -16;break;//1111 11111111100
  109.                                 case 0xff1f:CurrentError = 3;break;//1111 111100011110
  110.                                 case 0xfe3f:CurrentError = 6;break;//1111 111000111110       
  111.                                 case 0xfc7f:CurrentError = 12;break;//1111 110001111110
  112.                                 case 0xf8ff:CurrentError = 17;break;//1111 100011111110                 
  113.                                 case 0xff8f:CurrentError = -3;break;//1111 111110001110
  114.                                 case 0xffc7:CurrentError = -6;break;//1111 111111000110       
  115.                                 case 0xffe3:CurrentError = -12;break;//1111 111111100010
  116.                                 case 0xfff1:CurrentError = -17;break;//1111 111111110000
  117.                                 default:break;
  118.                         }
  119.                         if((CurrentError-LastError)>=6||((CurrentError-LastError)<=-6))
  120.                         {
  121.                                  BasicSpeed = 80 ;       
  122.                                  if((CurrentError>=7)||(CurrentError<=-7))
  123.                                  {
  124.                                          BasicSpeed = 40;
  125.                                  }
  126.                         }
  127.                         else
  128.                         {
  129.                                 BasicSpeed = basespeed;
  130.                         }
  131.                         DiffError = CurrentError - LastError;
  132.                         Output = CurrentError * 45 + DiffError * 560 ;
  133.                         LeftSpeed = BasicSpeed - 6 + Output;
  134.                         RightSpeed = BasicSpeed - Output;
  135.                         if(LeftSpeed>550)
  136.                         {
  137.                                 LeftSpeed = 550;
  138.                         }  
  139.                         else if(LeftSpeed < -500)
  140.                         {
  141.                                 LeftSpeed = -500;
  142.                         }
  143.                         if(RightSpeed>550)
  144.                         {
  145.                                 RightSpeed = 550;
  146.                         }
  147.                         else if(RightSpeed < -500)
  148.                         {
  149.                                 RightSpeed = -500;
  150.                         }
  151.                                
  152.                         motor(LeftSpeed,RightSpeed);
  153.                         //TIM_ClearITPendingBit(TIM4,TIM_FLAG_Update);
  154.                 }
  155.         }
  156. //        TIM_Cmd(TIM4, ENABLE);
  157.         TIM_ClearITPendingBit(TIM4,TIM_FLAG_Update);       
  158. }  

  160. void EXTI4_IRQHandler(void)
  161. {
  162.         TIM_Cmd(TIM4, DISABLE);
  163. //        delay_ms(15);
  164.         if( GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_4)==0)
  165.         {
  166.                 if(bizhangflag1 == 0)  //第一次壁障  抄近道
  167.                 {
  168.                         bizhangflag1 = 1;
  169.                         motor(0,0);
  170.                         delay_ms(145);
  171.                         motor(0,500);
  172.                         delay_ms(200);
  173.                         motor(545,550);
  174.                         delay_ms(1160);
  175.                         while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)&&
  176.                                  (GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f)&&(GPIO_ReadInputData(GPIOF)!=0xffc7)
  177.                                  &&(GPIO_ReadInputData(GPIOF)!=0xfe3f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xffcf)
  178.                                  &&(GPIO_ReadInputData(GPIOF)!=0xfff9)&&(GPIO_ReadInputData(GPIOF)!=0xf9ff)&&(GPIO_ReadInputData(GPIOF)!=0xfffb)&&
  179.                                  (GPIO_ReadInputData(GPIOF)!=0xfdff));
  180.                 }
  181.                 else
  182.                 {
  183.                         if(bizhangflag3 == 0)                //第二次壁障 減速  準備抓東西
  184.                         {
  185.                                 catchflag = 1;
  186.                                 bizhangflag3 = 1;
  187.                                 motor(0,0);
  188.                                 delay_ms(400);
  189.                                 basespeed = 120;
  190.                         }       
  191.                         else if(bizhangflag3 == 1)        //第三次壁障  減速  準備放東西
  192.                         {
  193.                                 catchflag = 1;
  194.                                 basespeed = 200;
  195.                         }       
  196.                 }
  197.         }
  198.         TIM_Cmd(TIM4, ENABLE);       
  199.         EXTI_ClearITPendingBit(EXTI_Line4);
  200. }                 
  201. void EXTI2_IRQHandler(void)
  202. {
  203. //        delay_ms(5);
  204.         if( (GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_2)==0)&&(catchflag == 1))
  205.         {
  206.                 if(bizhangflag2 == 0)                        //第一次觸碰開關(guān)   抓東西 繞開障礙物
  207.                 {
  208.                         TIM_Cmd(TIM4, DISABLE);
  209.                         catchflag = 0;
  210.                         bizhangflag2= 1;
  211.                         motor(50,50);
  212.                         delay_ms(10);
  213.                         basespeed = 490;
  214.                         starthand();
  215.                         delay_ms(500);
  216.                         motor(-400,-400);
  217.                         delay_ms(400);
  218.                         motor(0,0);
  219.                         delay_ms(100);
  220.                 //        motor(400,0);
  221.                         //delay_ms(8);
  222.                 //        motor(0,0);
  223.                 //        delay_ms(100);
  224.                         motor(280,540);
  225.                         while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)&&
  226.                                  (GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f)&&(GPIO_ReadInputData(GPIOF)!=0xffc7)
  227.                                  &&(GPIO_ReadInputData(GPIOF)!=0xfe3f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xffcf));
  228.                 //        motor(0,0);
  229.                 //
  230.                         delay_ms(30);
  231.                         TIM_Cmd(TIM4, ENABLE);       
  232.        
  233.                 }
  234.                 else                                   //第二次觸碰開關(guān)  放東西 全場結(jié)束
  235.                 {
  236.                         motor(0,0);
  237.                         delay_ms(100);
  238.                         endhand();
  239.                         while(1); //全場結(jié)束

  241.                 }
  242.         }
  243.         EXTI_ClearITPendingBit(EXTI_Line2);
  244.        
  245. }


復(fù)制代碼

所有資料51hei提供下載:
巡線小車程序.7z (209.03 KB, 下載次數(shù): 32)


作者: admin    時間: 2018-12-20 16:20
補全原理圖或者詳細說明一下電路連接即可獲得100+黑幣



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