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

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 6153|回復: 1
收起左側

單片機全場定位系統程序 小車靜止或移動過程中碼盤進行全場定位

[復制鏈接]
ID:102668 發表于 2016-1-10 23:04 | 顯示全部樓層 |閱讀模式
全場地位系統：小車靜止或移動過程中碼盤進行全場定位,用的是avr單片機.
00000.png

接線說明：
//左編碼器
PD3   INT1
PB0   PCINT0

//右編碼器
PD2   INT0
PC3   PCINT11


  1. /***********************************************************
  2. *   函數庫說明：差分定位數值計算函數庫                     *
  3. *   版本：      v1.00beta                                  *
  4. *   作者：      王卓然                                     *
  5. *   創建日期：  2008年3月29日                              *
  6. * -------------------------------------------------------- *
  7. *  [支 持 庫]                                              *
  8. *   支持庫名稱：                                           *
  9. *   需要版本：                                             *
  10. *   支持庫說明：                                           *
  11. * -------------------------------------------------------- *
  12. *  [版本更新]                                              *
  13. *   修改：                                                 *
  14. *   修改日期：                                             *
  15. *   版本：                                                 *
  16. * -------------------------------------------------------- *
  17. *  [版本歷史]                                              *
  18. * -------------------------------------------------------- *
  19. *  [使用說明]                                              *
  20. ***********************************************************/

  22. /********************
  23. * 頭 文 件 配 置 區 *
  24. ********************/
  25. # include "RD_MacroAndConst.h"
  26. # include "DL_Config.h"
  27. # include "RD_UseDLocate_LIB.h"
  28. # include "PF_Config.h"
  29. # include "Math.h"

  31. /********************
  32. *   系 統 宏 定 義  *
  33. ********************/

  35. /*------------------*
  36. *   常 數 宏 定 義  *
  37. *------------------*/

  39. //輪子直徑
  40. #ifndef WHEEL_D_L
  41.     #error No define WHEEL_D_L!
  42. #endif
  43. #ifndef WHEEL_D_R
  44.     #error No define WHEEL_D_R!
  45. #endif

  47. //輪間距
  48. #ifndef D_BTW_WHEEL
  49.     #error No define D_BTW_WHEEL
  50. #endif

  52. //每圈線精度
  53. #ifndef N_L
  54.     #error No define N_L
  55. #endif
  56. #ifndef N_R
  57.     #error No define N_R
  58. #endif

  60. //碼盤倍頻數
  61. #ifndef MULTIPLE
  62.     #error No define MULTIPLE
  63. #endif

  65. //計數器 到 弧長
  66. # define K_L    ((PI * (WHEEL_D_L)) / ((MULTIPLE) * (N_L)))
  67. # define K_R    ((PI * (WHEEL_D_R)) / ((MULTIPLE) * (N_R)))




  72. /*------------------*
  73. *   動 作 宏 定 義  *
  74. *------------------*/

  76. /********************
  77. *  自定義類型聲明區 *
  78. ********************/

  80. /********************
  81. *  模塊結構體定義區 *
  82. ********************/

  84. /********************
  85. *   函 數 聲 明 區  *
  86. ********************/
  87. float Get_Absolute_Angle(void);
  88. float fMod(float fNumberA,float fNumberB);
  89. BOOL PROC_Difference_Locate(void);

  91. /********************
  92. *   模塊函數聲明區  *
  93. ********************/

  95. /********************
  96. *   模塊變量聲明區  *
  97. ********************/

  99. /********************
  100. *   全局變量聲明區  *
  101. ********************/
  102. float g_fLastAngle = 0.0;
  103. float g_fX = 0.0;
  104. float g_fY = 0.0;

  106. /********************
  107. *  全局變量聲引用區 *
  108. ********************/
  109. extern BOOL g_bRunFlagB;

  111. /***********************************************************
  112. *   函數說明：獲取當前絕對角度函數                         *
  113. *   輸入：    無                                           *
  114. *   輸出：    角度值                                       *
  115. *   調用函數：無                                           *
  116. ***********************************************************/
  117. float Get_Absolute_Angle(void)
  118. {
  119.     return
  120.         (
  121.             (float)g_lCounterR * (float)K_R
  122.              - (float)g_lCounterL * (float)K_L
  123.          ) * (1.0 / (float)D_BTW_WHEEL);
  124. }

  126. /***********************************************************
  127. *   函數說明：獲取當前相對角度函數                         *
  128. *   輸入：    左編碼器微分量，右編碼器微分量               *
  129. *   輸出：    角度值（弧度）                               *
  130. *   調用函數：無                                           *
  131. ***********************************************************/
  132. float Get_Relative_Angle(INT32 lDCL,INT32 lDCR)
  133. {
  134.     if (lDCL == lDCR)
  135.     {
  136.         return 0.0;
  137.     }
  138.    
  139.     return
  140.         (
  141.             (float)lDCR * (float)K_R
  142.              - (float)lDCL * (float)K_L
  143.          ) * (1.0 / (float)D_BTW_WHEEL);
  144. }



  148. /***********************************************************
  149. *   函數說明：差分定位計算函數                             *
  150. *   輸入：    無                                           *
  151. *   輸出：    FALSE                                        *
  152. *   調用函數：無                                           *
  153. ***********************************************************/
  154. BOOL PROC_Difference_Locate(void)
  155. {
  156.     static INT32 s_lLastCounterL = 0;
  157.     static INT32 s_lLastCounterR = 0;
  158.    
  159.     if ((g_lCounterLImage - s_lLastCounterL)
  160.          == (g_lCounterRImage - s_lLastCounterR))
  161.     {
  162.         INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
  163.         float fR = ((float)nDeltaCounterL * (float)K_L);
  164.         
  165.         g_fX += fR * cos(g_fLastAngle);
  166.         g_fY += fR * sin(g_fLastAngle);
  167.     }
  168.     else
  169.     {
  170.         float fDeltaAngle,fAbsoluteAngle;
  171.         //計算角度微元
  172.         {
  173.             float fTempAngle = Get_Relative_Angle
  174.                             (
  175.                                 g_lCounterLImage,
  176.                                 g_lCounterRImage
  177.                             );
  178.             fDeltaAngle = (fTempAngle - g_fLastAngle);
  179.             fAbsoluteAngle = fDeltaAngle * 0.5 + g_fLastAngle;
  180.    
  181.             g_fLastAngle = fTempAngle;
  182.         }
  183.         //計算位置微元
  184.         {
  185.             INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
  186.             float fR = (((float)nDeltaCounterL * (float)K_L) * (1.0 / fDeltaAngle)
  187.                 + (float)D_BTW_WHEEL * 0.5);

  189.             fR = 2.0 * fR * sin(fDeltaAngle * 0.5);             //fR->l
  190.         
  191.             g_fX += fR * cos(fAbsoluteAngle);
  192.             g_fY += fR * sin(fAbsoluteAngle);
  193.         }
  194.     }
  195.    
  196.     //數據更新
  197.     s_lLastCounterL = g_lCounterLImage;
  198.     s_lLastCounterR = g_lCounterRImage;
  199.     g_bRunFlagB = TRUE;
  200.    
  201.     return FALSE;
  202. }

  204. /***********************************************************
  205. *   函數說明：實數求模函數                                 *
  206. *   輸入：    被除數，除數                                 *
  207. *   輸出：    取模結果                                     *
  208. *   調用函數：無                                           *
  209. ***********************************************************/
  210. float fMod(float fNumberA,float fNumberB)
  211. {
  212.     if (fNumberB < 0)
  213.     {
  214.         if (fNumberA < 0)
  215.         {
  216.             while(fNumberA < fNumberB)
  217.             {
  218.                 fNumberA -= fNumberB;
  219.             }
  220.         
  221.             return fNumberA;
  222.         }
  223.         else if (fNumberA > 0)
  224.         {
  225.             while((fNumberA + fNumberB) > fNumberB)
  226.             {
  227.                 fNumberA += fNumberB;
  228.             }
  229.             
  230.             return fNumberA;
  231.         }
  232.         else
  233.         {
  234.             return 0.0;
  235.         }
  236.     }
  237.     else if (fNumberB > 0)
  238.     {
  239.         if (fNumberA < 0)
  240.         {
  241.             while((fNumberA + fNumberB) < fNumberB)
  242.             {
  243.                 fNumberA += fNumberB;
  244.             }
  245.             
  246.             return fNumberA;
  247.         }
  248.         else if (fNumberA > 0)
  249.         {
  250.             while(fNumberA > fNumberB)
  251.             {
  252.                 fNumberA -= fNumberB;
  253.             }
  254.             
  255.             return fNumberA;
  256.         }
  257.         else
  258.         {
  259.             return 0.0;
  260.         }
  261.     }
  262.     else
  263.     {
  264.         return 0.0;
  265.     }
  266. }
復制代碼



全場定位系統.rar

113.25 KB, 下載次數: 39, 下載積分: 黑幣 -5

回復

使用道具 舉報

ID:323568 發表于 2018-5-7 17:19 | 顯示全部樓層
你好，我想問一下就是兩個編碼器輪子固定的位置是左右平行180度那種嗎
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 91av在线视频观看 | 曰韩一二三区 | 久久久久久99 | 欧美日韩国产精品一区二区 | 国产一级片免费在线观看 | 久久精品视频12 | av网站在线播放 | 日韩成人免费在线视频 | 成人网av| 欧美精品乱码99久久影院 | 亚洲国产在 | 亚洲一区不卡在线 | 日韩一区在线播放 | 午夜免费看视频 | av乱码| 日韩欧美二区 | 国产精品久久久久久久久久东京 | 中文字幕亚洲一区二区三区 | 国产一级特黄视频 | 黄a网| 精品国产18久久久久久二百 | 羞羞色视频 | 亚洲一区二区久久久 | 亚洲日韩中文字幕一区 | 日产久久 | 亚洲高清在线 | 欧美精品一 | 日韩在线 | 欧美成人综合 | 日本人爽p大片免费看 | 欧美三区| 国产69精品久久久久777 | 2019精品手机国产品在线 | 日韩a级片| 欧美一区二区三区国产精品 | 亚洲成人一区二区 | 涩涩视频在线观看 | 国产超碰人人爽人人做人人爱 | 日韩精品在线播放 | 国产乱码高清区二区三区在线 | 成人在线精品视频 |