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

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 6085|回復: 1
打印 上一主題 下一主題
收起左側

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

[復制鏈接]
跳轉到指定樓層
樓主
全場地位系統：小車靜止或移動過程中碼盤進行全場定位,用的是avr單片機.


接線說明：
//左編碼器
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

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

使用道具 舉報

沙發
ID:323568 發表于 2018-5-7 17:19 | 只看該作者
你好，我想問一下就是兩個編碼器輪子固定的位置是左右平行180度那種嗎
回復

使用道具 舉報

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

本版積分規則

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

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 久久精品国产一区二区电影 | 一区二区在线 | 免费亚洲成人 | 成人亚洲精品久久久久软件 | 精品国产欧美一区二区 | 麻豆精品一区二区三区在线观看 | 亚洲精品区 | 精品国产一区探花在线观看 | h在线 | 亚洲一区欧美一区 | 91精品国产91久久久久久丝袜 | 国产在线麻豆精品入口 | 国产成人精品一区二区三区视频 | 日韩国产欧美视频 | 四虎影院新地址 | 久久亚洲一区二区三区四区 | 国产高清免费视频 | 国产日韩欧美精品一区二区三区 | 欧美一级全黄 | 国产午夜三级一区二区三 | 激情久久网 | 中文字幕不卡在线观看 | 色在线免费视频 | 欧美日韩a | 久久亚洲综合 | 国产精品高清在线 | 午夜在线| 日韩av中文| 色噜噜色综合 | 国产一区二区三区www | 亚洲一区视频 | 亚洲一区中文字幕在线观看 | 亚洲国产中文在线 | 欧美日韩精品中文字幕 | 日韩欧美电影在线 | 国产一区二区在线免费播放 | 欧美5区 | wwwxxx国产 | 欧美亚洲视频在线观看 | 成人久久视频 | 91麻豆精品国产91久久久久久久久 |