Arduino超聲波自平衡小車

ID:666753 · 發表于 2020-4-4 17:10

有需要的可以下載

//利用超聲波測距傳感器實現小車平衡
char val='z'; //調節與控制命令字
double Kp=0.01, Kd=0.02; //PID系數
unsigned int PB_ad=0, KP_ad=0, KD_ad=0, i; //平衡點調整，PID各調整設定系數
unsigned int Len_Echo = 0; //回波時間
int E_0 = 0, E_1 = 0, PWM_Out; //誤差，PWM輸出
double Len_filter = 0, Len_0 = 100; //測距濾波，機械平衡距離
unsigned int TrigPin = 4; //HC-SR04觸發信號
unsigned int EchoPin = 5; //HC-SR04回波檢測
unsigned int M_IN1 = 6; // L298-IN1
unsigned int M_IN2 = 7; // L298-IN2
unsigned int M_IN3 = 8; // L298-IN3
unsigned int M_IN4 = 9; // L298-IN4
unsigned int M_ENA = 10; // L298-ENA
unsigned int M_ENB = 11; // L298-ENB
//初始化
void setup() {
Serial.begin(115200);
pinMode(M_ENA, OUTPUT); //電機控制
pinMode(M_IN1, OUTPUT);
pinMode(M_IN2, OUTPUT);
pinMode(M_ENB, OUTPUT);
pinMode(M_IN3, OUTPUT);
pinMode(M_IN4, OUTPUT);
pinMode(EchoPin, INPUT); //超聲波測距
pinMode(TrigPin, OUTPUT);
digitalWrite(TrigPin, LOW);
for(i=0; i<64; i++) {
PB_ad += analogRead(A0); //讀取平衡點電位器設定值
KP_ad += analogRead(A1); //讀取PID-P電位器設定值
KD_ad += analogRead(A2); //讀取PID-D電位器設定值
}
Kp *= KP_ad/64; //Kp = 3.23;
Kd *= KD_ad/64; //Kd = 8.14;
Len_0 += PB_ad/640; //Len_0 = 189;
i = 0;
}
//主循環程序
void loop() {
if (Serial.available() > 0) {
val = Serial.read();
if(val == 'F') {
Serial.print(Len_0); Serial.print("\t"); Serial.print(Kp); Serial.print("\t"); Serial.println(Kd);//查看設置參數
}
if(val == 'L') {
Serial.print(Len_Echo); Serial.print("\t"); Serial.println(Len_filter); //查看傳感器實測值,濾波值
}
}
digitalWrite(TrigPin, HIGH); //發送超聲波測量觸發脈沖
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
Len_Echo = pulseIn(EchoPin, HIGH); //回波時間測量
if((Len_Echo < 300) && (Len_Echo > 70)) {
Len_filter *= 0.7; //一階互補濾波
Len_filter += 0.3*Len_Echo;
i ++;
if(i > 50){
E_0 = Len_0 - Len_filter;
PWM_Out = Kp * E_0 + Kd * (E_0 - E_1);
E_1 = E_0;
i = 100;
SetMotorVoltage(PWM_Out);
}
}else {
SetMotorVoltage(0); //超出平衡范圍，停止PWM輸出
}
delay(5); //延時5毫秒，保證超聲測距可靠工作
}
//電機輸出
void SetMotorVoltage(int MotorVol) {
if(MotorVol >=0) {
digitalWrite(M_IN1, LOW);
digitalWrite(M_IN2, HIGH);
digitalWrite(M_IN3, LOW);
digitalWrite(M_IN4, HIGH);
}else {
digitalWrite(M_IN1, HIGH);
digitalWrite(M_IN2, LOW);
digitalWrite(M_IN3, HIGH);
digitalWrite(M_IN4, LOW);
MotorVol = -MotorVol;
}
if(MotorVol > 255) MotorVol = 255; //防止PWM值超過255
analogWrite(M_ENA, MotorVol);
analogWrite(M_ENB, MotorVol);
}

復制代碼

ID:1 · 發表于 2020-4-7 15:58

本帖需要重新編輯補全電路原理圖,源碼,詳細說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)

帳號		自動登錄	找回密碼
密碼			立即注冊

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

Arduino超聲波自平衡小車