I'm using an Arduino Uno line-tracking car, with five line sensors. My driver module is L298N. My car can track normally at low speeds(like 50~60). But when I turn the speed up(like 80~100)， it can't turn properly and just overshot the track. I think it's because my car isn't responding quickly. How can I solve this problem? (Please forgive my poor English)
Here are my codes:
// 电机设置 //
#define leftA_PIN 8
#define leftB_PIN 9
#define righA_PIN 12
#define righB_PIN 13
#define left_pwm 10 //左侧电机使能端控制
#define righ_pwm 11 //右侧电机使能端控制
int index; //积分分离参数
double alpha = 0.95; //(采样时间)/(采样时间+滤波器系数)
double Kp = 25, Ki = 0.1, Kd = 10; //pid弯道参数
double error = 0, P = 0, I = 0, D = 0, PID_value = 0; //pid直道参数
double previous_error = 0, previous_pid, previous_d; //误差值
double previous_left, previoue_right; //车轮之前的速度
int sensor[5] = { 0, 0, 0, 0, 0 }; //5个传感器数值的数组
static int initial_motor_speed = 80; //初始速度
void read_sensor_values(void); //读取初值
void calc_pid(void); //计算pid
void motor_control(void); //电机控制
void motor_pinint(); //引脚初始化
// 循迹模块设置 //
//传感器从左往右依次对应的引脚
#define leftA_track_PIN 3
#define leftB_track_PIN 4
#define middle_track_PIN 5
#define righA_track_PIN 6
#define righB_track_PIN 7
void setup() {
Serial.begin(9600);
track_pinint(); //循迹引脚初始化
motor_pinint(); //电机引脚初始化
}
void loop() {
read_sensor_values(); //循迹小车
calc_pid(); //计算PID值
motor_control(); //控制电机
}
/*循迹模块引脚初始化*/
void track_pinint() {
pinMode(leftA_track_PIN, INPUT); //设置引脚为输入引脚
pinMode(leftB_track_PIN, INPUT);
pinMode(middle_track_PIN, INPUT);
pinMode(righA_track_PIN, INPUT);
pinMode(righB_track_PIN, INPUT);
pinMode(2, OUTPUT); //设置2号引脚为传感器供电引脚
digitalWrite(2, HIGH);
// digitalWrite(leftA_track_PIN, HIGH); //初始化传感器的状态
// digitalWrite(leftB_track_PIN, HIGH);
// digitalWrite(middle_track_PIN, HIGH);
// digitalWrite(righA_track_PIN, HIGH);
// digitalWrite(righB_track_PIN, HIGH);
}
/*电机引脚初始化*/
void motor_pinint() {
pinMode(leftA_PIN, OUTPUT); //设置引脚为输出引脚
pinMode(leftB_PIN, OUTPUT);
pinMode(righA_PIN, OUTPUT);
pinMode(righB_PIN, OUTPUT);
pinMode(left_pwm, OUTPUT);
pinMode(righ_pwm, OUTPUT);
}
//速度设定范围(-255,255)
void motorsWrite(double speedL, double speedR) {
if (speedR > 0) {
digitalWrite(leftA_PIN, HIGH);
digitalWrite(leftB_PIN, LOW);
analogWrite(left_pwm, speedR);
} else {
digitalWrite(leftA_PIN, LOW);
digitalWrite(leftB_PIN, HIGH);
analogWrite(left_pwm, -speedR);
}
if (speedL > 0) {
digitalWrite(righA_PIN, HIGH);
digitalWrite(righB_PIN, LOW);
analogWrite(righ_pwm, speedL);
} else {
digitalWrite(righA_PIN, LOW);
digitalWrite(righB_PIN, HIGH);
analogWrite(righ_pwm, -speedL);
}
}
// //速度设定范围(-100，100)
// void motorsWritePct(double speedLpct, double speedRpct) {
// //speedLpct, speedRpct ranges from -100 to 100
// motorsWrite(speedLpct * 2.55, speedRpct * 2.55);
// }
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(righA_track_PIN);
sensor[4] = digitalRead(righB_track_PIN);
if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
error = 0;
} else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
error = 0;
} else if ((previous_error == 10) && (sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 1)) {
error = 7;
} else if ((previous_error == -10) && (sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
error = -7;
} else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
error = 10;
} else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
error = 4;
} else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 1)) {
error = 0.15;
} else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
error = 0.1;
} else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 1)) {
error = 0;
} else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 1)) {
error = -0.1;
} else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
error = -0.15;
} else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
error = -4;
} else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
error = -10;
}
}
void calc_pid() {
P = error;
if (abs(error) > 1.1) { //积分分离
index = 0;
} else {
index = 1;
I = I + error;
}
D = Kd * (1 - alpha) * (error - previous_error) + alpha * previous_d;
PID_value = (Kp * P) + (index * Ki * I / 2) + D;
if (error == 0) {
PID_value = 0;
}
previous_error = error;
previous_d = D;
previous_pid = PID_value;
delay(50);
if (abs(previous_error) > 5) {
delay(100);
if (abs(previous_error) > 8) {
delay(200);
}
}
}
void motor_control() {
double left_motor_speed = initial_motor_speed - PID_value;
double right_motor_speed = initial_motor_speed + PID_value;
if (abs(previous_error) > 1.1 && error == 0.15) {
if (right_motor_speed > previous_left) {
right_motor_speed = previous_left;
}
if (left_motor_speed > previoue_right) {
left_motor_speed = previoue_right;
}
}
if (left_motor_speed < -255) {
left_motor_speed = -255;
}
if (left_motor_speed > 255) {
left_motor_speed = 255;
}
if (right_motor_speed < -255) {
right_motor_speed = -255;
}
if (right_motor_speed > 255) {
right_motor_speed = 255;
}
previous_left = right_motor_speed;
previoue_right = left_motor_speed;
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();
}
thanks advance