Lập trình robot #2 : Lập trình robot dò line và kết hợp với điều khiển MTE

Robot dò line không phải là công nghệ gì mới mẻ cả nhưng nó là công nghệ được sử dụng rất nhiều trong cả các cuộc thi robocon lẫn ngoài thực tế. Giờ tuy công nghệ rất phát triển với la bàn số, cảm biến laze… nhưng công nghệ dò line vẫn có những ứng dụng nhất định chưa thể thay thế, thực tế là robot tự hành vận chuyển hàng hóa AGV vẫn sử dụng cách dò line này . Vì vậy ,ngày hôm nay mình sẽ hướng dẫn các bạn cách kết nối và lập trình xe dò line 1 cách đơn giản nhất. Nếu nhiều bạn quan tâm mình sẽ làm thêm phần PID động cơ nữa để robot chạy được mượt mà hơn

Chuẩn bị

Phần cứng
Arduino Uno	x1
Arduino motor shield L298	x1
PS2 Wireless Controller	x1
Mạch dò line L021	x1
Phần mềm
Arduino IDE
Thư viện
PS2X

Trước tiên chúng ta cùng nhau tìm hiểu về mạch dò line số MTE-L021 nhé!

Lập trình robot #2 : Lập trình robot dò line và kết hợp với điều khiển bằng tay PS2

Thông số kĩ thuật

Điện áp hoạt động	: 5 VDC
Vi xử lí tích hợp	: STM8S003F3P6
Số mắt line số	: 5 mắt
Tín hiệu trả về	: Digital
Kích thước	: 107x27x190mm

Nguyên lí hoạt động của phương pháp dò line bằng quang trở

Lập trình robot #2 : Lập trình robot dò line và kết hợp với điều khiển bằng tay PS2

MTE-L021 có khả năng học mẫu sân với nhiều màu sân khác nhau, khả năng chống nhiễu tốt do đã được tích hợp 1 chip STM8S003F3P6 để xử lý lấy mẫu sân và chống nhiễu. Đồng thời sản phẩm còn được đi kèm với một phụ kiện in3D để chống tác động tiêu cực từ bên ngoài.

Các bạn đấu nối mạch dò line với arduino theo đúng thứ tự này nhé, để lát chỉ việc nạp code vào là chạy thôi

À còn cách kết nối động với với kết nối tay PS2 thì các bạn xem lại bài viết trước : Xem tại đây

Lập trình robot #2 : Lập trình robot dò line và kết hợp với điều khiển bằng tay PS2

Code dò line

//***************************************************************//
//  Author       : RedFox                                        //
//  Website      : lamchucongnghe.com                            //
//  Fanpage      : fb.com//lamchucongnghevn                      //
//  Shop sendo   : sendo.vn/shop/lamchucongnghe_com              //
//  Shop shopee  : shopee.vn/lamchucongnghe                      //
//***************************************************************//
 
#define dongCoTrai    13      // Đảo chiều động cơ trái ( 0: chạy thuận, 1: chạy ngược)
#define dongCoPhai    12      // Đảo chiều động cơ phải ( 1: chạy thuận, 0: chạy ngược)
#define bamXungTrai   11      // Băm xung điều chỉnh tốc độ động cơ trái
#define bamXungPhai   10      // Băm xung điều chỉnh tốc độ động cơ phải
 
int trangThai;
int lech;                     // xác định xem robot đang bị lệch sang trái hay lệch sang phải
 
void setup() {
  Serial.begin(9600);
  pinMode(dongCoTrai, OUTPUT);
  pinMode(dongCoPhai, OUTPUT);
}
 
void loop() {
  trangThai = docDoLine(A0, A1, A2, A3, A4);     // Vị trí sensor tính từ trái sang phải,  Phát hiện line, trả về 0, Không có line, trả về 1
  //Serial.println(trangThai, HEX);
  switch(trangThai) {
    case 0x01:  dkDongCo(100, 20);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00001
    case 0x03:  dkDongCo(100, 40);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00011
    case 0x02:  dkDongCo(100, 55);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00010
    case 0x06:  dkDongCo(100, 75);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00110
    case 0x07:  dkDongCo(120, 110);Serial.println(trangThai, HEX);break;            // 0b000 00111
            
    case 0x04:  dkDongCo(110, 110);lech = 0;Serial.println(trangThai, HEX); break;  // 0b000 00100
         
    case 0x0C:  dkDongCo(110, 120);Serial.println(trangThai, HEX);break;            // 0b000 01100
    case 0x08:  dkDongCo(75, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 01000
    case 0x18:  dkDongCo(55, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 11000
    case 0x10:  dkDongCo(40, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 10000
    case 0x1C:  dkDongCo(20, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 11100
    default : {
                if(lech == 1)      {dkDongCo(100,  -30);Serial.println(trangThai, HEX);}
                else if(lech == 2) {dkDongCo(-30,  100);Serial.println(trangThai, HEX);}
      }
  }
}
 
void dkDongCo(float xungTrai, float xungPhai) {
  if(xungTrai >=0 ){
    digitalWrite(dongCoTrai, HIGH);
    analogWrite(bamXungTrai, xungTrai);
  }
  else{
    digitalWrite(dongCoTrai, LOW);
    analogWrite(bamXungTrai, -xungTrai);   // vì xung mang giá trị âm nên cần chuyển sang giá trị dương
  }
  if(xungPhai >=0 ){
    digitalWrite(dongCoPhai, HIGH);
    analogWrite(bamXungPhai, xungPhai);
  }
  else{
    digitalWrite(dongCoPhai, LOW);
    analogWrite(bamXungPhai, -xungPhai);   // vì xung mang giá trị âm nên cần chuyển sang giá trị dương
  }
}
 
// đọc tín hiệu từ 5 cảm biến dùng phương pháp dịch bit
uint8_t docDoLine(int sensor1, int sensor2, int sensor3, int sensor4, int sensor5) {
  int val;
  val = digitalRead(sensor1);
  val = val<<1 | digitalRead(sensor2);
  val = val<<1 | digitalRead(sensor3);
  val = val<<1 | digitalRead(sensor4);
  val = val<<1 | digitalRead(sensor5);
  return val;
}

Code robot dò line kết hợp với tay PS2

//***************************************************************//
//  Author       : RedFox                                        //
//  Website      : lamchucongnghe.com                            //
//  Fanpage      : fb.com//lamchucongnghevn                      //
//  Shop sendo   : sendo.vn/shop/lamchucongnghe_com              //
//  Shop shopee  : shopee.vn/lamchucongnghe                      //
//***************************************************************//
 
#include <PS2X_lib.h>
 
#define PS2_DAT       6       // Data Pin
#define PS2_CMD       7       // Command Pin
#define PS2_CS        8       // Attention Pin
#define PS2_CLK       9       // Clock Pin
 
#define dongCoTrai    13      // Đảo chiều động cơ trái ( 0: chạy thuận, 1: chạy ngược)
#define dongCoPhai    12      // Đảo chiều động cơ phải ( 1: chạy thuận, 0: chạy ngược)
#define bamXungTrai   11      // Băm xung điều chỉnh tốc độ động cơ trái
#define bamXungPhai   10      // Băm xung điều chỉnh tốc độ động cơ phải
 
PS2X ps2;                     // create PS2 Controller Class
 
bool doLine,analog;
int trangThai;
int lech;                     // xác định xem robot đang bị lệch sang trái hay lệch sang phải
 
void setup() {
  Serial.begin(9600);
  // Khởi tạo còi chip, động cơ trái, động cơ phải là đầu ra
  pinMode(dongCoTrai, OUTPUT);
  pinMode(dongCoPhai, OUTPUT);
  // Khởi tạo kết nối tay PS2
  ps2.config_gamepad(PS2_CLK, PS2_CMD, PS2_CS, PS2_DAT);
}
 
void loop() {
  // Bắt đầu đọc dữ liệu nhấn nút từ tay PS2
  ps2.read_gamepad();
  
  if(ps2.ButtonPressed(PSB_R2))          // Bật tắt chế độ dò line
    doLine = 1 - doLine;
  
  if(doLine) {
    chayDoLine();
    //Serial.println("doLine ON");
  }
  else {
    if(ps2.ButtonPressed(PSB_R1)) {         // chuyển chế độ điều khiển dùng joystick
      //Serial.println("Analog");
      analog = 1 - analog;
    }
  
  if(analog) {
    int analogLX = ps2.Analog(PSS_LX)-128;
    int analogLY = ps2.Analog(PSS_LY)-127;
    
    if(analogLY < 0 && analogLX == 0) {             
      dkDongCo(180, 180);
    }
    // Lùi
    else if(analogLY > 0 && analogLX == 0) {             
      dkDongCo(-120, -120);
    }
    // Quay trái
    else if(analogLY == 0 && analogLX < 0) {             
      dkDongCo(0, 180);
    }
    // Quay phải
    else if(analogLY == 0 && analogLX > 0) {             
      dkDongCo(180, 0);
    }
    // Cua trái
    else if(analogLY < 0 && analogLX < 0) {             
      dkDongCo(40, 150);
    }
    // Cua phải
    else if(analogLY > 0 && analogLX < 0) {            
      dkDongCo(150, 40);
    }
    // Lùi phải
    else if(analogLY > 0 && analogLX > 0) {            
      dkDongCo(-120, -40);
    }
    // Lùi trái
    else if(analogLY < 0 && analogLX > 1) {            
      dkDongCo(-40, -120);
    }
    // Dừng khi nhả các nút
    else if(analogLY == 0 && analogLX == 0) {
      dkDongCo(0, 0);
    }
  }
  else {
      int buttonUp = ps2.Button(PSB_PAD_UP);
      int buttonDown = ps2.Button(PSB_PAD_DOWN);
      int buttonLeft = ps2.Button(PSB_PAD_LEFT);
      int buttonRight = ps2.Button(PSB_PAD_RIGHT);
      if(buttonUp && !buttonDown && !buttonLeft && !buttonRight) {             
        dkDongCo(180, 180);
      }
      // Lùi
      else if(!buttonUp && buttonDown && !buttonLeft && !buttonRight) {             
        dkDongCo(-120, -120);
      }
      // Quay trái
      else if(!buttonUp && !buttonDown && buttonLeft && !buttonRight) {             
        dkDongCo(0, 180);
      }
      // Quay phải
      else if(!buttonUp && !buttonDown && !buttonLeft && buttonRight) {             
        dkDongCo(180, 0);
      }
      // Cua trái
      else if(buttonUp && !buttonDown && buttonLeft && !buttonRight) {             
        dkDongCo(40, 150);
      }
      // Cua phải
      else if(buttonUp && !buttonDown && !buttonLeft && buttonRight) {            
        dkDongCo(150, 40);
      }
      // Lùi phải
      else if(!buttonUp && buttonDown && !buttonLeft && buttonRight) {            
        dkDongCo(-120, -40);
      }
      // Lùi trái
      else if(!buttonUp && buttonDown && buttonLeft && !buttonRight) {            
        dkDongCo(-40, -120);
      }
      // Dừng khi nhả các nút
      else if(!buttonUp && !buttonDown && !buttonLeft && !buttonRight) {
        dkDongCo(0, 0);
      }
    }
  }
}
 
void chayDoLine() {
  trangThai = docDoLine(A0, A1, A2, A3, A4);     // Vị trí sensor tính từ trái sang phải,  Phát hiện line, trả về 0, Không có line, trả về 1
  //Serial.println(trangThai, HEX);
  switch(trangThai) {
    case 0x01:  dkDongCo(100, 20);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00001
    case 0x03:  dkDongCo(100, 40);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00011
    case 0x02:  dkDongCo(100, 55);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00010
    case 0x06:  dkDongCo(100, 75);lech = 1;Serial.println(trangThai, HEX); break;  // 0b000 00110
    case 0x07:  dkDongCo(120, 110);Serial.println(trangThai, HEX);break;            // 0b000 00111
            
    case 0x04:  dkDongCo(110, 110);lech = 0;Serial.println(trangThai, HEX); break;  // 0b000 00100
         
    case 0x0C:  dkDongCo(110, 120);Serial.println(trangThai, HEX);break;            // 0b000 01100
    case 0x08:  dkDongCo(75, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 01000
    case 0x18:  dkDongCo(55, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 11000
    case 0x10:  dkDongCo(40, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 10000
    case 0x1C:  dkDongCo(20, 100);lech = 2;Serial.println(trangThai, HEX); break;  // 0b000 11100
    default : {
                if(lech == 1)      {dkDongCo(100,  -30);Serial.println(trangThai, HEX);}
                else if(lech == 2) {dkDongCo(-30,  100);Serial.println(trangThai, HEX);}
      }
  }
}
 
void dkDongCo(float xungTrai, float xungPhai) {
  if(xungTrai >=0 ){
    digitalWrite(dongCoTrai, HIGH);
    analogWrite(bamXungTrai, xungTrai);
  }
  else{
    digitalWrite(dongCoTrai, LOW);
    analogWrite(bamXungTrai, -xungTrai);   // vì xung mang giá trị âm nên cần chuyển sang giá trị dương
  }
  if(xungPhai >=0 ){
    digitalWrite(dongCoPhai, HIGH);
    analogWrite(bamXungPhai, xungPhai);
  }
  else{
    digitalWrite(dongCoPhai, LOW);
    analogWrite(bamXungPhai, -xungPhai);   // vì xung mang giá trị âm nên cần chuyển sang giá trị dương
  }
}
 
uint8_t docDoLine(int sensor1, int sensor2, int sensor3, int sensor4, int sensor5) {
  int val;
  val = digitalRead(sensor1);
  val = val<<1 | digitalRead(sensor2);
  val = val<<1 | digitalRead(sensor3);
  val = val<<1 | digitalRead(sensor4);
  val = val<<1 | digitalRead(sensor5);
  return val;
}