设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 资料共享 › stm32循迹避障小车代码
返回列表 发新帖
查看: 3619|回复: 1
收起左侧

stm32循迹避障小车代码

[复制链接]
ID:380211 发表于 2018-12-20 08:11 | 显示全部楼层 |阅读模式
  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. //红外对管对应于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)                        //第一次触碰开关   抓东西 绕开障碍物
  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                                   //第二次触碰开关  放东西 全场结束
  235.                 {
  236.                         motor(0,0);
  237.                         delay_ms(100);
  238.                         endhand();
  239.                         while(1); //全场结束

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


复制代码

所有资料51hei提供下载:
巡线小车程序.7z (209.03 KB, 下载次数: 32)
回复

使用道具 举报

ID:1 发表于 2018-12-20 16:20 | 显示全部楼层
补全原理图或者详细说明一下电路连接即可获得100+黑币
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表