Code robot dò line 5 mắt

Đỗ Hữu Toàn gửi vào Chủ nhật, 17 Tháng 7, 2016 - 22:30

• 76009 lượt xem

Tiếp nối chuỗi bài viết của bạn Đinh Hồng Thái về robot dò line...Nhưng vì lý do nào đó, bạn Thái đã ngừng chuỗi bài viết của mình một cách dở dang!!! Hôm nay, mình sẽ hướng dẫn các bạn phần cuối của Series Robot dò line của bạn Đinh Hồng Thái.

Youtube: 

Liên hệ Mr. Khánh 097.276.8491 để được tư vấn triển khai các dự án IoT

Liên hệ Mr. Đạt ZN 037.3998.468 tư vấn đồ án môn học

Liên hệ Mr. Thân 097.111.3732 để tư vấn thiết kế PCB chất lượng cao 2-40 lớp 

Chia sẻ tình yêu với

 Arduino 

Hãy trở thành thành viên của Cộng đồng Arudino Việt Nam để mở khóa chức năng này.

Bạn ơi

Cùng đăng nhập vào Cộng đồng Arduino Việt Nam để mở hết các chức năng của Cộng đồng nhé.

Video giải thích đây nhé
Học lập trình ở đâu

Còn đây là code mẫu cho anh em

/*
 * 17/3/2016
 * Created by: Anh Robot
 * Website: linhkienrobotics.com
 * Tutorial video at hoclamrobot.vn
 */

#define inA1 6 //Định nghĩa chân in1 của động cơ A
#define inA2 7 //Định nghĩa chân in2 của động cơ A
#define inB1 8 //Định nghĩa chân in1 của động cơ B
#define inB2 9 //Định nghĩa chân in2 của động cơ B
#define linesens1 10 //Định nghĩa chân cảm biến line 1
#define linesens2 11 //Định nghĩa chân cảm biến line 2
#define linesens3 12 //Định nghĩa chân cảm biến line 3
#define linesens4 13 //Định nghĩa chân cảm biến line 4
void setup() {
  Serial.begin(9600);
  pinMode(inA1, OUTPUT);//Set chân in1 của dc A là output
  pinMode(inA2, OUTPUT);//Set chân in2 của dc A là output
  pinMode(inB1, OUTPUT);//Set chân in1 của dc B là output
  pinMode(inB2, OUTPUT);//Set chân in2 của dc B là output
  pinMode(linesens1, INPUT);//Set chân cảm biến 1 là input
  pinMode(linesens2, INPUT);//Set chân cảm biến 2 là input
  pinMode(linesens3, INPUT);//Set chân cảm biến 3 là input
  pinMode(linesens4, INPUT);//Set chân cảm biến 4 là input

}
//Các bạn có thể vào linhkienrobotics.com để tìm thấy nhiều hướng dẫn hơn về robotics nhé
void loop() {
    darkLineFollower (inA1, inA2, inB1, inB2, linesens1, linesens2, linesens3, linesens4);
    analogWrite(3,100);
    analogWrite(5,100);
    delayMicroseconds(1);
}

void darkLineFollower (byte inR1, byte inR2, byte inL1, byte inL2, byte sen1, byte sen2, byte sen3, byte sen4)
{
  //Hàm điều khiển robot bám line màu tối
  //inR1, inR2 và inL1, inL2 là các chân tín hiệu lần lượt điều khiển động cơ di chuyển bên phải và trái
  //sen1 đến sen4 là chân nhận tín hiệu từ cảm biến hồng ngoại
  //Bây giờ thì lập trình thôi
  switch (deviationDarkLine4Sensor (sen1, sen2, sen3, sen4))
  {
    case -1:
      robotMover( inR1, inR2, inL1, inL2, 6);// rẽ phải
      break;
    case -2:
      robotMover( inR1, inR2, inL1, inL2, 6);
      break;
    case 1:
      robotMover( inR1, inR2, inL1, inL2, 5);// rẽ trái
      break;
    case 2:
      robotMover( inR1, inR2, inL1, inL2, 5);
      break;
    case 0:
      robotMover( inR1, inR2, inL1, inL2, 1);// tiến thẳng
      break;
    case 3:
      robotMover( inR1, inR2, inL1, inL2, 2);// lệch vạch thì lùi
      break;

             }

   }
void robotMover (byte inR1, byte inR2, byte inL1, byte inL2, byte action)
{
  /*
  inR1 inR2 là 2 chân tín hiệu động cơ bên phải
  inL1 inL2 là 2 chân tín hiệu động cơ bên trái
  action= 0 đứng yên
  action =1 đi thẳng
  action =2 lùi lại
  action =3 quay trái
  action =4 quay phải
  action =5 rẽ trái
  action =6 rẽ phải

  */
  switch (action)
  {
    case 0:// không di chuyển
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 1://đi thẳng
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    case 2:// lùi lại
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    case 3:// quay trái
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    case 4:// quay phải
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    case 5:// rẽ trái
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 6:// rẽ phải
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    default:
      action = 0;

         }
}

void motorControlNoSpeed (byte in1, byte in2, byte direct)
{
  // in1 and in2 are 2 signal pins to control the motor
  // en is the enable pin
  // the defauspeed is the highest
  // direct includes:
  //    0:Stop
  //    1:Move on 1 direct
  //    2:Move on another direct
  switch (direct)
  {
    case 0:// Dừng không quay
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);
      break;
    case 1:// Quay chiều thứ 1
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      break;
    case 2:// Quay chiều thứ 2
      digitalWrite(in1, LOW);
      digitalWrite(in2, HIGH);
      break;
      //default:
  }
}
boolean IFSensor (byte PinNumb)
{
//0 sáng
//1 tối
  return(digitalRead (PinNumb));   
}

int deviationDarkLine4Sensor (int PinNumb1, int PinNumb2, int PinNumb3, int PinNumb4)
{
  int left = 0; //biến kiểm tra lệch trái
  int right = 0; // biến kiểm tra lệch phải
  left = IFSensor (PinNumb1)+IFSensor (PinNumb2); //kiểm tra mấy cảm biến trái ở trong màu đen
  right= IFSensor (PinNumb3)+IFSensor (PinNumb4); //kiểm tra mấy cảm biến phải ở trong màu đen
  Serial.print("left=");
  Serial.println(left);
  Serial.print("right=");
  Serial.println(right);
  if ((left!=0) || (right!=0))return left - right;
  else return 3;  
  /*
  Kết quả trả về là 0 là không lệch
  Âm là lệch trái
  Dương là lệch phải
  */
}