โปรเจค หุ่นยนต์เดินตามเส้น 4WD 3 เซ็นเซอร์ CTRT5000

   
     12. 3 Channel CTRT5000 Track Sensor Infrared Line Module

     13. สายไฟแดงดำ ขนาด 22AWG


เริ่มด้วย ต่อสายไฟสำหรับมอเตอร์ ทั้ง 4 ตัว ที่ 4WD smart car chassis ให้มา โดยให้ ขั้วลบ สายไฟสีดำ อยู่ด้านบน เหมือนกันทั้ง 4 ตัว

*** VCC ของ Arduino Sensor Shield V5.0  คือ V ***

หลังจากนั้นให้ทดสอบเบื้องต้น ว่าการหมุนของล้อถูกต้องหรือไม่ โดย

เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3



// Motor A pins (enableA = enable motor, pinA1 = forward, pinA2 = backward)
int enableA = 3;
int pinA1 = 6;
int pinA2 = 7;

//Motor B pins (enabledB = enable motor, pinB2 = forward, pinB2 = backward)
int enableB = 5;
int pinB1 = 8;
int pinB2 = 9;

//This lets you run the loop a single time for testing
boolean run = true;

void setup() {
 pinMode(enableA, OUTPUT);
 pinMode(pinA1, OUTPUT);
 pinMode(pinA2, OUTPUT);

 pinMode(enableB, OUTPUT);
 pinMode(pinB1, OUTPUT);
 pinMode(pinB2, OUTPUT);
}
void loop() {
  if (run) {
    delay(2000);
    enableMotors();
    //Go forward
    forward(600);
    //Go backward
    backward(600);
    //Turn left
    turnLeft(700);
    coast(200);
    //Turn right
    turnRight(700);
    coast(200);
    //This stops the loop
    run = false; 
  }
}

//Define high-level H-bridge commands

void enableMotors()
{
 motorAOn();
 motorBOn();
}

void disableMotors()
{
 motorAOff();
 motorBOff();
}

void forward(int time)
{
 motorAForward();
 motorBForward();
 delay(time);
}

void backward(int time)
{
 motorABackward();
 motorBBackward();
 delay(time);
}

void turnLeft(int time)
{
 motorABackward();
 motorBForward();
 delay(time);
}

void turnRight(int time)
{
 motorAForward();
 motorBBackward();
 delay(time);
}

void coast(int time)
{
 motorACoast();
 motorBCoast();
 delay(time);
}

void brake(int time)
{
 motorABrake();
 motorBBrake();
 delay(time);
}
//Define low-level H-bridge commands

//enable motors
void motorAOn()
{
 digitalWrite(enableA, HIGH);
}

void motorBOn()
{
 digitalWrite(enableB, HIGH);
}

 //disable motors
void motorAOff()
{
 digitalWrite(enableB, LOW);
}

void motorBOff()
{
 digitalWrite(enableA, LOW);
}

 //motor A controls
void motorAForward()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, LOW);
}

void motorABackward()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, HIGH);
}

//motor B controls
void motorBForward()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, LOW);
}

void motorBBackward()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, HIGH);
}

//coasting and braking
void motorACoast()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, LOW);
}

void motorABrake()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, HIGH);
}

void motorBCoast()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, LOW);
}

void motorBBrake()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, HIGH);
}

ถอดสาย USB ระหว่าง Arduino กับ คอมพิวเตอร์ออก และ หาอุปกรณ์ที่สามารถยกตัวรถ 4WD smart car chassis ขึ้นแล้ว ล้อไม่แตะพื้น เพือทดสอบการหมุนของล้อว่าถูกต้องหรือไม่

ใส่ แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน ไปที่ รางถ่าน และ ตรวจสอบขั้วของแบตเตอรี่ ใส่ถุกต้องหรือไม่


โปรแกรมนี้จะทำงานเพียง 1 ครั้ง ถ้าต้องการทดลองใหม่ให้ถอด แบตเตอรี่ออก (หรือ ปิดเปิด สวิทช์ไฟใหม่) แล้วใส่เข้าไปใหม่ เมื่อล้อหมุน ตรวจสอบการหมุน ขอล้อต่างๆถูกต้องหรือไม่ ถ้าต่อวงจรถูกต้อง ล้อ ทั้งสองข้างจะหมุนไปข้างหน้า 1ครั้ง กลับหลัง 1 ครั้ง และ เดินหน้าอีกหนึ่งครั้งแล้วจึงหยุด ถ้าไม่ถูกต้องให้แก้ไข เช่นการต่อขั้วของมอเตอร์ผิด เป็นต้น

ถ้าทุกอย่างถูกต้อง ทดลอง ยกลงวางที่พื้นแล้วทดสอบ อีกครั้ง ถอด แบตเตอรี่ออก (หรือ ปิดเปิด สวิทช์ไฟใหม่) แล้วใส่เข้าไปใหม่ ถ้าทุกอย่างถูกต้อง รถจะเคลื่อนที่ เดินหน้า -> ถอยหลัง -> เลี้ยวซ้าย -> เลี้ยวขวา

จากนั้นประกอบ 3 Channel CTRT5000 เข้ากับ บอร์ด Arduino Sensor Shield V5.0



หลักการทำงานของ เซ็นเซอร์ CTRT5000 ก็คือถ้าพบพื้นสีขาว ค่าที่อ่านได้ = 1 (NOLINE) และ เมื่อพบเทปสีดำ ค่าที่อ่านได้  = 0 (LINE)

เราจึงะนำค่าที่แตกต่างกันของเซ็นเซอร์ ทั้ง 3 ตัว  มาใช้เป็นเงื่อนไขตัวอย่างในการเขียนโปรแกรม ดังนี้

 L - C - R
 1 - 0 - 1 ให้หุ่นยนต์เดินหน้า
 0 - 1 - 1 ให้หุ่นยนต์เลี้ยวซ้าย
 1 - 1 - 0 ให้หุ่นยนต์เลี้ยวขวา
 0 - 0 - 0 ให้หุ่นยนต์หยุด
 1 - 1 - 1 ให้หุ่นยนต์ถอยหลัง
 0 - 0 - 1 ให้หุ่นยนต์เลี้ยวซ้าย 90 องศา
 1 - 0 - 0 ให้หุ่นยนต์เลี้ยวขวา 90 องศา

จากนั้นทดสอบ หุ่นยนต์เดินตามเส้น 4WD 3 เซ็นเซอร์ CTRT5000 ได้โดย  Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3

// connect the sensors to digital pins
#define LEFT_SENSORPIN 2
#define CENTER_SENSORPIN 10
#define RIGHT_SENSORPIN 4

// do you have black line on white ground or white line on black ground?
#define LINE LOW
#define NOLINE HIGH

// define some symbolic constants for our driving directions
enum {GO_AHEAD, GO_LEFT, GO_RIGHT, STOP, GO_POWERLEFT, GO_POWERRIGHT, GO_BACK};

// Motor A pins (enableA = enable motor, pinA1 = forward, pinA2 = backward)
int enableA = 3;
int pinA1 = 6;
int pinA2 = 7;

//Motor B pins (enabledB = enable motor, pinB2 = forward, pinB2 = backward)
int enableB = 5;
int pinB1 = 8;
int pinB2 = 9;

void setup()
{
  Serial.begin(9600);
  Serial.println("*** LINE FOLLOWER ***");
  pinMode(LEFT_SENSORPIN,INPUT);
  pinMode(CENTER_SENSORPIN,INPUT);
  pinMode(RIGHT_SENSORPIN,INPUT);

 pinMode(enableA, OUTPUT);
 pinMode(pinA1, OUTPUT);
 pinMode(pinA2, OUTPUT);

 pinMode(enableB, OUTPUT);
 pinMode(pinB1, OUTPUT);
 pinMode(pinB2, OUTPUT);

}

void loop()
{

//Run the motors at slightly less than full power

enableMotors();
analogWrite(enableA, 90);  // ปรับความเร็วหุ่นยนต์
analogWrite(enableB, 90); // ปรับความเร็วหุ่นยนต์

// read input from sensors
  byte leftSensor=digitalRead(LEFT_SENSORPIN);
  byte centerSensor=digitalRead(CENTER_SENSORPIN);
  byte rightSensor=digitalRead(RIGHT_SENSORPIN);

  // calculate direction
  byte goDirection;
  // if only center sensor detects line, go straight ahead
  if (leftSensor==NOLINE && centerSensor==LINE && rightSensor==NOLINE)
      goDirection= GO_AHEAD;
  // if only left sensor detects line, turn left
  else if (leftSensor==LINE && centerSensor==NOLINE && rightSensor==NOLINE)
      goDirection= GO_LEFT;
  // if only right sensor detects line, turn right  
  else if (leftSensor==NOLINE && centerSensor==NOLINE  && rightSensor==LINE)
      goDirection= GO_RIGHT;
  // if no sensor detects any line: we are either finished or out of control and will stop  
  else if (leftSensor==LINE && centerSensor==LINE && rightSensor==LINE)
      goDirection= STOP;
  else if (leftSensor==NOLINE && centerSensor==NOLINE && rightSensor==NOLINE)
      goDirection= GO_BACK;
  // if left and center sensor detect line, must be 90° turn to the left
  else if (leftSensor==LINE && centerSensor==LINE && rightSensor==NOLINE)
      goDirection= GO_POWERLEFT;
  // if right and center sensor detect line, must be 90° turn to the right
  else if (leftSensor==NOLINE && centerSensor==LINE && rightSensor==LINE)
      goDirection= GO_POWERRIGHT;


  // Now we have found the direction,
  // show it on Serial and control motors accordingly
  switch (goDirection)
  {
    case GO_AHEAD:
      Serial.println("Go ahead");
      forward(1);
   
      break;
    case GO_RIGHT:
      Serial.println("Turn right");
      turnRight(10);
   
      break;
    case GO_LEFT:
      Serial.println("Turn left");
       turnLeft(10);
   
      break;
    case STOP:
      Serial.println("No line detected - STOP!");
        coast(600);
       disableMotors();
      break;
    case GO_BACK:
      Serial.println("No line detected - BACK!");
     
     backward(1);
      break;
    case GO_POWERLEFT:
      Serial.println("Power-turn 90 degrees left");
      coast(10);
      turnLeft(600);
 
      break;
    case GO_POWERRIGHT:
      Serial.println("Power-turn 90 degrees right");
       coast(10);
       turnRight(600);
   
      break;

  }
}


//Define high-level H-bridge commands

void enableMotors()
{
 motorAOn();
 motorBOn();
}

void disableMotors()
{
 motorAOff();
 motorBOff();
}

void forward(int time)
{
 motorAForward();
 motorBForward();
 delay(time);
}

void backward(int time)
{
 motorABackward();
 motorBBackward();
 delay(time);
}

void turnLeft(int time)
{
 motorABackward();
 motorBForward();
 delay(time);
}

void turnRight(int time)
{
 motorAForward();
 motorBBackward();
 delay(time);
}

void coast(int time)
{
 motorACoast();
 motorBCoast();
 delay(time);
}

void brake(int time)
{
 motorABrake();
 motorBBrake();
 delay(time);
}
//Define low-level H-bridge commands

//enable motors
void motorAOn()
{
 digitalWrite(enableA, HIGH);
}

void motorBOn()
{
 digitalWrite(enableB, HIGH);
}

 //disable motors
void motorAOff()
{
 digitalWrite(enableB, LOW);
}

void motorBOff()
{
 digitalWrite(enableA, LOW);
}

 //motor A controls
void motorAForward()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, LOW);
}

void motorABackward()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, HIGH);
}

//motor B controls
void motorBForward()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, LOW);
}

void motorBBackward()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, HIGH);
}

//coasting and braking
void motorACoast()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, LOW);
}

void motorABrake()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, HIGH);
}

void motorBCoast()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, LOW);
}

void motorBBrake()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, HIGH);
}


วีดีโอผลลัพธ์การทำงานของ โปรเจค หุ่นยนต์เดินตามเส้น 4WD 3 เซ็นเซอร์ CTRT5000