標題: 程序更新！STM32+OV7670+IL9325找球（黑底紅球）帶2路PWM（PID）程序 [打印本頁]
作者: zhangpro666    時間: 2017-8-12 14:54
標題: 程序更新！STM32+OV7670+IL9325找球（黑底紅球）帶2路PWM（PID）程序
本帖最后由 zhangpro666 于 2017-8-13 11:13 編輯

呃...看到大家都在電賽，自己暑假無聊玩一下，如果大家覺得有用就拿去吧。用的是啟光電子的STM32，OV7670，ILI9325，理論上核心板通過跳線都可以實現(xiàn)。



原理圖：


stm32主程序：
  1. /*
  2. OV7670模塊 STM32函數(shù)

  4. 選用STM32F103RBT6芯片   
  5. TFT驅(qū)動IC為  R61505V

  7.   程序中 用到的TFT 為 我們店的 2.4寸TFT  驅(qū)動芯片為 R61505V        
  8.   TFT用到的GPIO  PB為數(shù)據(jù)口        PC.6  CS   PC.7  RS          PC.8  WR   PC.9  RD        PC.10 RES
  9.   此函數(shù) 用寄存器方式配置GPIO  在TFT的數(shù)據(jù) 命令腳配置上用的是寄存器  這樣能大幅度提高輸出速度

  11. 攝像頭方面
  12. 攝像頭 數(shù)據(jù)口 為PA口  AO-A7
  13. SCL    PC11
  14. SDA    PC12
  15. OE           PC3                 //片選信號(OE)
  16. WRST   PC4                 //寫指針復(fù)位
  17. RRST   PC2                 //讀指針復(fù)位
  18. RCLK   PC1                 //讀數(shù)據(jù)時鐘
  19. WEN           PC5                 //寫入FIFO使能
  20. VSY           PD2           //同步信號檢測IO

  22. 整個程序流程介紹

  24. 初始化TFT   
  25. 初始化ov7670  ov7670是通過SCCB 初始化的  即 SCL 和SDA就可以初始化OV7670  SDA SCL類似于I2C效果
  26.               所以在調(diào)模塊前 一定要確定OV7670是否已經(jīng)初始化
  27. 開啟中斷 判斷幀數(shù)據(jù)是否接收
  28. 死循環(huán) 讀取并顯示 攝像數(shù)據(jù)

  30.            
  31. */

  33. #include  "stm32f10x.h"
  34. #include  "delay.h"
  35. #include  "led.h"
  36. #include  "tft.h"
  37. #include  "led.h"
  38. #include "ov7670.h"
  39. #include  "gui.h"
  40. #include "usart.h"
  41. #include  "PID.h"

  43. #define KEY0  GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_9)        
  44. #define KEY1  GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_10)

  46. void coordinate_output(void);


  49. extern u8 ov_sta;        //幀次數(shù) 置位標志位
  50. struct{
  51. u8 photo;
  52. u8 pub_buf[14401];//120*120
  53. char buf[200];
  54. }CAM;



  58. //更新LCD顯示
  59. void camera_refresh(void)
  60. {
  61.         u32 j;
  62.          u16 color,n565Color;
  63.         u8 cRed=0,cBlue=0,cGreen=0;         
  64.         
  65.         TFT_SCAN(5);   //設(shè)置TFT掃描方向 為從下到上 從右到左
  66.         Address_set(60,0,179,119,0,0);                   //設(shè)置顯示范圍        
  67.         if(ov_sta==2)                                //判斷緩存區(qū)是否存好攝像數(shù)據(jù)
  68.         {
  69.                   
  70.                 OV7670_RRST_0;                                //開始復(fù)位讀指針
  71.                 OV7670_RCK_0;
  72.                 OV7670_RCK_1;
  73.                 OV7670_RCK_0;               
  74.                 OV7670_RRST_1;                                //復(fù)位讀指針結(jié)束
  75.                 OV7670_RCK_1;  
  76.                
  77.                 TFT_RS_1;                 //寫數(shù)據(jù)線拉高                為提高寫入速度

  79.                 //此用法 是將 下面寫數(shù)據(jù)的步驟完全拆開  主要是為了提高顯示速度  （下面注釋部分為參考部分）
  80.                      
  81.                 for(j=0;j<14400;j++)                           //分辨率為240x320   每個顏色點要兩個字節(jié) 所以 240x320x2=76800  ?
  82.                 {                        
  83.                         OV7670_RCK_0;                                 //每一次時鐘跳變 讀一次數(shù)據(jù)
  84.                         color=GPIOC->IDR&0XFF;        //讀數(shù)據(jù)   讀取顏色高字節(jié)數(shù)據(jù)
  85.                         OV7670_RCK_1;
  86.                         color<<=8;                                   //左移8位  將高字節(jié)移到高8位  為接收低8位字節(jié)做準備
  87.                         OV7670_RCK_0;
  88.                         color|=GPIOC->IDR&0XFF;        //讀數(shù)據(jù)   讀取顏色低字節(jié)數(shù)據(jù) 并轉(zhuǎn)化為16位 565 顏色數(shù)據(jù)
  89.                         OV7670_RCK_1;
  90.                   cRed = ((color & 0xf800)>>11);
  91.                         CAM.pub_buf[j]=cRed;
  92.                   n565Color = (cRed << 11) + (cGreen << 5) + (cBlue << 0);                 
  93.             GPIOB->ODR=n565Color;                           //對TFT并口快速寫數(shù)據(jù)
  94.       TFT_WR_0;                                                                      //開始寫入
  95.       TFT_WR_1;
  96.                 }  
  97.                  coordinate_output();
  98.                 EXTI_ClearITPendingBit(EXTI_Line2);  //清除LINE8上的中斷標志位
  99.                 ov_sta=0;                                        //開始下一次采集
  100.         }
  101. }
  102. u16 blight=15;//亮度判斷更改這里
  103. void coordinate_output(void)
  104. {
  105.         u16 i,j,k;
  106.         xmax=ymax=0;
  107.         xmin=ymin=119;
  108.         for(i=0;i<120;i++)
  109.         {
  110.                   for(j=0;j<120;j++)
  111.                         {
  112.                                             if(CAM.pub_buf[(i*120)+j]>=blight)
  113.                   {
  114.                                                                                  if(i>1&&i<119&&j<119&&j>1)//如果i值大于8且小于136（限制小球在8~136行），同理，限制小球在16~160列
  115.                                                                                  {
  116.                                                                                             if(j>xmax) {xmax=j;} //不斷更新邊緣，直到找到小球的直徑
  117.                                                                                              if(j<xmin) {xmin=j;}//不斷更新邊緣，直到找到小球的直徑
  118.                                                 
  119.                                                                                             if(i>ymax) {ymax=i;}//不斷更新邊緣，直到找到小球的直徑
  120.                                                                                              if(i<ymin) {ymin=i;}//不斷更新邊緣，直到找到小球的直徑                                                                                         
  121.                                                                                  }
  122.                   }
  123.                         }
  124.         }
  125.         for(k=0;k<14400;k++)
  126.         {
  127.                 CAM.pub_buf[k]=0x00;
  128.         }
  129.         x=(xmax+xmin)/2;
  130.         y=(ymax+ymin)/2;
  131.         if(x<1){x=1;}
  132.         if(x>119){x=119;}
  133.         if(y<1){y=1;}
  134.         if(y>119){y=119;}
  135.         if(x!=0&&y!=0)
  136.         {        
  137. //        GUI_sprintf_hzstr16x(20,200,"                                        ",Red,White);
  138.                 sprintf(CAM.buf,"X:%d   ",x);
  139.                 GUI_sprintf_hzstr16x(20,200,CAM.buf,Red,White);
  140.                 sprintf(CAM.buf,"Y:%d   ",y);
  141.                 GUI_sprintf_hzstr16x(80,200,CAM.buf,Red,White);
  142.                 printf("X:%d",x);
  143.                 printf("Y:%d",y);
  144.                 printf("\r\n");
  145.         }
  146. }

  148. void disp_picture(void)
  149. {
  150.         u16 i,j=0;
  151.         u16 COLOR;
  152.         u8 aRed=0,aBlue=0,aGreen=0;
  153.         coordinate_output();
  154.         
  155.         TFT_SCAN(5);
  156.         Address_set(60,0,179,119,0,0);        
  157.         TFT_RS_1;               

  159.         for(i=0;i<14400;i++)                           
  160.                 {                        
  161.                         aRed=CAM.pub_buf[i];
  162.                   printf("%d        ",aRed);
  163.                         j++;
  164.                         if(j>=120){printf("\r\n");j=0;}               
  165.                   COLOR = (aRed << 11) + (aGreen << 5) + (aBlue << 0);
  166.             GPIOB->ODR=COLOR;                           //對TFT并口快速寫數(shù)據(jù)
  167.       TFT_WR_0;                                                                      //開始寫入
  168.       TFT_WR_1;               
  169.                 }               
  170.          
  171.                 EXTI_ClearITPendingBit(EXTI_Line2);
  172.                
  173. }

  175. int main(void)
  176. {
  177. u8 lightmode=2,saturation=3,brightness=2,contrast=2;
  178. u8 effect=0,flag=0;
  179.           delay_init();                     //延時函數(shù)初始化
  180.           NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);                        
  181.           Lcd_Init();                         //LCD  初始化
  182.          LED_Init();                  
  183.           uart_init(115200);
  184.          TIM2_Int_Init(1,1799);
  185.           TFT_CS(0);                         //打開LCD片選使能
  186.       GUI_Clear(White);                //清屏

  188.                   while(OV7670_Init())//初始化OV7670
  189.         {
  190.             GUI_sprintf_hzstr16x(20,150,"OV7670 Error!!            ",Blue,White);
  191.                 delay_ms(200);
  192.                 printf("OV7670 ERROR!!!!!\r\n");
  193.         }

  195.           GUI_Clear(White);                //清屏                初始化成功后 紅色清屏

  197.                 CAM.photo=0;
  198. //         delay_ms(1500);
  199.          //攝像頭 參數(shù)設(shè)置  屏蔽后為默認效果                    
  200.         OV7670_Light_Mode(lightmode);
  201.         OV7670_Color_Saturation(saturation);
  202.         OV7670_Brightness(brightness);
  203.         OV7670_Contrast(contrast);
  204.          OV7670_Special_Effects(effect);         

  206.         EXTI2_Init();                                           //初始化中斷線
  207.         OV7670_Window_Set(10,174,120,120);
  208.         OV7670_CS=0;               
  209.           while(1)                                                           //死循環(huán)顯示 攝像數(shù)據(jù)
  210.           {
  211.                 camera_refresh();
  212. //                 test_motor();
  213.           }
  214. }

復(fù)制代碼

全部資料下載地址：
STM32找球.rar (429.95 KB, 下載次數(shù): 60)
紫電a原理圖.pdf (102.27 KB, 下載次數(shù): 30)


作者: zhangpro666    時間: 2017-8-12 14:59
支持坐標1~119
作者: zhangpro666    時間: 2017-8-13 11:12

  1. <p>#include "pid.h"
  2. #include "usart.h"
  3. #include "led.h"</p><p>int x,y,xmax,ymax,xmin,ymin;</p><p>
  4. struct{</p><p>int setspeed;
  5. int actspeed;
  6. int err;  
  7. int err_last;   
  8. int err_next;      
  9. float Kp,Ki,Kd;
  10. float integral;
  11. int incrementSpeed;</p><p>}pid;</p><p>void PID_init(void)
  12. {
  13. pid.setspeed=0;
  14. pid.actspeed=60;
  15. pid.err=0;
  16. pid.Kp=0.25;  //這里調(diào)節(jié)P參數(shù)
  17. pid.Ki=0.25;  //這里調(diào)節(jié)I參數(shù)
  18. pid.Kd=0.21;  //這里調(diào)節(jié)D參數(shù)
  19. }
  20. int PID_adjustment(int speed,int act) //PID處理函數(shù)，輸入X、Y到目標的距離  輸出應(yīng)該移動的距離
  21. {
  22. pid.setspeed=speed;
  23. pid.actspeed=act;
  24. pid.err=pid.setspeed-pid.actspeed;
  25. pid.incrementSpeed=pid.Kp*(pid.err-pid.err_next)+pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);
  26. // pid.actspeed+=pid.incrementSpeed;
  27. pid.err_last=pid.err_next;
  28.     pid.err_next=pid.err;

  30. return pid.incrementSpeed;
  31. }</p><p>void DIST_TO_PWM(int dist,u8 zhou) //距離轉(zhuǎn)換 輸入 距離 方向 軸 直接調(diào)整
  32. {
  33.    if(zhou==0)//X軸
  34.    {
  35.     timer1=30+dist*0.2;  //這里調(diào)整X軸，注意！如果X軸反了，把+改成-
  36. //    printf("PIDPWMX:%d",timer1);
  37. //    printf("\r\n");
  38.    }
  39.    if(zhou==1)//Y軸
  40.    {
  41.     timer2=30+dist*0.2;  //這里調(diào)節(jié)Y軸，注意！如果Y軸反了，把+改成-
  42. //    printf("PIDPWMY:%d",timer2);
  43. //    printf("\r\n");
  44.    }
  45. }
  46. void zuobiao_process(u16 ux,u16 uy)//輸入目標的坐標
  47. {
  48. int xdist,ydist,dist;//x、y離坐標的距離
  49. xdist=ux-x;
  50. ydist=uy-y;
  51. dist=PID_adjustment(0,xdist);
  52. if(dist>50){dist=50;} //限制距離調(diào)節(jié)在50
  53. // printf("PIDx:%d",dist);
  54. if(dist<0){DIST_TO_PWM(dist,0);}//處理X軸
  55. if(dist>0){DIST_TO_PWM(dist,0);}
  56. if(dist==0){timer1=30;}
  57. dist=PID_adjustment(0,ydist);
  58. if(dist>50){dist=50;}  //限制距離調(diào)節(jié)在50
  59. // printf("PIDy:%d",dist);
  60. // printf("\r\n");
  61. if(dist!=0){DIST_TO_PWM(dist,1);}//處理Y軸
  62. if(dist==0){timer2=30;}
  63. }

  65. </p>
復(fù)制代碼

程序更新！誰說STM32F103慢？黑底找球+兩路PWM舵機輸出（帶PID），可以在程序里面調(diào)節(jié)PID大小，實測輸出FPS:20左右！


截屏_20170813_105441.jpg (900.13 KB, 下載次數(shù): 102)

截屏_20170813_105441.jpg

PID V1.00.rar

4.99 MB, 下載次數(shù): 25, 下載積分: 黑幣 -5

作者: plj213    時間: 2019-4-4 16:19

謝謝分享。。。。。。。。。
作者: plj213    時間: 2019-4-4 16:23

謝謝分享。。。。
作者: 卡洛琦    時間: 2019-4-5 06:51
謝謝分享！��！



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