這一期爲創客們帶來基於PID算法的循跡小車製作
1.標準賽道示意圖：
（該賽道包含了：左直角、右直角、十字路口等路況）
2.灰度傳感器安裝示意圖：
（可根據實際情況調節各傳感器之間的間距）
3.硬件原理圖：
4.控制邏輯：
5.程序如下：
#define leftA_PIN 7
#define leftB_PIN 6
#define left_Pwm_PIN 5
#define STBY 8
#define rightA_PIN 9
#define rightB_PIN 10
#define right_Pwm_PIN 11
#define leftA_track_PIN 2
#define leftB_track_PIN 3
#define middle_track_PIN 4
#define rightA_track_PIN 12
#define rightB_track_PIN 13
float Kp = 10, Ki = 0.5, Kd = 0; //pid彎道參數參數
float error = 0, P = 0, I = 0, D = 0, PID_value = 0; //pid直道參數
float decide = 0; //元素判斷
float previous_error = 0, previous_I = 0; //誤差值
int sensor[5] = {0, 0, 0, 0, 0}; //5個傳感器數值的數組
static int initial_motor_speed = 60; //初始速度
void read_sensor_values(void); //讀取初值
void calc_pid(void); //計算pid
void motor_control(void); //電機控制
void motor_pinint( ); //引腳初始化
void _stop(); //停車
void setup()
{
Serial.begin(9600); //串口波特率115200（PC端使用）
track_pinint(); //循跡引腳初始化
motor_pinint(); //電機引腳初始化
}
void loop()
{
read_sensor_values(); //循跡小車
calc_pid();
motor_control();
}
/*循跡模塊引腳初始化*/
void track_pinint()
{
pinMode (leftA_track_PIN, INPUT); //設置引腳爲輸入引腳
pinMode (leftB_track_PIN, INPUT); //設置引腳爲輸入引腳
pinMode (middle_track_PIN, INPUT);//設置引腳爲輸入引腳
pinMode (rightA_track_PIN, INPUT); //設置引腳爲輸入引腳
pinMode (rightB_track_PIN, INPUT); //設置引腳爲輸入引腳
}
/*電機引腳初始化*/
void motor_pinint()
{
pinMode (leftA_PIN, OUTPUT); //設置引腳爲輸出引腳
pinMode (leftB_PIN, OUTPUT); //設置引腳爲輸出引腳
pinMode (left_Pwm_PIN, OUTPUT); //設置引腳爲輸出引腳
pinMode (STBY, OUTPUT); //設置引腳爲輸出引腳
pinMode (rightA_PIN, OUTPUT); //設置引腳爲輸出引腳
pinMode (rightB_PIN, OUTPUT); //設置引腳爲輸出引腳
pinMode (right_Pwm_PIN, OUTPUT); //設置引腳爲輸出引腳
}
//停車函數
void _stop()
{
digitalWrite(leftA_PIN, LOW);
digitalWrite(leftB_PIN, LOW);
analogWrite(left_Pwm_PIN,255); //左輪靜止不動
digitalWrite(rightA_PIN, LOW);
digitalWrite(rightB_PIN, LOW);
analogWrite(right_Pwm_PIN,255); //右輪靜止不動
}
//速度設定範圍(-255,255)
void motorsWrite(int speedL, int speedR)
{
digitalWrite(STBY, HIGH);
if (speedR > 0)
{
digitalWrite(rightA_PIN, HIGH);
digitalWrite(rightB_PIN, LOW);
analogWrite(right_Pwm_PIN,speedR);
}
else
{
digitalWrite(rightA_PIN, LOW);
digitalWrite(rightB_PIN, HIGH);
analogWrite(right_Pwm_PIN,-speedR);
}
if (speedL > 0)
{
digitalWrite(leftA_PIN, HIGH);
digitalWrite(leftB_PIN, LOW);
analogWrite(left_Pwm_PIN,speedL);
}
else
{
digitalWrite(leftA_PIN, LOW);
digitalWrite(leftB_PIN, HIGH);
analogWrite(left_Pwm_PIN,-speedL);
}
}
//速度設定範圍(-100，100)
void motorsWritePct(int speedLpct, int speedRpct)
{
//speedLpct, speedRpct ranges from -100 to 100
motorsWrite(speedLpct * 2.55, speedRpct * 2.55);
}
void motorsStop()
{
digitalWrite(leftA_PIN, LOW);
digitalWrite(leftB_PIN, LOW);
analogWrite(left_Pwm_PIN,255); //左輪靜止不動
digitalWrite(rightA_PIN, LOW);
digitalWrite(rightB_PIN, LOW);
analogWrite(right_Pwm_PIN,255); //右輪靜止不動
}
void read_sensor_values()
{
sensor[0] = digitalRead(leftA_track_PIN);
sensor[1] = digitalRead(leftB_track_PIN);
sensor[2] = digitalRead(middle_track_PIN);
sensor[3] = digitalRead(rightA_track_PIN);
sensor[4] = digitalRead(rightB_track_PIN);
if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
{
decide = 1;//十字路口 1 1 1 1 1 直行
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
{
decide = 1;//十字路口 0 1 1 1 0 直行
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
{
decide = 2;//90度路口 0 0 1 1 1 右轉90度
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
{
decide = 2;//90度路口 0 0 1 1 0 右轉90度
}
else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
decide = 3;//90度路口 1 1 1 0 0 左轉90度
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
decide = 3;//90度路口 0 1 1 0 0 左轉90度
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
decide = 3;//向上銳角 0 1 1 0 0 向上銳角
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1))
{
error = 2;// 0 0 0 0 1
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0))
{
error = 1;// 0 0 0 1 0
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
error = 0;// 0 0 1 0 0
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
{
error = -1;// 0 1 0 0 0
}
else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
{
error = -2;// 1 0 0 0 0
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
{
if (error == -2)
{// 0 0 0 0 0
error = -3;
}
else
{
error = 3;
}
}
}
void calc_pid()
{
P = error;
I = I + error;
D = error - previous_error;
PID_value = (Kp * P) + (Ki * I) + (Kd * D);
previous_error = error;
}
void motor_control()
{
int left_motor_speed = initial_motor_speed + PID_value;
int right_motor_speed = initial_motor_speed + PID_value;
if(left_motor_speed < -255)
{
left_motor_speed = -255;
}
if(left_motor_speed > 255)
{
left_motor_speed = 255;
}
motorsWrite(left_motor_speed,right_motor_speed);
Serial.print("move_A: ");
Serial.print(left_motor_speed, OCT);
Serial.print(" move_B: ");
Serial.print(right_motor_speed, OCT);
Serial.print(" error: ");
Serial.print(error, OCT);
Serial.print(" P: ");
Serial.print(Kp, OCT);
Serial.print(" PID_value: ");
Serial.print(PID_value, OCT);
Serial.println();
}
傳送門：https://blog.csdn.net/qq_38351824/article/details/81275262
詳情請關注億航創客官方抖音賬號：EhangGroup