久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開(kāi)始

搜索
查看: 4248|回復(fù): 2
打印 上一主題 下一主題
收起左側(cè)

電賽STM32F1紅色小球識(shí)別的板球控制-ov7670程序

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:472829 發(fā)表于 2019-1-25 14:52 | 只看該作者 回帖獎(jiǎng)勵(lì) |倒序?yàn)g覽 |閱讀模式
電賽板球系統(tǒng)STM32單片機(jī)源程序如下:
  1. #include "led.h"
  2. #include "delay.h"
  3. #include "key.h"
  4. #include "sys.h"
  5. #include "lcd.h"
  6. #include "usart.h"         
  7. #include "string.h"
  8. #include "ov7670.h"
  9. #include "tpad.h"
  10. #include "timer.h"
  11. #include "exti.h"
  12. #include "usmart.h"
  13. #include "EasyTracered.h"
  14. #include "io_in_out.h"
  15. #include "oled.h"
  16. #include "gui.h"
  17. #include "pid.h"
  18. /************************************************
  19. 作者：TZT
  20. ************************************************/
  21. void camera_refresh(void);
  22. /*---------------------結(jié)構(gòu)體全局定義-----------------*/
  23. RESULT Resured;//判定為的目標(biāo)條件
  24. /*---------------------LCD顯示-----------------*/
  25. const u8*LMODE_TBL[5]={"Auto","Sunny","Cloudy","Office","Home"};                                                        //5種光照模式            
  26. const u8*EFFECTS_TBL[7]={"Normal","Negative","B&W","Redish","Greenish","Bluish","Antique"};        //7種特效
  27. extern u8 ov_sta;        //在exit.c里 面定義
  28. extern u8 ov_frame;        //在timer.c里面定義               
  29. /*---------------------------結(jié)構(gòu)體變量-------------------*/
  30. extern PIDTypdDef M1PID;     //PID1
  31. extern PIDTypdDef M2PID;     //PID2
  32. /*---------------------------x和y的誤差-------------------*/
  33.           float  x1_error;
  34.           float  x2_error;
  35.           float  x3_error;
  36.           float  x4_error;
  37.                 float  y1_error;
  38.           float  y2_error;
  39.           float  y3_error;
  40.           float  y4_error;

  42.           float  x1_1error;
  43.           float  x2_2error;
  44.           float  x3_3error;
  45.           float  x4_4error;
  46.                 float  y1_1error;
  47.           float  y2_2error;
  48.           float  y3_3error;
  49.           float  y4_4error;
  50.                
  51.                 float  sx_1error;
  52.           float  sx_2error;
  53.           float  sx_3error;
  54.           float  sx_4error;
  55.                 float  sx_5error;
  56.           float  sx_6error;

  58.     float  sy_1error;
  59.           float  sy_2error;
  60.           float  sy_3error;
  61.           float  sy_4error;
  62.                 float  sy_5error;
  63.           float  sy_6error;
  64.                
  65.                 float  sx_1need = 209.0;
  66.           float  sx_2need = 254.0;
  67.     float  sx_3need = 209.0;
  68.           float  sx_4need = 163.0;               
  69.                 float  sx_5need = 117.0;
  70.           float  sx_6need = 70.0;
  71.                 float  sx_7need = 118.0;
  72.                
  73.                 float  sy_1need = 71.0 ;
  74.           float  sy_2need = 116.0;
  75.     float  sy_3need = 161.0;
  76.           float  sy_4need = 117.0;               
  77.                 float  sy_5need = 71.0;
  78.           float  sy_6need = 118.0;
  79.                 float  sy_7need = 162.0;
  80. /*---------------------------菜單選擇---------------------*/
  81. extern unsigned char    RWXZ;     //#任務(wù)選擇
  82. int32_t   PWM1;           //#PWM1
  83. int32_t   PWM2;           //#PWM2

  85. extern float   need_x1 = 209.0;  
  86. extern float   need_x2 = 209.0;
  87. extern float   need_x3 = 118.0;
  88. extern float   need_x4 = 118.0;

  90. extern float   need_y1 = 182.0;  
  91. extern float   need_y2 = 71.0;
  92. extern float   need_y3 = 71.0;
  93. extern float   need_y4 = 182.0;

  95. float   new_x1 ;  
  96. float   new_x2 ;
  97. float   new_x3 ;
  98. float   new_x4 ;

  100. float   new_y1 ;  
  101. float   new_y2 ;
  102. float   new_y3 ;
  103. float   new_y4 ;

  105. extern int32_t  SET_1;           
  106. extern int32_t  SET_2;           
  107. extern int32_t  SET_3;           
  108. extern int32_t  SET_4;           
  109. /*---------------------位置和PWM變量-----------------*/
  110. u16 r,y;
  111. u16 s,z;

  113. int32_t wz_x,wz_y;      //顯示坐標(biāo)

  115. #define M1_PWMMAX  180  //舵機(jī)一最大值

  117. #define M2_PWMMAX  140  //舵機(jī)二最大值


  120. void wkqwz(void)
  121. {
  122.                         if(PFin(7)==0)
  123.                         {
  124.                                 delay_ms(10);
  125.                                 if(PFin(7)==0)
  126.                                 {
  127.                                         while(1)
  128.                                         {
  129.                                    camera_refresh();//更新顯示
  130.                        wz_x = Resured.x;
  131.                        wz_y = Resured.y;
  132.                        OLED_ShowNum(0,0,wz_x,4,16);                     //這句話把存進(jìn)緩存的數(shù)顯示出來(lái)
  133.                        OLED_ShowNum(64,0,wz_y,4,16);
  134.                  OLED_Refresh_Gram();
  135.         if(PFin(7)==1)
  136.                                 {
  137.                                 delay_ms(10);
  138.                                            if(PFin(7)==1)
  139.                                 {
  140.                                         break;       
  141.                                 }
  142.                                 }                                
  143.                           }
  144.                                 }
  145.                         }
  146. }
  147. /*----------------控制舵機(jī)的轉(zhuǎn)動(dòng)角度----------*/
  148. void kz_hs(int32_t  pwm1,int32_t pwm2)
  149. {

  151.                  pwm1 = pwm1 + 1235;   //目標(biāo)121，158 實(shí)際219,260
  152.            pwm2        = 1275 - pwm2;   //   

  154.            TIM_SetCompare1(TIM4,pwm1);//前后       
  155.            TIM_SetCompare4(TIM4,pwm2);//
  156. }
  157. /*----------------------任務(wù)------------------*/
  158. void MODE1(void) //任務(wù)一
  159. {
  160.           TIM_SetCompare1(TIM4,1235);//前后       
  161.     TIM_SetCompare4(TIM4,1275);//左右
  162. }
  163. void MODE2(void) //任務(wù)二
  164. {
  165.        
  166.   // x = 163;
  167.         // y = 117;
  168.           const  float priod = 2000.0;  //周期(毫秒)
  169.           const  uint32_t x_1 = 219;
  170.           const  uint32_t y_1 = 260;
  171.           const  uint32_t need_x = 121;
  172.           const  uint32_t need_x_erro = 98;
  173.           const  uint32_t need_y = 158;
  174.           const  uint32_t need_y_erro = 102;
  175.           static uint32_t MoveTimeCnt = 0;
  176.           float  Normalization = 0.0;
  177.           float  set_x,set_y;
  178.        

  180.     MoveTimeCnt += 5;       
  181.           Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  182.           if(Normalization>=1.0)
  183.                 {
  184.                  Normalization = 1.0;
  185.                 }
  186.     set_x = 254.0 - (91.0 * Normalization) ;
  187.                 set_y = 205.0 - (88.0 * Normalization) ;       
  188.                        
  189.           PID_M1_SetPoint(set_x);        //x的期望值
  190.           PID_M1_SetKp(8.5);
  191.           PID_M1_SetKi(0.04);
  192.           PID_M1_SetKd(100);
  193.     PID_M2_SetPoint(set_y);        //y的期望值
  194.           PID_M2_SetKp(8.4);
  195.           PID_M2_SetKi(0.04);
  196.           PID_M2_SetKd(100);
  197.     PWM1=PID_M1_PosLocCalc((float)Resured.x);
  198.           PWM2=PID_M2_PosLocCalc((float)Resured.y);
  199.        
  200.           if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;     
  201.        
  202.           if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  203.        
  204.           if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  205.        
  206.           if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  208.     kz_hs(PWM1,PWM2);//控制函數(shù)

  210. }
  211. void MODE3(void) //任務(wù)三
  212. {
  213.           // x1 = 163 , y1 = 207
  214.           // x2 = 163 , y2 = 117
  215.           const  float priod = 1900.0;  //周期(毫秒)
  216.           const  uint32_t x_1 = 219;
  217.           const  uint32_t y_1 = 260;
  218.           const  uint32_t need_x = 121;
  219.           const  uint32_t need_x_erro = 98;
  220.           const  uint32_t need_y = 158;
  221.           const  uint32_t need_y_erro = 102;
  222.           static uint32_t MoveTimeCnt = 0;
  223.           float  Normalization = 0.0;
  224.     float  set_x,set_y;
  225.           float  hc_time = 600;
  226.        
  227.     MoveTimeCnt += 5;                                                                         //每5ms運(yùn)算1次
  228.        
  229.         if(MoveTimeCnt<=priod)
  230.         {
  231.                
  232.                  Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  233.            if(Normalization>=1.0)
  234.                  {
  235.                   Normalization = 1.0;
  236.                  }
  237.     set_x = 254.0 - (91.0 * Normalization);//  x = 72 ,y = 28;
  238.                 set_y = 205.0 - (-2.0 * Normalization) ;       
  239.                  
  240.                 PID_M1_SetPoint(set_x);        //x的期望值
  241.           PID_M1_SetKp(8.7);
  242.           PID_M1_SetKi(0.07);
  243.           PID_M1_SetKd(100);
  244.     PID_M2_SetPoint(set_y);        //y的期望值
  245.           PID_M2_SetKp(8.6);
  246.           PID_M2_SetKi(0.07);
  247.           PID_M2_SetKd(100);                 
  248.         }
  249.   else if(MoveTimeCnt>(priod+hc_time))
  250.                 {
  251.                         Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod;         //對(duì)板球周期歸一化
  252.            if(Normalization>=1.0)
  253.                  {
  254.                   Normalization = 1.0;
  255.                  }
  256.     set_x = 163.0 - (0.0 * Normalization);//  x = 72 ,y = 28;
  257.                 set_y = 207.0 - (90.0 * Normalization) ;       
  258.                  
  259.                 PID_M1_SetPoint(set_x);        //x的期望值
  260.           PID_M1_SetKp(8.2);
  261.           PID_M1_SetKi(0.04);
  262.           PID_M1_SetKd(100);
  263.     PID_M2_SetPoint(set_y);        //y的期望值
  264.           PID_M2_SetKp(8.1);
  265.           PID_M2_SetKi(0.04);
  266.           PID_M2_SetKd(100);
  267.                 }
  268.         PWM1=PID_M1_PosLocCalc((float)Resured.x);
  269.                
  270.         PWM2=PID_M2_PosLocCalc((float)Resured.y);
  271.        
  272.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;     
  273.        
  274.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  275.        
  276.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  277.        
  278.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  280.   kz_hs(PWM1,PWM2);//控制函數(shù)
  281.                
  282. }  
  283. void MODE4(void) //任務(wù)四
  284. {
  285.          // x = 72; y = 28;
  286.                 const  float priod = 2000.0;  //周期(毫秒)
  287.           const  uint32_t x_1 = 219;
  288.           const  uint32_t y_1 = 260;
  289.           const  uint32_t need_x = 121;
  290.           const  uint32_t need_x_erro = 98;
  291.           const  uint32_t need_y = 158;
  292.           const  uint32_t need_y_erro = 102;
  293.           static uint32_t MoveTimeCnt = 0;
  294.           float  Normalization = 0.0;
  295.           float  set_x,set_y;
  296.     float  hc_time = 1000;
  297.        
  298.     MoveTimeCnt += 5;       
  299.           Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化

  301.         if(MoveTimeCnt<=priod)
  302.         {
  303.                
  304.                  Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  305.            if(Normalization>=1.0)
  306.                  {
  307.                   Normalization = 1.0;
  308.                  }
  309.     set_x = 254.0 - (66.0 * Normalization);//  x = 72 ,y = 28;
  310.                 set_y = 205.0 - (120.0 * Normalization) ;       
  311.                  
  312.                 PID_M1_SetPoint(set_x);        //x的期望值
  313.           PID_M1_SetKp(8.2);
  314.           PID_M1_SetKi(0.07);
  315.           PID_M1_SetKd(100);
  316.     PID_M2_SetPoint(set_y);        //y的期望值
  317.           PID_M2_SetKp(8.1);
  318.           PID_M2_SetKi(0.07);
  319.           PID_M2_SetKd(100);         
  320.         }
  321.         /*---------------------------------------------*/
  322.   else if(MoveTimeCnt>priod)
  323.                 {
  324.                         Normalization = (float)(MoveTimeCnt-priod) / priod;         //對(duì)板球周期歸一化
  325.            if(Normalization>=1.0)
  326.                  {
  327.                   Normalization = 1.0;
  328.                  }
  329.     set_x = 188.0 - (116.0 * Normalization);//  x = 72 ,y = 28;
  330.                 set_y = 85.0 - (57.0 * Normalization) ;       
  331.                  
  332.                 PID_M1_SetPoint(set_x);        //x的期望值
  333.           PID_M1_SetKp(8.2);
  334.           PID_M1_SetKi(0.02);
  335.           PID_M1_SetKd(100);
  336.     PID_M2_SetPoint(set_y);        //y的期望值
  337.           PID_M2_SetKp(8.1);
  338.           PID_M2_SetKi(0.02);
  339.           PID_M2_SetKd(100);       
  340.                 }
  341.                
  342.     PWM1=PID_M1_PosLocCalc((float)Resured.x);
  343.           PWM2=PID_M2_PosLocCalc((float)Resured.y);
  344.        
  345.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;      
  346.        
  347.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  348.        
  349.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  350.        
  351.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  353.   kz_hs(PWM1,PWM2);//控制函數(shù)
  354. }
  355. void MODE5(void) //任務(wù)五
  356. {
  357.            // x1 = 254; y1 = 205;
  358.            // x2 = 254; y2 = 116;
  359.            // x3 = 163; y3 = 25;
  360.            // x3 = 72 ; y4 = 28;
  361.        
  362.                 const  float priod = 2000.0;  //周期(毫秒)
  363.           const  uint32_t x_1 = 219;
  364.           const  uint32_t y_1 = 260;
  365.           const  uint32_t need_x = 121;
  366.           const  uint32_t need_x_erro = 98;
  367.           const  uint32_t need_y = 158;
  368.           const  uint32_t need_y_erro = 102;
  369.           static uint32_t MoveTimeCnt = 0;
  370.           float  Normalization = 0.0;
  371.           float  set_x,set_y;
  372.           float  hc_time = 500;
  373.           
  374.        
  375.     MoveTimeCnt += 5;                                                                       //每5ms運(yùn)算1次
  376.         /*-----------------1 到 2 ----------------------------*/
  377.         if(MoveTimeCnt<=priod)
  378.         {       
  379.                  Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  380.            if(Normalization>=1.0)
  381.                  {
  382.                   Normalization = 1.0;
  383.                  }
  384.     set_x = 254.0 - (0.0 * Normalization);//  x = 254 ,y = 116;
  385.                 set_y = 205.0 - (89.0 * Normalization) ;       
  386.                  
  387.                 PID_M1_SetPoint(set_x);        //x的期望值
  388.           PID_M1_SetKp(8.2);
  389.           PID_M1_SetKi(0.04);
  390.           PID_M1_SetKd(100);
  391.     PID_M2_SetPoint(set_y);        //y的期望值
  392.           PID_M2_SetKp(8.1);
  393.           PID_M2_SetKi(0.04);
  394.           PID_M2_SetKd(100);                 
  395.         }
  396.         /*-----------------2 到 6 ----------------------------*/
  397.                 else if((MoveTimeCnt > (priod+hc_time))&&(MoveTimeCnt < (2.0 * priod + hc_time)))
  398.                 {
  399.                         Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod;         //對(duì)板球周期歸一化
  400.            if(Normalization>=1.0)
  401.                  {
  402.                   Normalization = 1.0;
  403.                  }
  404.     set_x = 254.0 - (91.0 * Normalization);//  x = 72 ,y = 28;
  405.                 set_y = 116.0 - (91.0 * Normalization) ;       
  406.                  
  407.                 PID_M1_SetPoint(set_x);        //x的期望值
  408.           PID_M1_SetKp(6.2);
  409.           PID_M1_SetKi(0.01);
  410.           PID_M1_SetKd(100);
  411.     PID_M2_SetPoint(set_y);        //y的期望值
  412.           PID_M2_SetKp(6.1);
  413.           PID_M2_SetKi(0.01);
  414.           PID_M2_SetKd(100);
  415.                 }
  416.         /*-----------------6 到 9 ----------------------------*/
  417.                 else if(MoveTimeCnt>( 2.0 * priod + hc_time ))
  418.                 {
  419.                         Normalization = (float)(MoveTimeCnt-(2.0 * priod + hc_time)) / priod;         //對(duì)板球周期歸一化
  420.            if(Normalization>=1.0)
  421.                  {
  422.                   Normalization = 1.0;
  423.                  }
  424.     set_x = 163.0 - (91.0 * Normalization);//  x = 163 ,y = 25;
  425.                 set_y = 25.0 - (0.0 * Normalization) ;       
  426.                  
  427.                 PID_M1_SetPoint(set_x);        //x的期望值
  428.           PID_M1_SetKp(8.2);
  429.           PID_M1_SetKi(0.02);
  430.           PID_M1_SetKd(100);
  431.     PID_M2_SetPoint(set_y);        //y的期望值
  432.           PID_M2_SetKp(8.1);
  433.           PID_M2_SetKi(0.02);
  434.           PID_M2_SetKd(100);       
  435.                 }
  436.         PWM1=PID_M1_PosLocCalc((float)Resured.x);
  437.                
  438.         PWM2=PID_M2_PosLocCalc((float)Resured.y);
  439.        
  440.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;     
  441.        
  442.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  443.        
  444.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  445.        
  446.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  448.   kz_hs(PWM1,PWM2);//控制函數(shù)

  450. }
  451. void MODE6(void) //任務(wù)六 走 四個(gè)點(diǎn) 走不了對(duì)角線
  452. {

  454.           const  float priod = 1500.0;  //周期(毫秒)
  455.           const  uint32_t x_1 = 219;
  456.           const  uint32_t y_1 = 260;
  457.           const  uint32_t need_x = 121;
  458.           const  uint32_t need_x_erro = 98;
  459.           const  uint32_t need_y = 158;
  460.           const  uint32_t need_y_erro = 102;
  461.           static uint32_t MoveTimeCnt = 0;
  462.           float  Normalization = 0.0;
  463.           float  set_x,set_y;
  464.           float  hc_time = 100;
  465.        
  466.     MoveTimeCnt += 5;                                                                       //每5ms運(yùn)算1次

  468. /*----------------------180------------a到b-----------------------*/
  469. if(((need_x1-need_x2)==180)||((need_x1-need_x2)==-180)||((need_y1-need_y2)==180)||((need_y1-need_y2)==-180))
  470. {
  471.         if(MoveTimeCnt<=priod)
  472.         {       
  473.                  Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  474.            if(Normalization>=1.0)
  475.                  {
  476.                   Normalization = 1.0;
  477.                  }
  478.                 new_x1 = (need_x1 + need_x2)/2.0 ;  //209-209
  479.                 new_y1 = (need_y1 + need_y2)/2.0 ;  //209-209
  480.                
  481.     new_x1=  (new_x1 + 163) /2.0;  //209
  482.                 new_y1 = (new_y1 + 117)/2.0 ; //117
  483.                
  484.     x1_1error = new_x1 - need_x1;    //163-250 = -87
  485.                 y1_1error = new_y1 - need_y1;    //72-28  = 44
  486.                  
  487.     set_x = need_x1 + (x1_1error * Normalization); //250 + -87 = 163
  488.                 set_y = need_y1 + (y1_1error * Normalization) ;//26 + 44        = 70
  489.                  
  490.                 PID_M1_SetPoint(set_x);        //x的期望值
  491.           PID_M1_SetKp(8.2);
  492.           PID_M1_SetKi(0.02);
  493.           PID_M1_SetKd(100);
  494.     PID_M2_SetPoint(set_y);        //y的期望值
  495.           PID_M2_SetKp(8.1);
  496.           PID_M2_SetKi(0.02);
  497.           PID_M2_SetKd(100);                                  
  498.         }
  499.         else if((MoveTimeCnt > (priod + hc_time) )&&( MoveTimeCnt < (2.0 * priod + hc_time)))
  500.                 {
  501.                         Normalization = (float)(MoveTimeCnt- priod ) / priod;         //對(duì)單擺周期歸一化
  502.            if(Normalization>=1.0)
  503.                  {
  504.                   Normalization = 1.0;
  505.                  }
  506.     x1_error = need_x2  - new_x1 ;
  507.                 y1_error = need_y2  - new_y1;
  508.                  
  509.     set_x = new_x1  + (x1_error* Normalization);//  x = 254 ,y = 116;
  510.                 set_y = new_y1  + (y1_error * Normalization) ;       
  511.                  
  512.                 PID_M1_SetPoint(set_x);        //x的期望值
  513.           PID_M1_SetKp(8.2);
  514.           PID_M1_SetKi(0.02);
  515.           PID_M1_SetKd(100);
  516.     PID_M2_SetPoint(set_y);        //y的期望值
  517.           PID_M2_SetKp(8.1);
  518.           PID_M2_SetKi(0.02);
  519.           PID_M2_SetKd(100);                                        
  520.    }       
  521. }
  522. else if(MoveTimeCnt<= 2.0 * priod )
  523.         {       
  524.                  Normalization = (float)MoveTimeCnt /  2.0 * priod;         //對(duì)板球周期歸一化
  525.            if(Normalization>=1.0)
  526.                  {
  527.                   Normalization = 1.0;
  528.                  }
  529.                 x1_error = need_x1 - need_x2;  //209-209
  530.     y1_error = need_y1 - need_y2;         // 182-71
  531.     set_x = need_x1 - (x1_error * Normalization);//  x = 254 ,y = 116;
  532.                 set_y = need_y1 - (y1_error * Normalization) ;       
  533.                  
  534.                 PID_M1_SetPoint(set_x);        //x的期望值
  535.           PID_M1_SetKp(20.2);
  536.           PID_M1_SetKi(0.01);
  537.           PID_M1_SetKd(100);
  538.     PID_M2_SetPoint(set_y);        //y的期望值
  539.           PID_M2_SetKp(20.1);
  540.           PID_M2_SetKi(0.01);
  541.           PID_M2_SetKd(100);  
  542.         }

  544. /*----------------------180------------b到c-----------------------*/       
  545. if(((need_x1-need_x2)==180)||((need_x1-need_x2)==-180)||((need_y1-need_y2)==180)||((need_y1-need_y2)==-180))
  546. {
  547.          if((MoveTimeCnt > (2.0 * priod+ hc_time) )&&( MoveTimeCnt < (3.0 * priod+ hc_time)))
  548.                 {
  549.                         Normalization = (float)(MoveTimeCnt- (2.0 * priod- hc_time)) / priod;         //對(duì)板球周期歸一化
  550.            if(Normalization>=1.0)
  551.                  {
  552.                   Normalization = 1.0;
  553.                  }
  554.                 new_x2 = (need_x2 + need_x3)/2.0 ;  //209-209
  555.                 new_y2 = (need_y2 + need_y3)/2.0 ;  //209-209
  556.                
  557.     new_x2=  (new_x2 + 163) /2.0;  //209
  558.                 new_y2 = (new_y2 + 117)/2.0 ; //117
  559.                
  560.     x2_2error = new_x2 - need_x2;    //163-250 = -87
  561.                 y2_2error = new_y2 - need_y2;    //72-28  = 44
  562.                  
  563.     set_x = need_x2 + (x2_2error * Normalization); //250 + -87 = 163
  564.                 set_y = need_y2 + (y2_2error * Normalization) ;//26 + 44        = 70
  565.                  
  566.                 PID_M1_SetPoint(set_x);        //x的期望值
  567.           PID_M1_SetKp(8.2);
  568.           PID_M1_SetKi(0.02);
  569.           PID_M1_SetKd(100);
  570.     PID_M2_SetPoint(set_y);        //y的期望值
  571.           PID_M2_SetKp(8.1);
  572.           PID_M2_SetKi(0.02);
  573.           PID_M2_SetKd(100);                                           
  574.         }
  575.         else if((MoveTimeCnt > (3.0 * priod + hc_time))&&( MoveTimeCnt < (4.0 * priod+ hc_time)))
  576.                 {
  577.                         Normalization = (float)(MoveTimeCnt- (3.0*priod- hc_time) ) / priod;         //對(duì)單擺周期歸一化
  578.            if(Normalization>=1.0)
  579.                  {
  580.                   Normalization = 1.0;
  581.                  }
  582.     x2_error = need_x3  - new_x2 ;
  583.                 y2_error = need_y3  - new_y2;
  584.                  
  585.     set_x = new_x2  + (x2_error* Normalization);//  x = 254 ,y = 116;
  586.                 set_y = new_y2  + (y2_error * Normalization) ;       
  587.                  
  588.                 PID_M1_SetPoint(set_x);        //x的期望值
  589.           PID_M1_SetKp(8.2);
  590.           PID_M1_SetKi(0.02);
  591.           PID_M1_SetKd(100);
  592.     PID_M2_SetPoint(set_y);        //y的期望值
  593.           PID_M2_SetKp(8.1);
  594.           PID_M2_SetKi(0.02);
  595.           PID_M2_SetKd(100);                                       
  596.    }       
  597. }
  598. else if((MoveTimeCnt > (2.0 * priod + hc_time))&&(MoveTimeCnt < (4.0 * priod + hc_time)))
  599.                 {
  600.                         Normalization = (float)(MoveTimeCnt-(2.0 * priod+ hc_time)) / 2.0 * priod;         //對(duì)板球周期歸一化
  601.            if(Normalization>=1.0)
  602.                  {
  603.                   Normalization = 1.0;
  604.                  }
  605.                 x2_error = need_x2 - need_x3; // 209- 118
  606.     y2_error = need_y2 - need_y3;        // 71 -71
  607.     set_x = need_x2 - (x2_error * Normalization);//  x = 72 ,y = 28;
  608.                 set_y = need_y2  -  (y2_error * Normalization) ;       
  609.                  
  610.                 PID_M1_SetPoint(set_x);        //x的期望值
  611.           PID_M1_SetKp(20.2);
  612.           PID_M1_SetKi(0.01);
  613.           PID_M1_SetKd(100);
  614.     PID_M2_SetPoint(set_y);        //y的期望值
  615.           PID_M2_SetKp(20.1);
  616.           PID_M2_SetKi(0.01);
  617.           PID_M2_SetKd(100);                               
  618.                 }
  619. /*----------------------180------------c到d-----------------------*/       
  620. if(((need_x1-need_x2)==180)||((need_x1-need_x2)==-180)||((need_y1-need_y2)==180)||((need_y1-need_y2)==-180))
  621. {
  622.                  if((MoveTimeCnt > (4.0 * priod+ hc_time) )&&( MoveTimeCnt < (5.0 * priod+ hc_time)))
  623.                 {
  624.                         Normalization = (float)(MoveTimeCnt- (4.0 * priod- hc_time)) / priod;         //對(duì)板球周期歸一化
  625.            if(Normalization>=1.0)
  626.                  {
  627.                   Normalization = 1.0;
  628.                  }
  629.                 new_x3 = (need_x3 + need_x4)/2.0 ;  //209-209
  630.                 new_y3 = (need_y3 + need_y4)/2.0 ;  //209-209
  631.                
  632.     new_x3=  (new_x3 + 163) /2.0;  //209
  633.                 new_y3 = (new_y3 + 117)/2.0 ; //117
  634.                
  635.     x3_3error = new_x3 - need_x3;    //163-250 = -87
  636.                 y3_3error = new_y3 - need_y3;    //72-28  = 44
  637.                  
  638.     set_x = need_x3 + (x3_3error * Normalization); //250 + -87 = 163
  639.                 set_y = need_y3 + (y3_3error * Normalization) ;//26 + 44        = 70
  640.                  
  641.                 PID_M1_SetPoint(set_x);        //x的期望值
  642.           PID_M1_SetKp(8.2);
  643.           PID_M1_SetKi(0.02);
  644.           PID_M1_SetKd(100);
  645.     PID_M2_SetPoint(set_y);        //y的期望值
  646.           PID_M2_SetKp(8.1);
  647.           PID_M2_SetKi(0.02);
  648.           PID_M2_SetKd(100);                                                  
  649.         }
  650.         else if((MoveTimeCnt > (5.0 * priod + hc_time))&&( MoveTimeCnt < (6.0 * priod+ hc_time)))
  651.                 {
  652.                         Normalization = (float)(MoveTimeCnt- (5.0 * priod - hc_time)) / priod;         //對(duì)單擺周期歸一化
  653.            if(Normalization>=1.0)
  654.                  {
  655.                   Normalization = 1.0;
  656.                  }
  657.     x3_error = need_x4  - new_x3 ;
  658.                 y3_error = need_y4  - new_y3;
  659.                  
  660.     set_x = new_x3  + (x3_error* Normalization);//  x = 254 ,y = 116;
  661.                 set_y = new_y3  + (y3_error * Normalization) ;       
  662.                  
  663.                 PID_M1_SetPoint(set_x);        //x的期望值
  664.           PID_M1_SetKp(8.2);
  665.           PID_M1_SetKi(0.02);
  666.           PID_M1_SetKd(100);
  667.     PID_M2_SetPoint(set_y);        //y的期望值
  668.           PID_M2_SetKp(8.1);
  669.           PID_M2_SetKi(0.02);
  670.           PID_M2_SetKd(100);                       
  671.    }       
  672. }                
  673.           else if(MoveTimeCnt> (6.0 * priod + hc_time ))
  674.                 {
  675.                         Normalization = (float)(MoveTimeCnt-(4.0 * priod+ hc_time) ) / priod;         //對(duì)板球周期歸一化
  676.            if(Normalization>=1.0)
  677.                  {
  678.                   Normalization = 1.0;
  679.                  }
  680.                 x3_error = need_x3 - need_x4;  //118-118
  681.     y3_error = need_y3 - need_y4;         //73 - 182
  682.     set_x = need_x3 - (x3_error * Normalization);//  x = 163 ,y = 25;
  683.                 set_y = need_y3 - (y3_error * Normalization) ;       
  684.                  
  685.                 PID_M1_SetPoint(set_x);        //x的期望值
  686.           PID_M1_SetKp(20.2);
  687.           PID_M1_SetKi(0.01);
  688.           PID_M1_SetKd(100);
  689.     PID_M2_SetPoint(set_y);        //y的期望值
  690.           PID_M2_SetKp(20.1);
  691.           PID_M2_SetKi(0.01);
  692.           PID_M2_SetKd(100);                                                          
  693.                 }               

  695.         PWM1=PID_M1_PosLocCalc((float)Resured.x);
  696.                
  697.         PWM2=PID_M2_PosLocCalc((float)Resured.y);
  698.        
  699.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;      
  700.        
  701.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  702.        
  703.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  704.        
  705.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  707.   kz_hs(PWM1,PWM2);//控制函數(shù)

  709. }
  710. void MODE7(void) //任務(wù)七 轉(zhuǎn) 3 圈 回 9
  711. {
  712.           const  float priod = 1000.0;  //周期(毫秒)
  713.           const  uint32_t x_1 = 219;
  714.           const  uint32_t y_1 = 260;
  715.           const  uint32_t need_x = 121;
  716.           const  uint32_t need_x_erro = 98;
  717.           const  uint32_t need_y = 158;
  718.           const  uint32_t need_y_erro = 102;
  719.           static uint32_t MoveTimeCnt = 0;
  720.           float  Normalization = 0.0;
  721.     float  set_x,set_y;
  722.           float  hc_time = 1500;
  723.        
  724.     MoveTimeCnt += 5;                                                                       //每5ms運(yùn)算1次
  725.        
  726.         /*--------------------4-------------------------*/
  727.         if(MoveTimeCnt<=priod)
  728.         {
  729.                
  730.                  Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  731.            if(Normalization>=1.0)
  732.                  {
  733.                   Normalization = 1.0;
  734.                  }
  735.     set_x = 163.0 - (0.0 * Normalization);//  x = 72 ,y = 28;
  736.                 set_y = 207.0 - (45.0 * Normalization) ;       
  737.                  
  738.                 PID_M1_SetPoint(set_x);        //x的期望值
  739.           PID_M1_SetKp(6.3);
  740.           PID_M1_SetKi(0.01);
  741.           PID_M1_SetKd(100);
  742.     PID_M2_SetPoint(set_y);        //y的期望值
  743.           PID_M2_SetKp(6.2);
  744.           PID_M2_SetKi(0.01);
  745.           PID_M2_SetKd(100);                 
  746.         }
  747.         /*---------------------R1_1-----------------------*/
  748.   else if((MoveTimeCnt>priod)&&(MoveTimeCnt<2.0*priod))   
  749.                 {
  750.                         Normalization = (float)(MoveTimeCnt-priod) / priod;         //對(duì)板球周期歸一化
  751.            if(Normalization>=1.0)
  752.                  {
  753.                   Normalization = 1.0;
  754.                  }
  755.     set_x = 163.0 - (-46.0 * Normalization);//  x = 72 ,y = 28;
  756.                 set_y = 162.0 - (45.0 * Normalization) ;       
  757.                  
  758.                 PID_M1_SetPoint(set_x);        //x的期望值
  759.           PID_M1_SetKp(6.2);
  760.           PID_M1_SetKi(0.01);
  761.           PID_M1_SetKd(100);
  762.     PID_M2_SetPoint(set_y);        //y的期望值
  763.           PID_M2_SetKp(6.1);
  764.           PID_M2_SetKi(0.01);
  765.           PID_M2_SetKd(100);
  766.                 }
  767.                 /*---------------------R1_2-----------------------*/
  768.                   else if((MoveTimeCnt>2.0*priod)&&(MoveTimeCnt<3.0*priod))  
  769.                 {
  770.                         Normalization = (float)(MoveTimeCnt-2.0*priod) / priod;         //對(duì)板球周期歸一化
  771.            if(Normalization>=1.0)
  772.                  {
  773.                   Normalization = 1.0;
  774.                  }
  775.     set_x = 209.0 - (46.0 * Normalization);//  x = 72 ,y = 28;
  776.                 set_y = 117.0 - (46.0 * Normalization) ;       
  777.                  
  778.                 PID_M1_SetPoint(set_x);        //x的期望值
  779.           PID_M1_SetKp(6.2);
  780.           PID_M1_SetKi(0.01);
  781.           PID_M1_SetKd(100);
  782.     PID_M2_SetPoint(set_y);        //y的期望值
  783.           PID_M2_SetKp(6.1);
  784.           PID_M2_SetKi(0.01);
  785.           PID_M2_SetKd(100);
  786.                 }
  787.                 /*---------------------R1_3-----------------------*/
  788.                 else if((MoveTimeCnt>(4.0*priod))&&(MoveTimeCnt<(5.0*priod)))  //4
  789.                 {
  790.                         Normalization = (float)(MoveTimeCnt-4.0*priod) / priod;         //對(duì)板球周期歸一化
  791.            if(Normalization>=1.0)
  792.                  {
  793.                   Normalization = 1.0;
  794.                  }
  795.     set_x = 163.0 - (46.0 * Normalization);//  x = 72 ,y = 28;
  796.                 set_y = 71.0 - (-47.0 * Normalization) ;       
  797.                  
  798.                 PID_M1_SetPoint(set_x);        //x的期望值
  799.           PID_M1_SetKp(6.2);
  800.           PID_M1_SetKi(0.01);
  801.           PID_M1_SetKd(100);
  802.     PID_M2_SetPoint(set_y);        //y的期望值
  803.           PID_M2_SetKp(6.1);
  804.           PID_M2_SetKi(0.01);
  805.           PID_M2_SetKd(100);
  806.                 }
  807.                 /*---------------------R1_4-----------------------*/
  808.                  else if((MoveTimeCnt>5.0*priod)&&(MoveTimeCnt<6.0*priod))                        
  809.                 {
  810.                         Normalization = (float)(MoveTimeCnt-5.0*priod) / priod;         //對(duì)板球周期歸一化
  811.            if(Normalization>=1.0)
  812.                  {
  813.                   Normalization = 1.0;
  814.                  }
  815.     set_x = 117.0 - (-46.0 * Normalization);//  x = 72 ,y = 28;
  816.                 set_y = 118.0 - (-47.0 * Normalization) ;       
  817.                  
  818.                 PID_M1_SetPoint(set_x);        //x的期望值
  819.           PID_M1_SetKp(6.2);
  820.           PID_M1_SetKi(0.01);
  821.           PID_M1_SetKd(100);
  822.     PID_M2_SetPoint(set_y);        //y的期望值
  823.           PID_M2_SetKp(6.1);
  824.           PID_M2_SetKi(0.01);
  825.           PID_M2_SetKd(100);
  826.                 }
  827.                 /*---------------------R2_1-----------------------*/
  828.    else if((MoveTimeCnt>6.0*priod)&&(MoveTimeCnt<7.0*priod))   
  829.                 {
  830.                         Normalization = (float)(MoveTimeCnt-6.0*priod) / priod;         //對(duì)板球周期歸一化
  831.            if(Normalization>=1.0)
  832.                  {
  833.                   Normalization = 1.0;
  834.                  }
  835.     set_x = 163.0 - (-46.0 * Normalization);//  x = 72 ,y = 28;
  836.                 set_y = 162.0 - (45.0 * Normalization) ;       
  837.                  
  838.                 PID_M1_SetPoint(set_x);        //x的期望值
  839.           PID_M1_SetKp(6.2);
  840.           PID_M1_SetKi(0.01);
  841.           PID_M1_SetKd(100);
  842.     PID_M2_SetPoint(set_y);        //y的期望值
  843.           PID_M2_SetKp(6.1);
  844.           PID_M2_SetKi(0.01);
  845.           PID_M2_SetKd(100);
  846.                 }
  847.                 /*---------------------R2_2-----------------------*/
  848.                   else if((MoveTimeCnt>7.0*priod)&&(MoveTimeCnt<8.0*priod))  
  849.                 {
  850.                         Normalization = (float)(MoveTimeCnt-7.0*priod) / priod;         //對(duì)板球周期歸一化
  851.            if(Normalization>=1.0)
  852.                  {
  853.                   Normalization = 1.0;
  854.                  }
  855.     set_x = 209.0 - (46.0 * Normalization);//  x = 72 ,y = 28;
  856.                 set_y = 117.0 - (46.0 * Normalization) ;       
  857.                  
  858.                 PID_M1_SetPoint(set_x);        //x的期望值
  859.           PID_M1_SetKp(6.2);
  860.           PID_M1_SetKi(0.01);
  861.           PID_M1_SetKd(100);
  862.     PID_M2_SetPoint(set_y);        //y的期望值
  863.           PID_M2_SetKp(6.1);
  864.           PID_M2_SetKi(0.01);
  865.           PID_M2_SetKd(100);
  866.                 }
  867.                 /*---------------------R2_3-----------------------*/
  868.                 else if((MoveTimeCnt>(8.0*priod))&&(MoveTimeCnt<(9.0*priod)))  //4
  869.                 {
  870.                         Normalization = (float)(MoveTimeCnt-8.0*priod) / priod;         //對(duì)板球周期歸一化
  871.            if(Normalization>=1.0)
  872.                  {
  873.                   Normalization = 1.0;
  874.                  }
  875.     set_x = 163.0 - (46.0 * Normalization);//  x = 72 ,y = 28;
  876.                 set_y = 71.0 - (-47.0 * Normalization) ;       
  877.                  
  878.                 PID_M1_SetPoint(set_x);        //x的期望值
  879.           PID_M1_SetKp(6.2);
  880.           PID_M1_SetKi(0.01);
  881.           PID_M1_SetKd(100);
  882.     PID_M2_SetPoint(set_y);        //y的期望值
  883.           PID_M2_SetKp(6.1);
  884.           PID_M2_SetKi(0.01);
  885.           PID_M2_SetKd(100);
  886.                 }
  887.                 /*---------------------R2_4-----------------------*/
  888.                  else if((MoveTimeCnt>9.0*priod)&&(MoveTimeCnt<10.0*priod))                        
  889.                 {
  890.                         Normalization = (float)(MoveTimeCnt-9.0*priod) / priod;         //對(duì)板球周期歸一化
  891.            if(Normalization>=1.0)
  892.                  {
  893.                   Normalization = 1.0;
  894.                  }
  895.     set_x = 117.0 - (-46.0 * Normalization);//  x = 72 ,y = 28;
  896.                 set_y = 118.0 - (-47.0 * Normalization) ;       
  897.                  
  898.                 PID_M1_SetPoint(set_x);        //x的期望值
  899.           PID_M1_SetKp(6.2);
  900.           PID_M1_SetKi(0.01);
  901.           PID_M1_SetKd(100);
  902.     PID_M2_SetPoint(set_y);        //y的期望值
  903.           PID_M2_SetKp(6.1);
  904.           PID_M2_SetKi(0.01);
  905.           PID_M2_SetKd(100);
  906.                 }
  907.                                 /*---------------------R3_1-----------------------*/
  908.    else if((MoveTimeCnt>10.0*priod)&&(MoveTimeCnt<11.0*priod))   
  909.                 {
  910.                         Normalization = (float)(MoveTimeCnt-10.0*priod) / priod;         //對(duì)板球周期歸一化
  911.            if(Normalization>=1.0)
  912.                  {
  913.                   Normalization = 1.0;
  914.                  }
  915.     set_x = 163.0 - (-46.0 * Normalization);//  x = 72 ,y = 28;
  916.                 set_y = 162.0 - (45.0 * Normalization) ;       
  917.                  
  918.                 PID_M1_SetPoint(set_x);        //x的期望值
  919.           PID_M1_SetKp(6.2);
  920.           PID_M1_SetKi(0.01);
  921.           PID_M1_SetKd(100);
  922.     PID_M2_SetPoint(set_y);        //y的期望值
  923.           PID_M2_SetKp(6.1);
  924.           PID_M2_SetKi(0.01);
  925.           PID_M2_SetKd(100);
  926.                 }
  927.                 /*---------------------R3_2-----------------------*/
  928.                   else if((MoveTimeCnt>11.0*priod)&&(MoveTimeCnt<12.0*priod))  
  929.                 {
  930.                         Normalization = (float)(MoveTimeCnt-11.0*priod) / priod;         //對(duì)板球周期歸一化
  931.            if(Normalization>=1.0)
  932.                  {
  933.                   Normalization = 1.0;
  934.                  }
  935.     set_x = 209.0 - (46.0 * Normalization);//  x = 72 ,y = 28;
  936.                 set_y = 117.0 - (46.0 * Normalization) ;       
  937.                  
  938.                 PID_M1_SetPoint(set_x);        //x的期望值
  939.           PID_M1_SetKp(6.2);
  940.           PID_M1_SetKi(0.01);
  941.           PID_M1_SetKd(100);
  942.     PID_M2_SetPoint(set_y);        //y的期望值
  943.           PID_M2_SetKp(6.1);
  944.           PID_M2_SetKi(0.01);
  945.           PID_M2_SetKd(100);
  946.                 }
  947.                 /*---------------------R3_3-----------------------*/
  948.                 else if((MoveTimeCnt>(12.0*priod))&&(MoveTimeCnt<(13.0*priod)))  //4
  949.                 {
  950.                         Normalization = (float)(MoveTimeCnt-12.0*priod) / priod;         //對(duì)板球周期歸一化
  951.            if(Normalization>=1.0)
  952.                  {
  953.                   Normalization = 1.0;
  954.                  }
  955.     set_x = 163.0 - (46.0 * Normalization);//  x = 72 ,y = 28;
  956.                 set_y = 71.0 - (-47.0 * Normalization) ;       
  957.                  
  958.                 PID_M1_SetPoint(set_x);        //x的期望值
  959.           PID_M1_SetKp(6.2);
  960.           PID_M1_SetKi(0.01);
  961.           PID_M1_SetKd(100);
  962.     PID_M2_SetPoint(set_y);        //y的期望值
  963.           PID_M2_SetKp(6.1);
  964.           PID_M2_SetKi(0.01);
  965.           PID_M2_SetKd(100);
  966.                 }
  967.                 /*---------------------R3_4-----------------------*/
  968.                  else if((MoveTimeCnt>13.0*priod)&&(MoveTimeCnt<14.0*priod))                        
  969.                 {
  970.                         Normalization = (float)(MoveTimeCnt-13.0*priod) / priod;         //對(duì)板球周期歸一化
  971.            if(Normalization>=1.0)
  972.                  {
  973.                   Normalization = 1.0;
  974.                  }
  975.     set_x = 117.0 - (-46.0 * Normalization);//  x = 72 ,y = 28;
  976.                 set_y = 118.0 - (-47.0 * Normalization) ;       
  977.                  
  978.                 PID_M1_SetPoint(set_x);        //x的期望值
  979.           PID_M1_SetKp(6.2);
  980.           PID_M1_SetKi(0.01);
  981.           PID_M1_SetKd(100);
  982.     PID_M2_SetPoint(set_y);        //y的期望值
  983.           PID_M2_SetKp(6.1);
  984.           PID_M2_SetKi(0.01);
  985.           PID_M2_SetKd(100);
  986.                 }
  987.                         /*---------------------R4_1-----------------------*/
  988.    else if((MoveTimeCnt>14.0*priod)&&(MoveTimeCnt<15.0*priod))   
  989.                 {
  990.                         Normalization = (float)(MoveTimeCnt-14.0*priod) / priod;         //對(duì)板球周期歸一化
  991.            if(Normalization>=1.0)
  992.                  {
  993.                   Normalization = 1.0;
  994.                  }
  995.     set_x = 163.0 - (-46.0 * Normalization);//  x = 72 ,y = 28;
  996.                 set_y = 162.0 - (45.0 * Normalization) ;       
  997.                  
  998.                 PID_M1_SetPoint(set_x);        //x的期望值
  999.           PID_M1_SetKp(6.2);
  1000.           PID_M1_SetKi(0.01);
  1001.           PID_M1_SetKd(100);
  1002.     PID_M2_SetPoint(set_y);        //y的期望值
  1003.           PID_M2_SetKp(6.1);
  1004.           PID_M2_SetKi(0.01);
  1005.           PID_M2_SetKd(100);
  1006.                 }
  1007.                 /*---------------------R4_2-----------------------*/
  1008.                   else if((MoveTimeCnt>15.0*priod)&&(MoveTimeCnt<16.0*priod))  
  1009.                 {
  1010.                         Normalization = (float)(MoveTimeCnt-15.0*priod) / priod;         //對(duì)板球周期歸一化
  1011.            if(Normalization>=1.0)
  1012.                  {
  1013.                   Normalization = 1.0;
  1014.                  }
  1015.     set_x = 209.0 - (46.0 * Normalization);//  x = 72 ,y = 28;
  1016.                 set_y = 117.0 - (46.0 * Normalization) ;       
  1017.                  
  1018.                 PID_M1_SetPoint(set_x);        //x的期望值
  1019.           PID_M1_SetKp(6.2);
  1020.           PID_M1_SetKi(0.01);
  1021.           PID_M1_SetKd(100);
  1022.     PID_M2_SetPoint(set_y);        //y的期望值
  1023.           PID_M2_SetKp(6.1);
  1024.           PID_M2_SetKi(0.01);
  1025.           PID_M2_SetKd(100);
  1026.                 }
  1027.                                 /*---------------------回九-----------------------*/
  1028.                   else if((MoveTimeCnt>16.0*priod)&&(MoveTimeCnt<17.0*priod))  
  1029.                 {
  1030.                         Normalization = (float)(MoveTimeCnt-16.0*priod) / priod;         //對(duì)板球周期歸一化
  1031.            if(Normalization>=1.0)
  1032.                  {
  1033.                   Normalization = 1.0;
  1034.                  }
  1035.     set_x = 163.0 - (91.0 * Normalization);//  x = 72 ,y = 28;
  1036.                 set_y = 71.0 - (43.0 * Normalization) ;       
  1037.                  
  1038.                 PID_M1_SetPoint(set_x);        //x的期望值
  1039.           PID_M1_SetKp(6.2);
  1040.           PID_M1_SetKi(0.01);
  1041.           PID_M1_SetKd(100);
  1042.     PID_M2_SetPoint(set_y);        //y的期望值
  1043.           PID_M2_SetKp(6.1);
  1044.           PID_M2_SetKi(0.01);
  1045.           PID_M2_SetKd(100);
  1046.                 }
  1047.                
  1048.         PWM1=PID_M1_PosLocCalc((float)Resured.x);
  1049.                
  1050.         PWM2=PID_M2_PosLocCalc((float)Resured.y);
  1051.        
  1052.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;     
  1053.        
  1054.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  1055.        
  1056.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  1057.        
  1058.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  1060.   kz_hs(PWM1,PWM2);

  1062. }
  1063. void MODE8(void) //發(fā)揮
  1064. {
  1065.        
  1066. //                float  sx_1need = 209.0;
  1067. //          float  sx_2need = 254.0;
  1068. //    float  sx_3need = 209.0;
  1069. //          float  sx_4need = 163.0;               
  1070. //                float  sx_5need = 117.0;
  1071. //          float  sx_6need = 70.0;
  1072. //                float  sx_7need = 118.0;
  1073. //               
  1074. //                float  sy_1need = 71.0 ;
  1075. //          float  sy_2need = 116.0;
  1076. //    float  sy_3need = 161.0;
  1077. //          float  sy_4need = 117.0;               
  1078. //                float  sy_5need = 71.0;
  1079. //          float  sy_6need = 118.0;
  1080. //                float  sy_7need = 162.0;
  1081.           const  float priod = 2000.0;  //周期(毫秒)
  1082.           const  uint32_t x_1 = 219;
  1083.           const  uint32_t y_1 = 260;
  1084.           const  uint32_t need_x = 121;
  1085.           const  uint32_t need_x_erro = 98;
  1086.           const  uint32_t need_y = 158;
  1087.           const  uint32_t need_y_erro = 102;
  1088.           static uint32_t MoveTimeCnt = 0;
  1089.           float  Normalization = 0.0;
  1090.           float  set_x,set_y;
  1091.           float  hc_time = 500;
  1092.        
  1093.     MoveTimeCnt += 5;                                                                       //每5ms運(yùn)算1次
  1094.         /*-----------------A 到 B ----------------------------*/
  1095.        
  1096.        
  1097.         if(MoveTimeCnt<=priod)
  1098.         {       
  1099.                  Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  1100.            if(Normalization>=1.0)
  1101.                  {
  1102.                   Normalization = 1.0;
  1103.                  }
  1104.                 sx_1error = sx_1need - sx_2need;  //209-209
  1105.     sy_1error = sy_1need - sy_2need;         // 182-71
  1106.     set_x = sx_1need - (sx_1error * Normalization);//  x = 254 ,y = 116;
  1107.                 set_y = sy_1need - (sy_1error * Normalization) ;       
  1108.                  
  1109.                 PID_M1_SetPoint(set_x);        //x的期望值
  1110.           PID_M1_SetKp(8.2);
  1111.           PID_M1_SetKi(0.04);
  1112.           PID_M1_SetKd(100);
  1113.     PID_M2_SetPoint(set_y);        //y的期望值
  1114.           PID_M2_SetKp(8.1);
  1115.           PID_M2_SetKi(0.04);
  1116.           PID_M2_SetKd(100);                 
  1117.         }
  1118.         /*-----------------B 到 C ----------------------------*/
  1119.                 else if((MoveTimeCnt > (priod+hc_time))&&(MoveTimeCnt < (2.0 * priod + hc_time)))
  1120.                 {
  1121.                         Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod;         //對(duì)板球周期歸一化
  1122.            if(Normalization>=1.0)
  1123.                  {
  1124.                   Normalization = 1.0;
  1125.                  }
  1126.                  
  1127.                 sx_2error = sx_2need - sx_3need;  //209-209
  1128.     sy_2error = sy_2need - sy_3need;        // 182-71
  1129.     set_x = sx_2need - (sx_2error * Normalization);//  x = 254 ,y = 116;
  1130.                 set_y = sy_2need - (sy_2error * Normalization) ;       
  1131.                  
  1132.                 PID_M1_SetPoint(set_x);        //x的期望值
  1133.           PID_M1_SetKp(8.2);
  1134.           PID_M1_SetKi(0.04);
  1135.           PID_M1_SetKd(100);
  1136.     PID_M2_SetPoint(set_y);        //y的期望值
  1137.           PID_M2_SetKp(8.1);
  1138.           PID_M2_SetKi(0.04);
  1139.           PID_M2_SetKd(100);       
  1140.                 }
  1141.         /*-----------------C 到 D ----------------------------*/
  1142.                         else if((MoveTimeCnt > (2.0* priod+hc_time))&&(MoveTimeCnt < (3.0 * priod + hc_time)))
  1143.                 {
  1144.                         Normalization = (float)(MoveTimeCnt-(2.0 * priod + hc_time)) / priod;         //對(duì)板球周期歸一化
  1145.            if(Normalization>=1.0)
  1146.                  {
  1147.                   Normalization = 1.0;
  1148.                  }
  1149.                  
  1150.                 sx_3error = sx_3need - sx_4need;  //209-209
  1151.     sy_3error = sy_3need - sy_4need;        // 182-71
  1152.     set_x = sx_3need - (sx_3error * Normalization);//  x = 254 ,y = 116;
  1153.                 set_y = sy_3need - (sy_3error * Normalization) ;       
  1154.                  
  1155.                 PID_M1_SetPoint(set_x);        //x的期望值
  1156.           PID_M1_SetKp(8.2);
  1157.           PID_M1_SetKi(0.04);
  1158.           PID_M1_SetKd(100);
  1159.     PID_M2_SetPoint(set_y);        //y的期望值
  1160.           PID_M2_SetKp(8.1);
  1161.           PID_M2_SetKi(0.04);
  1162.           PID_M2_SetKd(100);       
  1163.                 }
  1164.                         else if((MoveTimeCnt > (3.0* priod+hc_time))&&(MoveTimeCnt < (4.0 * priod + hc_time)))
  1165.                 {
  1166.                         Normalization = (float)(MoveTimeCnt-(3.0 * priod + hc_time)) / priod;         //對(duì)板球周期歸一化
  1167.            if(Normalization>=1.0)
  1168.                  {
  1169.                   Normalization = 1.0;
  1170.                  }
  1171.                  
  1172.                 sx_4error = sx_4need - sx_5need;  //209-209
  1173.     sy_4error = sy_4need - sy_5need;        // 182-71
  1174.     set_x = sx_4need - (sx_4error * Normalization);//  x = 254 ,y = 116;
  1175.                 set_y = sy_4need - (sy_4error * Normalization) ;       
  1176.                  
  1177.                 PID_M1_SetPoint(set_x);        //x的期望值
  1178.           PID_M1_SetKp(8.2);
  1179.           PID_M1_SetKi(0.04);
  1180.           PID_M1_SetKd(100);
  1181.     PID_M2_SetPoint(set_y);        //y的期望值
  1182.           PID_M2_SetKp(8.1);
  1183.           PID_M2_SetKi(0.04);
  1184.           PID_M2_SetKd(100);       
  1185.                 }
  1186.                                 else if((MoveTimeCnt > (4.0* priod+hc_time))&&(MoveTimeCnt < (5.0 * priod + hc_time)))
  1187.                 {
  1188.                         Normalization = (float)(MoveTimeCnt-(4.0 * priod + hc_time)) / priod;         //對(duì)板球周期歸一化
  1189.            if(Normalization>=1.0)
  1190.                  {
  1191.                   Normalization = 1.0;
  1192.                  }
  1193.                  
  1194.                 sx_5error = sx_5need - sx_6need;  //209-209
  1195.     sy_5error = sy_5need - sy_6need;        // 182-71
  1196.     set_x = sx_5need - (sx_5error * Normalization);//  x = 254 ,y = 116;
  1197.                 set_y = sy_5need - (sy_5error * Normalization) ;       
  1198.                  
  1199.                 PID_M1_SetPoint(set_x);        //x的期望值
  1200.           PID_M1_SetKp(8.2);
  1201.           PID_M1_SetKi(0.04);
  1202.           PID_M1_SetKd(100);
  1203.     PID_M2_SetPoint(set_y);        //y的期望值
  1204.           PID_M2_SetKp(8.1);
  1205.           PID_M2_SetKi(0.04);
  1206.           PID_M2_SetKd(100);       
  1207.                 }
  1208.                         else if((MoveTimeCnt > (5.0* priod+hc_time))&&(MoveTimeCnt < (6.0 * priod + hc_time)))
  1209.                 {
  1210.                         Normalization = (float)(MoveTimeCnt-(5.0 * priod + hc_time)) / priod;         //對(duì)板球周期歸一化
  1211.            if(Normalization>=1.0)
  1212.                  {
  1213.                   Normalization = 1.0;
  1214.                  }
  1215.                  
  1216.                 sx_6error = sx_6need - sx_7need;  //209-209
  1217.     sy_6error = sy_6need - sy_7need;         // 182-71
  1218.     set_x = sx_6need - (sx_6error * Normalization);//  x = 254 ,y = 116;
  1219.                 set_y = sy_6need - (sy_6error * Normalization) ;       
  1220.                  
  1221.                 PID_M1_SetPoint(set_x);        //x的期望值
  1222.           PID_M1_SetKp(8.2);
  1223.           PID_M1_SetKi(0.04);
  1224.           PID_M1_SetKd(100);
  1225.     PID_M2_SetPoint(set_y);        //y的期望值
  1226.           PID_M2_SetKp(8.1);
  1227.           PID_M2_SetKi(0.04);
  1228.           PID_M2_SetKd(100);       
  1229.                 }
  1230.         PWM1=PID_M1_PosLocCalc((float)Resured.x);
  1231.                
  1232.         PWM2=PID_M2_PosLocCalc((float)Resured.y);
  1233.        
  1234.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;     
  1235.        
  1236.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  1237.        
  1238.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  1239.        
  1240.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  1242.   kz_hs(PWM1,PWM2);
  1243. }


  1246. void MODE9(void) //附加 簡(jiǎn)單任務(wù)六
  1247. {
  1248.                 const  float priod = 2000.0;  //周期(毫秒)
  1249.           const  uint32_t x_1 = 219;
  1250.           const  uint32_t y_1 = 260;
  1251.           const  uint32_t need_x = 121;
  1252.           const  uint32_t need_x_erro = 98;
  1253.           const  uint32_t need_y = 158;
  1254.           const  uint32_t need_y_erro = 102;
  1255.           static uint32_t MoveTimeCnt = 0;
  1256.           float  Normalization = 0.0;
  1257.           float  set_x,set_y;
  1258.           float  hc_time = 100;
  1259.        
  1260.     MoveTimeCnt += 5;       
  1261.        
  1262. if(MoveTimeCnt<=priod)
  1263.         {       
  1264.                  Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  1265.            if(Normalization>=1.0)
  1266.                  {
  1267.                   Normalization = 1.0;
  1268.                  }
  1269.                 x1_error = need_x1 - need_x2;  //209-209
  1270.     y1_error = need_y1 - need_y2;         // 182-71
  1271.     set_x = need_x1 - (x1_error * Normalization);//  x = 254 ,y = 116;
  1272.                 set_y = need_y1 - (y1_error * Normalization) ;       
  1273.                  
  1274.                 PID_M1_SetPoint(set_x);        //x的期望值
  1275.           PID_M1_SetKp(8.2);
  1276.           PID_M1_SetKi(0.04);
  1277.           PID_M1_SetKd(100);
  1278.     PID_M2_SetPoint(set_y);        //y的期望值
  1279.           PID_M2_SetKp(8.1);
  1280.           PID_M2_SetKi(0.04);
  1281.           PID_M2_SetKd(100);                 
  1282.         }
  1283.         /*-----------------B 到 C ----------------------------*/
  1284.                 else if((MoveTimeCnt > (priod+hc_time))&&(MoveTimeCnt < (2.0 * priod + hc_time)))
  1285.                 {
  1286.                         Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod;         //對(duì)板球周期歸一化
  1287.            if(Normalization>=1.0)
  1288.                  {
  1289.                   Normalization = 1.0;
  1290.                  }
  1291.                 x2_error = need_x2 - need_x3; // 209- 118
  1292.     y2_error = need_y2 - need_y3;        // 71 -71
  1293.     set_x = need_x2 - (x2_error * Normalization);//  x = 72 ,y = 28;
  1294.                 set_y = need_y2  -  (y2_error * Normalization) ;       
  1295.                  
  1296.                 PID_M1_SetPoint(set_x);        //x的期望值
  1297.           PID_M1_SetKp(8.2);
  1298.           PID_M1_SetKi(0.04);
  1299.           PID_M1_SetKd(100);
  1300.     PID_M2_SetPoint(set_y);        //y的期望值
  1301.           PID_M2_SetKp(8.1);
  1302.           PID_M2_SetKi(0.04);
  1303.           PID_M2_SetKd(100);       
  1304.                 }
  1305.         /*-----------------C 到 D ----------------------------*/
  1306.                 else if(MoveTimeCnt>( 2.0 * priod + hc_time ))
  1307.                 {
  1308.                         Normalization = (float)(MoveTimeCnt-(2.0 * priod + hc_time)) / priod;         //對(duì)板球周期歸一化
  1309.            if(Normalization>=1.0)
  1310.                  {
  1311.                   Normalization = 1.0;
  1312.                  }
  1313.                 x3_error = need_x3 - need_x4;  //118-118
  1314.     y3_error = need_y3 - need_y4;         //73 - 182
  1315.     set_x = need_x3 - (x3_error * Normalization);//  x = 163 ,y = 25;
  1316.                 set_y = need_y3 - (y3_error * Normalization) ;       
  1317.                  
  1318.                 PID_M1_SetPoint(set_x);        //x的期望值
  1319.           PID_M1_SetKp(8.2);
  1320.           PID_M1_SetKi(0.04);
  1321.           PID_M1_SetKd(100);
  1322.     PID_M2_SetPoint(set_y);        //y的期望值
  1323.           PID_M2_SetKp(8.1);
  1324.           PID_M2_SetKi(0.04);
  1325.           PID_M2_SetKd(100);       
  1326.                 }






  1333.         PWM1=PID_M1_PosLocCalc((float)Resured.x);
  1334.                
  1335.         PWM2=PID_M2_PosLocCalc((float)Resured.y);
  1336.        
  1337.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;      //100
  1338.        
  1339.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  1340.        
  1341.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  1342.        
  1343.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  1345.   kz_hs(PWM1,PWM2);//控制函數(shù)
  1346. }
  1347. void MODE10(void)//無(wú)效
  1348. {
  1349.                 const  float priod = 50000.0;  //周期(毫秒)
  1350.           const  uint32_t x_1 = 219;
  1351.           const  uint32_t y_1 = 260;
  1352.           const  uint32_t need_x = 121;
  1353.           const  uint32_t need_x_erro = 98;
  1354.           const  uint32_t need_y = 158;
  1355.           const  uint32_t need_y_erro = 102;
  1356.           static uint32_t MoveTimeCnt = 0;
  1357.           float  Normalization = 0.0;
  1358.           float A = 26.0;
  1359.           float phase = 0.0;
  1360.           float Omega = 0.0;
  1361.                 float set_x = 0.0;
  1362.           float set_y = 0.0;
  1363.                        
  1364.     MoveTimeCnt += 5;       
  1365.           Normalization = (float)MoveTimeCnt / priod;         //對(duì)板球周期歸一化
  1366.     Omega = 2.0*3.14159*Normalization;                         //對(duì)2π進(jìn)行歸一化處理                               
  1367.          
  1368.                 phase = 3.141592/2.0;                 //逆時(shí)針旋轉(zhuǎn)相位差90°

  1370.           set_x = A*sin(Omega) + 163.0;                         //計(jì)算出X方向當(dāng)前擺角
  1371.           set_y = A*sin(Omega+phase) + 117.0;          //計(jì)算出Y方向當(dāng)前擺角
  1372.                
  1373.                 PID_M1_SetPoint(set_x);        //x的期望值
  1374.           PID_M1_SetKp(8.2);
  1375.           PID_M1_SetKi(0.02);
  1376.           PID_M1_SetKd(100);
  1377.     PID_M2_SetPoint(set_y);        //y的期望值
  1378.           PID_M2_SetKp(8.1);
  1379.           PID_M2_SetKi(0.02);
  1380.           PID_M2_SetKd(100);       
  1381.                
  1382.         PWM1=PID_M1_PosLocCalc((float)Resured.x);
  1383.                
  1384.         PWM2=PID_M2_PosLocCalc((float)Resured.y);
  1385.        
  1386.         if(PWM1 > M1_PWMMAX)       PWM1=M1_PWMMAX;      //100
  1387.        
  1388.         if(PWM1 < -M1_PWMMAX)      PWM1=-M1_PWMMAX;
  1389.        
  1390.         if(PWM2 > M2_PWMMAX)       PWM2=M2_PWMMAX;
  1391.        
  1392.         if(PWM2< -M2_PWMMAX)       PWM2=-M2_PWMMAX;

  1394.   kz_hs(PWM1,PWM2);//控制函數(shù)
  1395. }
  1396. /*--------------全國(guó)電子設(shè)計(jì)競(jìng)賽顯示---------------*/
  1397. void QGDZJS(void)
  1398. {
  1399.         OLED_Init();                                    //初始化OLED  
  1400.         OLED_Showone16x16(0,0,1,0);   //全
  1401.         OLED_Showone16x16(16,0,1,1);  //國(guó)
  1402.   OLED_Showone16x16(32,0,1,2);  //電
  1403.         OLED_Showone16x16(48,0,1,3);  //子
  1404.   OLED_Showone16x16(64,0,1,4);  //設(shè)
  1405.         OLED_Showone16x16(80,0,1,5);  //計(jì)
  1406.   OLED_Showone16x16(96,0,1,6);  //競(jìng)
  1407.         OLED_Showone16x16(112,0,1,7); //賽
  1408.         OLED_ShowChar(0,16,'X',16,1); //顯示一個(gè)字符
  1409.         OLED_ShowChar(0,32,'Y',16,1); //顯示一個(gè)字符
  1410.         OLED_Refresh_Gram();                        //oled顯示函數(shù)
  1411. }
  1412. /*---------------------攝像頭初始化----------------*/
  1413. void show_7670(void)
  1414. {
  1415.         u8 lightmode=0,saturation=2,contrast=2;
  1416.         u8 effect=0;         
  1417.   POINT_COLOR=RED;                        //設(shè)置字體為紅色
  1418.          
  1419.         LCD_ShowString(30,50,200,16,16,"ELITE STM32F103 ^_^");       
  1420.   LCD_ShowString(30,210,200,16,16,"OV7670 Init...");          
  1421.         while(OV7670_Init())//初始化OV7670
  1422.         {
  1423.                 LCD_ShowString(30,210,200,16,16,"OV7670 Error!!");
  1424.                 delay_ms(200);
  1425.           LCD_Fill(30,210,239,246,WHITE);
  1426.                 delay_ms(200);
  1427.         }
  1428.         LCD_ShowString(30,210,200,16,16,"OV7670 Init OK");
  1429.         delay_ms(1500);                   
  1430.         OV7670_Light_Mode(lightmode);
  1431.         OV7670_Color_Saturation(saturation);
  1432.         OV7670_Contrast(contrast);
  1433.         OV7670_Special_Effects(effect);         
  1434.        
  1435.         //TIM6_Int_Init(10000,7199);                        //10Khz計(jì)數(shù)頻率,1秒鐘中斷                                                                          
  1436.         EXTI8_Init();                                                //使能定時(shí)器捕獲
  1437.         OV7670_Window_Set(12,176,240,320);        //設(shè)置窗口          
  1438.   OV7670_CS=0;                       
  1439.         LCD_Clear(BLACK);                                                          

  1441. }
  1442. /*---------------------更新LCD顯示-----------------*/
  1443. void camera_refresh(void)
  1444. {
  1445.   
  1446. //   TARGET_CONDI Conditionred={215,240,20,240,30,160,30,30,320,240};        //紅色1   API參數(shù)  hsl的閾值，識(shí)別時(shí)用的
  1447. //         unsigned char  H_MIN;//目標(biāo)最小色調(diào)
  1448. //   unsigned char  H_MAX;//目標(biāo)最大色調(diào)       
  1449. //   
  1450. //         unsigned char  S_MIN;//目標(biāo)最小飽和度  
  1451. //   unsigned char  S_MAX;//目標(biāo)最大飽和度
  1452. //       
  1453. //         unsigned char  L_MIN;//目標(biāo)最小亮度  
  1454. //   unsigned char  L_MAX;//目標(biāo)最大亮度
  1455. //       
  1456. //         unsigned int  WIDTH_MIN;//目標(biāo)最小寬度
  1457. //         unsigned int  HIGHT_MIN;//目標(biāo)最小高度

  1459. //         unsigned int  WIDTH_MAX;//目標(biāo)最大寬度
  1460. //         unsigned int  HIGHT_MAX;//目標(biāo)最大高度
  1461.         TARGET_CONDI Conditionred={215,240,20,240,30,160,15,15,320,240};        //紅色1   API參數(shù)  hsl的閾值，識(shí)別時(shí)用的
  1462.         u32 j;
  1463.         u16 color;         
  1464.         if(ov_sta)//有幀中斷更新？
  1465.         {
  1466.                 LCD_Scan_Dir(U2D_L2R);                //從上到下,從左到右  
  1467.                 if(lcddev.id==0X1963)LCD_Set_Window((lcddev.width-240)/2,(lcddev.height-320)/2,240,320);//將顯示區(qū)域設(shè)置到屏幕中央
  1468.                 else if(lcddev.id==0X5510||lcddev.id==0X5310)LCD_Set_Window((lcddev.width-320)/2,(lcddev.height-240)/2,320,240);//將顯示區(qū)域設(shè)置到屏幕中央
  1469.                 LCD_WriteRAM_Prepare();     //開(kāi)始寫(xiě)入GRAM       
  1470.                 OV7670_RRST=0;                                //開(kāi)始復(fù)位讀指針
  1471.                 OV7670_RCK_L;
  1472.                 OV7670_RCK_H;
  1473.                 OV7670_RCK_L;
  1474.                 OV7670_RRST=1;                                //復(fù)位讀指針結(jié)束
  1475.                 OV7670_RCK_H;
  1476.                 for(j=0;j<76800;j++)
  1477.                 {
  1478.                         OV7670_RCK_L;
  1479.                         color=GPIOC->IDR&0XFF;        //讀數(shù)據(jù)
  1480.                         OV7670_RCK_H;
  1481.                         color<<=8;  
  1482.                         OV7670_RCK_L;
  1483.                         color|=GPIOC->IDR&0XFF;        //讀數(shù)據(jù)
  1484.                         OV7670_RCK_H;
  1485.                         LCD->LCD_RAM=color;   
  1486.                 }   

  1488.      if(Trace(&Conditionred,&Resured) )                      //API
  1489.                         {                               
  1490.                                 LCD_Fillred(Resured.x-Resured.w/2,Resured.y-Resured.h/2,Resured.x+Resured.w/2,Resured.y-Resured.h/2+1,0xf800);//u16 x,u16 y,u16 width,u16 hight,u16 Color
  1491.                                 LCD_Fillred(Resured.x-Resured.w/2,Resured.y-Resured.h/2,Resured.x-Resured.w/2+1,Resured.y+Resured.h/2,0xf800);
  1492.                                 LCD_Fillred(Resured.x-Resured.w/2,Resured.y+Resured.h/2,Resured.x+Resured.w/2,Resured.y+Resured.h/2+1,0xf800);
  1493.                                 LCD_Fillred(Resured.x+Resured.w/2,Resured.y-Resured.h/2,Resured.x+Resured.w/2+1,Resured.y+Resured.h/2,0xf800);
  1494.                                 LCD_Fillred(Resured.x-2,Resured.y-2,Resured.x+2,Resured.y+2,0xf800);
  1495.        
  1496.                                 r=Resured.x;
  1497.                           y=Resured.y;
  1498.                         }               
  1499.                 //printf("%d  %d\r\n ",r,y);
  1500.                 ov_sta=0;                                        //清零幀中斷標(biāo)記
  1501.                 ov_frame++;
  1502.                 LCD_Scan_Dir(DFT_SCAN_DIR);        //恢復(fù)默認(rèn)掃描方向
  1503.         }
  1504. }          
  1505. /*---------------------主函數(shù)----------------------*/
  1506. int main(void)
  1507. {         
  1508.         u8 msgbuf[15];                                  //消息緩存區(qū)
  1509.         u8 i=0;          
  1510.         u8 tm=0;
  1511.         delay_init();                        //延時(shí)函數(shù)初始化          
  1512.   NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//設(shè)置中斷優(yōu)先級(jí)分組為組2：2位搶占優(yōu)先級(jí)，2位響應(yīng)優(yōu)先級(jí)
  1513.         uart_init(115200);                   //串口初始化為 115200
  1514.   LED_Init();                                            //初始化與LED連接的硬件接口
  1515.         LCD_Init();                                             //初始化LCD  
  1516.   io_in_out_Init();       // io口輸入輸出初始化
  1517.   TIM4_PWM_Init(3599,49); //pwm 20khz
  1518.   show_7670();            //7670和lcd的相關(guān)配置
  1519.   QGDZJS();               //OLED初始化 并且顯示全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽字樣
  1520.         TIM_SetCompare1(TIM4,1235);//  控制前后舵機(jī)初始化角度       
  1521.   TIM_SetCompare4(TIM4,1275);//  控制左右舵機(jī)初始化角度       
  1522.         while(1)
  1523.         {         
  1524.                 /*--------------選擇 、修改 、確定 ...都是通過(guò) 4個(gè) 自鎖開(kāi)關(guān) 和 4個(gè)按鍵來(lái)選擇。------*/
  1525.                          PWMMENUS();  //修改 A B C D 四個(gè)區(qū)域 出發(fā)到結(jié)束的位置
  1526.        wkqwz();     //未啟動(dòng)位置  作用：在還沒(méi)啟動(dòng)任務(wù)的時(shí)候先觀察小球在的位置是否滿足要求
  1527.        MENUS();     //總?cè)蝿?wù)選擇
  1528.                    camera_refresh();//更新顯示
  1529.         }
  1530.         }          
  1531. /*---------------------定時(shí)器二--------------------*/
  1532. void TIM2_IRQHandler(void)   //TIM2中斷  TIM2的初始化在 總?cè)蝿?wù)選擇里
  1533. {               

  1535.         if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //檢查指定的TIM中斷發(fā)生與否:TIM 中斷源
  1536.                 {
  1537.                 TIM_ClearITPendingBit(TIM2, TIM_IT_Update  );  //清除TIMx的中斷待處理位:TIM 中斷源
  1538.                 if(((Resured.x>5)&&(Resured.x<315))&&((Resured.y>5)&&(Resured.y<235)))       
  1539.                 {
  1540.      switch(RWXZ)
  1541.                  {
  1542.                          case 1 : MODE1(); break;
  1543.                          case 2 : MODE2(); break;
  1544.                          case 3 : MODE3(); break;
  1545.                          case 4 : MODE4(); break;
  1546.                          case 5 : MODE5(); break;
  1547.                          case 6 : MODE6(); break;
  1548.                          case 7 : MODE7(); break;
  1549.                          case 8 : MODE8(); break;               
  1550.        case 9 : MODE9(); break;       
  1551.        case 10 : MODE10(); break;                                 
  1552.                  }
  1553.          }
  1554.                 }
  1555. }
復(fù)制代碼

所有資料51hei提供下載:
F1紅色小球識(shí)別的板球控制-ov7670.7z (276.36 KB, 下載次數(shù): 83)


分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏4 分享淘帖 頂頂1 踩踩
回復(fù)

使用道具 舉報(bào)

沙發(fā)
ID:1 發(fā)表于 2019-1-25 18:36 | 只看該作者
本帖需要重新編輯補(bǔ)全電路原理圖,源碼,詳細(xì)說(shuō)明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)
回復(fù)

使用道具 舉報(bào)

板凳
ID:138247 發(fā)表于 2019-5-2 15:25 | 只看該作者

謝謝樓主分享。。
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表
主站蜘蛛池模板： 美女黄网 | 91精品国产美女在线观看 | www.毛片| 久久精品免费看 | 成人网址在线观看 | 久久精品亚洲国产 | 一区在线视频 | 日韩av在线免费 | 久久国产精品网站 | 欧美日韩黄 | 古典武侠第一页久久777 | 免费在线观看黄视频 | 亚洲国产精品久久久久久 | 涩涩视频在线观看免费 | 午夜精品久久 | 欧美一级黄色片免费观看 | 国产成年人视频 | 在线亚洲电影 | 亚洲高清网 | 丝袜一区二区三区 | 丝袜毛片 | 亚洲美女天堂网 | 黄网站免费在线 | 国产精品精品视频一区二区三区 | 欧美黄色片 | 欧美成人a∨高清免费观看 色999日韩 | 亚洲欧美中文日韩在线v日本 | 美国av毛片 | 久久久久久国产精品免费免费 | 欧美日本一区二区 | 国产精品欧美一区二区三区不卡 | 久久国产精品色av免费观看 | 中文字幕精品一区二区三区精品 | 一区二区三区视频在线 | 久久久久久久亚洲精品 | 久久精品毛片 | 欧美日韩高清在线一区 | 精品久久视频 | 久色一区| 亚洲精品无人区 | 国产精品一区在线观看 |