找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

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

Arduino超聲波自平衡小車

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:666753 發(fā)表于 2020-4-4 17:10 | 只看該作者 回帖獎勵 |倒序?yàn)g覽 |閱讀模式
有需要的可以下載
  1. //利用超聲波測距傳感器實(shí)現(xiàn)小車平衡
  2. char val='z'; //調(diào)節(jié)與控制命令字
  3. double Kp=0.01, Kd=0.02; //PID系數(shù)
  4. unsigned int PB_ad=0, KP_ad=0, KD_ad=0, i; //平衡點(diǎn)調(diào)整，PID各調(diào)整設(shè)定系數(shù)
  5. unsigned int Len_Echo = 0; //回波時間
  6. int E_0 = 0, E_1 = 0, PWM_Out; //誤差，PWM輸出
  7. double Len_filter = 0, Len_0 = 100; //測距濾波，機(jī)械平衡距離
  8. unsigned int TrigPin = 4; //HC-SR04觸發(fā)信號
  9. unsigned int EchoPin = 5; //HC-SR04回波檢測
  10. unsigned int M_IN1 = 6; // L298-IN1
  11. unsigned int M_IN2 = 7; // L298-IN2
  12. unsigned int M_IN3 = 8; // L298-IN3
  13. unsigned int M_IN4 = 9; // L298-IN4
  14. unsigned int M_ENA = 10; // L298-ENA
  15. unsigned int M_ENB = 11; // L298-ENB
  16. //初始化
  17. void setup() {
  18.   Serial.begin(115200);
  19.   pinMode(M_ENA, OUTPUT); //電機(jī)控制
  20.   pinMode(M_IN1, OUTPUT);
  21.   pinMode(M_IN2, OUTPUT);
  22.   pinMode(M_ENB, OUTPUT);
  23.   pinMode(M_IN3, OUTPUT);
  24.   pinMode(M_IN4, OUTPUT);   
  25.   pinMode(EchoPin, INPUT); //超聲波測距
  26.   pinMode(TrigPin, OUTPUT);
  27.   digitalWrite(TrigPin, LOW);
  28.   for(i=0; i<64; i++) {
  29.     PB_ad += analogRead(A0); //讀取平衡點(diǎn)電位器設(shè)定值
  30.     KP_ad += analogRead(A1); //讀取PID-P電位器設(shè)定值
  31.     KD_ad += analogRead(A2); //讀取PID-D電位器設(shè)定值
  32.   }
  33.   Kp *= KP_ad/64; //Kp = 3.23;
  34.   Kd *= KD_ad/64; //Kd = 8.14;
  35.   Len_0 += PB_ad/640; //Len_0 = 189;
  36.   i = 0;
  37. }
  38. //主循環(huán)程序
  39. void loop() {
  40.   if (Serial.available() > 0) {
  41.     val = Serial.read();
  42.     if(val == 'F') {
  43.       Serial.print(Len_0); Serial.print("\t"); Serial.print(Kp); Serial.print("\t"); Serial.println(Kd);//查看設(shè)置參數(shù)
  44.     }
  45.     if(val == 'L') {
  46.       Serial.print(Len_Echo); Serial.print("\t"); Serial.println(Len_filter); //查看傳感器實(shí)測值,濾波值
  47.     }
  48.   }
  49.   digitalWrite(TrigPin, HIGH); //發(fā)送超聲波測量觸發(fā)脈沖
  50.   delayMicroseconds(10);
  51.   digitalWrite(TrigPin, LOW);
  52.   Len_Echo = pulseIn(EchoPin, HIGH); //回波時間測量
  53.   if((Len_Echo < 300) && (Len_Echo > 70)) {
  54.     Len_filter *= 0.7; //一階互補(bǔ)濾波
  55.     Len_filter += 0.3*Len_Echo;
  56.     i ++;
  57.     if(i > 50){
  58.       E_0 = Len_0 - Len_filter;
  59.       PWM_Out = Kp * E_0 + Kd * (E_0 - E_1);
  60.       E_1 = E_0;
  61.       i = 100;
  62.       SetMotorVoltage(PWM_Out);
  63.     }   
  64.   }else {
  65.     SetMotorVoltage(0); //超出平衡范圍，停止PWM輸出
  66.   }
  67.   delay(5); //延時5毫秒，保證超聲測距可靠工作
  68. }
  69. //電機(jī)輸出
  70. void SetMotorVoltage(int MotorVol) {
  71.   if(MotorVol >=0) {
  72.     digitalWrite(M_IN1, LOW);
  73.     digitalWrite(M_IN2, HIGH);
  74.     digitalWrite(M_IN3, LOW);
  75.     digitalWrite(M_IN4, HIGH);
  76.   }else {
  77.     digitalWrite(M_IN1, HIGH);
  78.     digitalWrite(M_IN2, LOW);
  79.     digitalWrite(M_IN3, HIGH);
  80.     digitalWrite(M_IN4, LOW);
  81.     MotorVol = -MotorVol;
  82.   }
  83.   if(MotorVol > 255) MotorVol = 255; //防止PWM值超過255
  84.   analogWrite(M_ENA, MotorVol);
  85.   analogWrite(M_ENB, MotorVol);
  86. }

復(fù)制代碼

超聲自平衡小車程序.zip

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

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

使用道具 舉報

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

使用道具 舉報

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

本版積分規(guī)則

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

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

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