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

QQ登錄

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

搜索
查看: 10743|回復(fù): 9
收起左側(cè)

Arduino智能小車(chē)程序+資料 pid算法

  [復(fù)制鏈接]
ID:559809 發(fā)表于 2019-7-11 15:06 | 顯示全部樓層 |閱讀模式
  1. #include "pid.h"
  2. #ifdef ARDUINO_DEBUG
  3. int debugLeftSpeed;
  4. int debugRightSpeed;
  5. uint8_t debugIrs = 0;
  6. #endif
  7. const float motorSpeed = 140; //
  8. const int IR_PIN[] = {A0, A1, A2, A3, A4}; //
  9. const int IN_A1 = 13;   //
  10. const int IN_A2 = 12;   //
  11. const int IN_B1 = 11;  //
  12. const int IN_B2 = 10;  //
  13. const int _pwmLeftPin = 5;
  14. const int _pwmRightPin = 6;
  15. pid_t pid;
  16. float pidValue = 0;
  17. bool turnFlag = false;
  18. void setup(void)
  19. {
  20.   int i;
  21.   /********************1.初始化電機(jī)模塊引腳13,12,11,10為輸出模式******************/
  22.   /*
  23.    *   編寫(xiě)代碼
  24.   */
  25.     /********************2.設(shè)置小車(chē)啟動(dòng)速度為140---set_speed()******************/
  26.   /*
  27.    *   編寫(xiě)代碼
  28.   */
  29.   
  30.   /********************3.初始化循跡模塊引腳A0, A1, A2, A3, A4為輸入模式******************/
  31.    /*
  32.    *   編寫(xiě)代碼
  33.   */
  34.   //3.設(shè)置PID參數(shù)
  35.   pid.sampleTime = SAMPLE_TIME;
  36.   pid.Kp = KP_VALUE;
  37.   pid.Ki = KI_VALUE;
  38.   pid.Kd = KD_VALUE;
  39.   pid.error = 0;
  40.   pid.previous_error = 0;
  41.   Serial.begin(115200);
  42.   delay(5000);
  43.   goForward();
  44.   return;
  45. }

  47. //get ir data and middle filter
  48. uint8_t getIrData(void)  //
  49. {
  50.   int i, j;
  51.   uint8_t level;
  52.   uint8_t temp;
  53.   uint8_t irs[9] = {0};//采集9次傳感器的傳感器的數(shù)據(jù)
  54.         //減少取平均值，濾波。
  55.   for (j = 0; j < 9; j ++) {
  56.    
  57.       for (i = 0; i < 5; i++) {
  58.         level = digitalRead(IR_PIN[i]);
  59.         if (level) {
  60.           bitSet(irs[j], i); // 設(shè)置irs[j]的第jbit 為1
  61.         } else {
  62.           bitClear(irs[j], i); //清除irs[j]的第jbit為0
  63.         }
  64.       }
  65.   }
  66.   //對(duì)irs中的數(shù)據(jù)，進(jìn)行9次排序，取最中間的值。
  67.   for (i = 0; i < 9 - 1; i ++) {
  68.     for (j = 0; j < 9 - i - 1; j ++) {
  69.       if (irs[j] > irs[j + 1]) {
  70.         temp = irs[j];
  71.         irs[j] = irs[j + 1];
  72.         irs[j + 1] = temp;
  73.       }
  74.     }
  75.   }
  76.   
  77. #ifdef ARDUINO_DEBUG
  78.   debugIrs = irs[4];
  79. #endif
  80.   return irs[4];
  81. }
  82. int calcErrorByIrsValue(uint8_t irs)
  83. {
  84.   //右邊壓線(xiàn)為負(fù)，左邊壓線(xiàn)為正。
  85.   //要左轉(zhuǎn)為負(fù)，要右轉(zhuǎn)為正
  86.   int curError = pid.error; //獲得上一次的pid誤差值
  87.   //壓線(xiàn)輸出低電平0，沒(méi)有壓線(xiàn)輸出高電平1
  88.   switch (irs) { //h'h
  89.     case B11110: curError = -8; break; //最右邊壓線(xiàn)
  90.    
  91.     case B10000:
  92.     case B11000: curError = -7; break; //右四個(gè)或者右3個(gè)沿線(xiàn)
  93.    
  94.     case B11100: curError = -6; break; //右兩個(gè)沿線(xiàn)
  95.     case B11101: curError = -4; break; //右倒數(shù)第二個(gè)沿線(xiàn)
  96.     case B11001: curError = -2; break; //
  97.    
  98.     case B00000:       //全部壓線(xiàn)和中間壓線(xiàn)，對(duì)應(yīng)"十字"彎道，直接郭即可。
  99.     case B11011: curError = 0;  break;
  100.    
  101.     case B10011: curError = 2;  break;
  102.     case B10111: curError = 4;  break;
  103.     case B00111: curError = 6;  break;
  104.    
  105.     case B00011:
  106.     case B00001: curError = 7;  break;
  107.    
  108.     case B01111: curError = 8;  break;
  109. //都沒(méi)有沿線(xiàn)，按照上次的PID值來(lái)計(jì)算
  110.     case B11111: curError = pid.error > 0 ? 9 : - 9; break;
  111.   }
  112.   return curError;
  113. }

  115. void _sortData(int *p, int n)
  116. {
  117.   int temp;
  118.   int i, j;
  119.   
  120.   /**編寫(xiě)冒泡排序，對(duì)采集到的錯(cuò)誤碼，進(jìn)行十次從小到大排序 **/
  121.   /*
  122.    *   編寫(xiě)代碼
  123.   */
  124.   return;
  125. }

  127. //計(jì)算當(dāng)前的誤差值，得到當(dāng)前的偏移值
  128. void calcCurrentError(void)
  129. {
  130.   int i;
  131.   uint8_t irs;
  132.   float sum = 0;
  133.   int errorData[10];
  134.   //1.采集10次循跡模塊獲得的錯(cuò)誤碼
  135.   for (i = 0; i < 10; i ++) {
  136.     irs =  getIrData();  //得到這次傳感器采集的數(shù)據(jù)（5路數(shù)據(jù)）
  137.     errorData[i] =  calcErrorByIrsValue(irs);
  138.   }
  139.   //2.要求對(duì)errorData中的數(shù)據(jù)進(jìn)行排序（從小到大排序）
  140.     /*
  141.    *   編寫(xiě)代碼
  142.   */
  143.   //3.十次中去處一個(gè)最高數(shù)，去除一個(gè)最低數(shù)，求中間8次錯(cuò)誤碼數(shù)據(jù)的和。
  144.   /*
  145.    *   編寫(xiě)代碼
  146.   */
  147. //4.求8次錯(cuò)誤碼數(shù)據(jù)中的偏差。
  148.   pid.error = sum / 8;
  149.   return;
  150. }
  151. void turnRight(void)
  152. {
  153.     digitalWrite(IN_B1, HIGH); //左正
  154.     digitalWrite(IN_B2, LOW);
  155.     digitalWrite(IN_A1, LOW); //右反
  156.     digitalWrite(IN_A2, HIGH);
  157.    
  158. }
  159. void turnLeft(void)
  160. {
  161.     digitalWrite(IN_B1, LOW);// 左反
  162.     digitalWrite(IN_B2, HIGH);
  163.     digitalWrite(IN_A1, HIGH); //右正
  164.     digitalWrite(IN_A2, LOW);
  165.    
  166. }
  167. void goForward(void)
  168. {
  169.   digitalWrite(IN_B1, HIGH); //左正傳
  170.   digitalWrite(IN_B2, LOW);
  171.   digitalWrite(IN_A1, HIGH); //右正轉(zhuǎn)
  172.   digitalWrite(IN_A2, LOW);
  173. }
  174. void motorControl(float pidValue, bool turnFlag)
  175. {
  176.   int leftMotorSpeed = 0;
  177.   int rightMotorSpeed = 0;
  178.   
  179.   leftMotorSpeed  = constrain((motorSpeed + pidValue), -255, 255);
  180.   rightMotorSpeed = constrain((motorSpeed - pidValue), -255, 255);
  181.   if (turnFlag) {
  182.     if (abs(leftMotorSpeed) > abs(rightMotorSpeed)) {
  183.       leftMotorSpeed  = abs(leftMotorSpeed);
  184.       rightMotorSpeed = leftMotorSpeed;
  185.     } else {
  186.       rightMotorSpeed =  abs(rightMotorSpeed);
  187.       leftMotorSpeed  = rightMotorSpeed;
  188.     }
  189.   } else {
  190.     leftMotorSpeed  = leftMotorSpeed  > 0 ? leftMotorSpeed  : -leftMotorSpeed;
  191.     rightMotorSpeed = rightMotorSpeed > 0 ? rightMotorSpeed : -rightMotorSpeed;
  192.   }
  193.   analogWrite(_pwmLeftPin,  leftMotorSpeed );
  194.   analogWrite(_pwmRightPin, rightMotorSpeed);
  195. #ifdef ARDUINO_DEBUG
  196.   debugLeftSpeed  = leftMotorSpeed ;
  197.   debugRightSpeed = rightMotorSpeed;
  198. #endif
  199.   return;
  200. }
  201. bool calculatePid(float *pValue)
  202. {
  203.   float P = 0;
  204.   static float I = 0 ;
  205.   float D = 0 ;
  206.   static unsigned long lastTime = 0;
  207.   //獲得機(jī)器從啟動(dòng)到現(xiàn)在所運(yùn)行的時(shí)間
  208.   unsigned long now = millis();
  209.   int timeChange = now - lastTime;//得到這次的時(shí)間
  210.   //若是我們的這次變化的時(shí)間，小于pid的
  211.   //采樣時(shí)間，就需要等待一下。
  212.   if (timeChange < pid.sampleTime) {
  213.     return false;
  214.   }
  215.   P = pid.error; //本次小車(chē)傳感器的偏移大小
  216.   I = I + pid.error;//歷史的偏移
  217.   D = pid.error - pid.previous_error; //最近兩次的差值
  218.   *pValue = (pid.Kp * P) + (pid.Ki * I) + (pid.Kd * D) + 1;
  219.   //constrain()函數(shù)的功能：若是*pvalue > motorSpeed,取 motorSpeed，
  220.   //                       若是*pvalue < -motorSpeed,取 -motorSpeed,
  221.   //        若是-pValue<=*pvalue <= motorSpeed,取*pvalue
  222.   *pValue = constrain(*pValue, -motorSpeed,motorSpeed);
  223.   pid.previous_error = pid.error; //記錄本次的pid.error，作為下一次的歷史差值。
  224.   lastTime = now; // 記錄本次的運(yùn)行時(shí)間，作為下一次時(shí)間的歷史時(shí)間參考。
  225.   return true;
  226. }

  228. void calcDirection(void) //判斷小車(chē)的轉(zhuǎn)向
  229. {
  230.   //pid.error > 0 說(shuō)明小車(chē)左邊壓線(xiàn)多，要
  231.   if (pid.error >= 7 && pid.error <= 8) {
  232.     turnRight();
  233.     turnFlag = true;
  234.   } else if (pid.error >= -8 && pid.error <= -7) {
  235.     turnLeft();
  236.     turnFlag = true;
  237.   } else if(pid.error == 9 || pid.error == -9){
  238.     turnRight();
  239.     turnFlag = true;
  240.     }else {
  241.     goForward();
  242.     turnFlag = false;
  243.   }
  244.   return;
  245. }
  246. void loop(void)
  247. {
  248.   bool ok;
  249.   float  pidValue;
  250.   calcCurrentError();
  251.   ok = calculatePid(&pidValue); //計(jì)算當(dāng)前pid的差值。，若是就緒了
  252.   if (ok) {
  253.     calcDirection();
  254.     motorControl(pidValue, turnFlag);
  255.   }
  256.   return;
  257. }
復(fù)制代碼

pid_student.zip

3.1 KB, 下載次數(shù): 148, 下載積分: 黑幣 -5

Arduino 智能小車(chē) - 尋跡.pdf

322.06 KB, 下載次數(shù): 149, 下載積分: 黑幣 -5

回復(fù)

使用道具 舉報(bào)

ID:560595 發(fā)表于 2019-7-16 13:46 | 顯示全部樓層
這個(gè)要頂
回復(fù)

使用道具 舉報(bào)

ID:505027 發(fā)表于 2019-7-16 21:26 | 顯示全部樓層
這個(gè)好，記錄一下
回復(fù)

使用道具 舉報(bào)

ID:594846 發(fā)表于 2019-8-4 22:31 | 顯示全部樓層
真好，贊一個(gè)！
回復(fù)

使用道具 舉報(bào)

ID:609660 發(fā)表于 2019-9-9 19:22 來(lái)自手機(jī) | 顯示全部樓層
這個(gè)資源確實(shí)好
回復(fù)

使用道具 舉報(bào)

ID:149774 發(fā)表于 2019-9-21 21:44 | 顯示全部樓層
做個(gè)記號(hào)，回頭學(xué)習(xí)一下
回復(fù)

使用道具 舉報(bào)

ID:614623 發(fā)表于 2019-9-22 00:16 | 顯示全部樓層
這個(gè)可以參考
回復(fù)

使用道具 舉報(bào)

ID:652804 發(fā)表于 2019-12-15 15:42 | 顯示全部樓層
謝謝，學(xué)習(xí)學(xué)習(xí)。
回復(fù)

使用道具 舉報(bào)

ID:28942 發(fā)表于 2020-9-15 19:24 | 顯示全部樓層
編輯不過(guò)是什么原因？
回復(fù)

使用道具 舉報(bào)

ID:1011808 發(fā)表于 2022-4-5 13:47 | 顯示全部樓層
arduino如何設(shè)置小車(chē)啟動(dòng)速度
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

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

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

快速回復(fù) 返回頂部 返回列表