# 基于Arduino与PID算法 循迹小车的感受

//  本代码使用pid动态算法，“//后的部分是不用pid”，

const int IN_A1=3;
const int IN_A2=5;
const int IN_B1=6;
const int IN_B2=9;
const int sensor1=A1;
const int sensor2=A2;
const int sensor3=A3;
const int sensor4=A4;
const int sensor5=A5;
float Kp = 20, Ki = 0.01, Kd = 0.1;
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;
float previous_error = 0;
int DIN_A1 = 50;
int DIN_B1 = 50;
void setup() {
pinMode(IN_A1,OUTPUT);
pinMode(IN_A2,OUTPUT);
pinMode(IN_B1,OUTPUT);
pinMode(IN_B2,OUTPUT);
pinMode(sensor1,INPUT);
pinMode(sensor2,INPUT);
pinMode(sensor3,INPUT);
pinMode(sensor4,INPUT);
pinMode(sensor5,INPUT);
}
void loop() {
char num2,num3,num5,num1,num4;
num1=digitalRead(sensor1);
num2=digitalRead(sensor2);
num3=digitalRead(sensor3);
num4=digitalRead(sensor4);
num5=digitalRead(sensor5);
if(num1==0&&num2!=0&&num3!=0&&num4!=0&&num5!=0){
if(error<0){
error=3.2;}
else{error=1.6;}
//analogWrite(IN_A1,90);
//analogWrite(IN_B1,30);
}else if(num2==0&&num1!=0&&num3!=0&&num4!=0&&num5!=0){
if(error<0){
error=2;}
else{error=1;}
//analogWrite(IN_A1,80);
//analogWrite(IN_B1,40);
}else if(num4==0&&num2!=0&&num3!=0&&num1!=0&&num5!=0){
if(error>0){
error=-2;}
else{error=-1;}
//analogWrite(IN_A1,40);
//analogWrite(IN_B1,80);
}else if(num5==0&&num4!=0&&num3!=0&&num2!=0&&num1!=0){
if(error>0){
error=-3.2;}
else{error=-1.6;}
//analogWrite(IN_A1,30);
//analogWrite(IN_B1,90);
}else{
error=0;
//analogWrite(IN_A1,70);
//analogWrite(IN_B1,70);
}
P = error;
I = I + error;
D = error - previous_error;
PID_value = (Kp * P) + (Ki * I) + (Kd * D);
previous_error = error;
DIN_A1  += PID_value;
DIN_B1  -= PID_value;
if(DIN_A1>60){
DIN_A1=60;}
if(DIN_B1>60){
DIN_B1=60;}
if(DIN_B1<30){
DIN_B1=30;}
if(DIN_A1<30){
DIN_A1=30;}
analogWrite(IN_A1,DIN_A1);
analogWrite(IN_B1,DIN_B1);
}


09-27
08-11 5万+
01-19 904
06-06
05-14
12-08 2574
11-17 7592
10-15 1721
05-03 3863
07-09
11-04
©️2020 CSDN 皮肤主题: 深蓝海洋 设计师:CSDN官方博客