Bộ điều khiển PID - ứng dụng phần 2 - xe dò line dùng thuật toán PID

Tiép nối bài viết về xe dò line cảm ơn Đỗ Hữu Toàn đã viết hộ mình phần 4. hôm nay mình sẽ làm cho chiếc xe dò line đi mượt và có hồn hơn 

Các bạn chuẩn bị phần cứng như phần 4 có điều phải nối lại dây theo code sau mình không muốn nhắc đến phần cứng 

Xây dựng chương trình và thuật toán

Giá trị sensor Vị trí của xe
0 0 1 0 0 chính giữa
1 0 0 0 0 bên phải line
0 0 0 0 1   bên trái line

Như thế này nếu số 1 là giá trị khi nhận ra line thì số 1 càng dịch sang trái hoặc sang phải thì ta có 10 mức lệch line trong cả trường hợp có hai cảm biến cùng phát hiện ra line, khi đó chúng ta sẽ áp dụng PID để tính toán để đưa ra giá trị điều kiển PWM đến mạch cầu H

các giá trị khi lệch line

Giá trị sensor Giá trị tương ứng
0 0 0 0 1 4
0 0 0 1 1 3
0 0 0 1 0 2
0 0 1 1 0 1
0 0 1 0 0 0
0 1 1 0 0 -1
0 1 0 0 0 -2
1 1 0 0 0 -3
1 0 0 0 0 -4
0 0 0 0 0 -5

Lập trình để nhận và chuyển đổi giá trị

void read_sensor_values()
{
  sensor[0]=digitalRead(A0);
  sensor[1]=digitalRead(A1);
  sensor[2]=digitalRead(A2);
  sensor[3]=digitalRead(A3);
  sensor[4]=digitalRead(A4);
  
  if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==1))
  error=4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==1)&&(sensor[4]==1))
  error=3;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==1)&&(sensor[4]==0))
  error=2;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[4]==1)&&(sensor[4]==0))
  error=1;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[4]==0)&&(sensor[4]==0))
  error=0;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-1;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-2;
  else if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-3;
  else if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
    if(error==-4) error=-5;
    else error=5;

}

Bây giờ là phần quan trọng áp dụng công thức PID ta có code để tính toán đưa ra giá trị điều kiển PWM ( mình tham khảo trên internet nha )

PID_value chính là biến để lưu giá trị điều kiển động cơ
void calculate_pid()
{
    P = error;
    I = I + error;
    D = error – previous_error;
    
    PID_value = (Kp*P) + (Ki*I) + (Kd*D);
    
    previous_error=error;
}
Khi đã có PID_value lúc này chúng ta biết xe đang lệch line như thế nào và cần giá trị nào để phù hợp nhất điều kiển xe đi đúng line 

Motor control

void motor_control()
{
    // Calculating the effective motor speed:
    int left_motor_speed = initial_motor_speed-PID_value;
    int right_motor_speed = initial_motor_speed+PID_value;
    
    // The motor speed should not exceed the max PWM value
    constrain(left_motor_speed,0,255);
    constrain(right_motor_speed,0,255);
    
    analogWrite(9,left_motor_speed);   //Left Motor Speed
    analogWrite(10,right_motor_speed);  //Right Motor Speed
    //following lines of code are to make the bot move forward
    /*The pin numbers and high, low values might be different
    depending on your connections */
    digitalWrite(4,HIGH);
    digitalWrite(5,LOW);
    digitalWrite(6,LOW);
    digitalWrite(7,HIGH);
}

Bây giời đến void loop

void loop()
{
    read_sensor_values();
    calculate_pid();
    motor_control();
}

và đây là hoạt động của xe

Những hình ảnh về dự án: 
Bài viết truyền cảm hứng: 
lên
34 thành viên đã đánh giá bài viết này hữu ích.
Các dự án được truyền cảm hứng

Cách dùng Module L298N để điều khiển 4 động cơ DC

Trên cộng đồng đã có nhiều bài viết về động cơ, nhưng chưa có bài viết nào nói về cách để điều khiển cùng 1 lúc 4 động cơ mà chỉ dùng 1 module L298N. Nên hôm nay mình sẽ chia sẻ về cách để điều khiển cùng lúc 4 động cơ trên 1 module L298N.

lên
25 thành viên đã đánh giá bài viết này hữu ích.
Các bài viết cùng tác giả

Giới thiệu mô-đun Voice Recognition Module V3 ( điều kiển giọng nói với ARDUINO )

Bài viết sẽ gới thiệu với các bạn về một mô-đun nhận giạng giọng nói. Với mô-đun này các bạn có thể tạo ra nhiều sản phẩm rất ứng dụng với mình, mình sẽ ứng dụng nó cho người khuyết tật như điều khiển thiết bị bằng giọng nói trong thời gian thực.

lên
33 thành viên đã đánh giá bài viết này hữu ích.
Từ khóa: 

Làm xe dò line với module tự làm. Phần 1 - Tại sao không ?

Bài viết sẽ hướng dẫn các bận tất tật cách để tạo ra một chiếc xe dò line " đi theo vạch " đơn giản chi phí thấp. Với module tự làm " do mình không có điều kiện để sắm module nên toàn tự làm để sài và lấy kinh nghiệm luôn" hì không có $ để làm dự án khủng hay đẹp cho các bạn thông cảm nha

lên
34 thành viên đã đánh giá bài viết này hữu ích.