|
|
該小車(chē)采用Arduino UNO主控制核心，通過(guò)傳感器傳來(lái)的信號(hào)，對(duì)當(dāng)前環(huán)境作出判斷，最后對(duì)電機(jī)做出相應(yīng)的動(dòng)作。單片機(jī)通過(guò)紅外傳感器檢測(cè)場(chǎng)地黑線(xiàn)，從而控制電機(jī)驅(qū)動(dòng)模塊，改變電機(jī)轉(zhuǎn)速來(lái)控制小車(chē)方向，從而達(dá)到循跡的目的
整個(gè)系統(tǒng)包括Arduino UNO主控板、電機(jī)驅(qū)動(dòng)模塊L298n、循跡傳感器TCR5000L、鋰電池（建議是可充放電的）和車(chē)體。
循跡代碼：
float Kp , Ki = 0, Kd ; //pid彎道參數(shù)參數(shù)
float error = 0, P = 0, I = 0, D = 0, PID_value = 0; //pid直道參數(shù)
float previous_error = 0, previous_I = 0; //誤差值
static int initial_motor_speed = 70; //此處值為0-255的值，受電池電壓影響，需要自己調(diào)試
int A[5] = {19, 18, 17, 16, 15};
double L, R;
float a, b, c, d, p, m;
int ena = 5;
int enb = 10;
void setup()
{
Serial.begin(9600);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(9, OUTPUT);
pinMode(ena, OUTPUT);
pinMode(enb, OUTPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(A4, INPUT);
pinMode(A5, INPUT);
}
void loop()
{
Kp =3.25;
Kd =30; //此處的Kp和Kd也需使用者根據(jù)車(chē)的結(jié)構(gòu)和既定軌跡調(diào)試進(jìn)行調(diào)試，
m = 0;
a = 3, b = 6, c = 11, d = 18, p = 1;
trac_run();
}
void trac()
{
unsigned char temp = 0b00000; //臨時(shí)變量用于新一輪采集
for (int i = 0; i < 5; i++)
temp |= digitalRead(A[ i]) << i; //輪詢(xún)5個(gè)傳感器輸出，并將查詢(xún)結(jié)果轉(zhuǎn)換為編碼形式
switch (temp)
{
case 0b01111: error = -d; break;
case 0b00111: error = -c; break;
case 0b10111: error = -b; break;
case 0b10011: error = -a, m = 10; break;
case 0b11011: error = 0, m = 20; break;
case 0b11001: error = a, m = 10; break;
case 0b11101: error = b; break;
case 0b11100: error = c; break;
case 0b11110: error = d; break;
default: p = 1; break;
}
}
void pid()
{
P = error;
I = I + error;
D = error - previous_error;
PID_value = (Kp * P) + (Ki * I) + (Kd * D);
previous_error = error;
}
void trac_run()
{
trac();
pid();
L = p * ( initial_motor_speed + PID_value + m);
R = p * ( initial_motor_speed - PID_value + m);
if (abs(L) > 255) L = L / abs(L) * 255;
if (abs(R) > 255) R = R / abs(R) * 255;
if (abs(L) == 0) L = 1;
if (abs(R) == 0) R = 1;
motor();
}
void motor()
{
if (L > 0 && R > 0)
analogWrite(enb, L),
analogWrite(ena, R),
motordir(0, 1, 1, 0);
else if (- L > 0 && - R > 0)
analogWrite(enb, -L),
analogWrite(ena, -R),
motordir(1, 0, 0, 1);
else if (- L > 0 && R > 0)
analogWrite(enb, -L),
analogWrite(ena, R),
motordir(0, 1, 0, 1);
else if (L > 0 && -R > 0)
analogWrite(enb, L),
analogWrite(ena, -R),
motordir(1, 0, 1, 0);
else
motordir(0, 0, 0, 0);
}
void motordir(int out1, int out2, int out3, int out4) //輸入1 0，控制方向
{
digitalWrite(6, out1);
digitalWrite(7, out2);
digitalWrite(8, out3);
digitalWrite(9, out4);
}
|
-
圖片1.png
(493.12 KB, 下載次數(shù): 0)
下載附件
2025-7-3 10:25 上傳
整體圖
評(píng)分
-
查看全部評(píng)分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
