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

.

หุ่นยนต์เดินตามเส้น 2WD 3 เซ็นเซอร์ CTRT5000 
อุปกรณ์ที่ต้องใช้ก็คือ

     1. 2WD Smart Robot Car Chassis Kits

     2. Arduino UNO R3 - Made in italy

     3. Arduino Sensor Shield V5.0

     4. Motor Drive Module L298N

     5. สาย Jumper Female to Male ยาว 20cm.

     6. สาย Jumper Female to Female ยาว 20cm.

     7. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน

     8. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน

     9. เสารองแผ่นพีซีบีแบบโลหะ ยาว 25 มม.

     10. สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว10มม.
   
     11. 3 Channel CTRT5000 Track Sensor Infrared Line Module


เริ่มต้นด้วยการ ประกอบ 2WD Smart Car Robot Chassis Kits และประกอบ รางถ่านแบบ 18650 และ เชื่อมต่อ Motor Drive Module L298N  เข้ากับ Arduino Sensor Shield V 5.0 วงจรตามรูป


หมายเหตุ : ถ้ามี Jumper อยู่ที่ขา ENA และ ENB ของ บอร์ด L298N  ให้ถอดออก



การเชื่อมต่อระหว่าง บอร์ด Arduino Sensor Shield V5.0 กับ บอร์ด L298N  



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



ประกอบ 2WD Smart Car Robot Chassis Kits และประกอบ รางถ่านแบบ 18650 และ เชื่อมต่อ 5V และ GND ของ Motor Drive Module L298N  เข้ากับ V และ G ของ Arduino Sensor Shield 






เชื่อมต่อการควบคุมมอเตอร์ ของ Motor Drive Module L298N  เข้ากับ  Arduino Sensor Shield






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

เปิดโปรแกรม 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(200);
    //Go backward
    backward(200);
    //Turn left
    turnLeft(400);
    coast(200);
    //Turn right
    turnRight(400);
    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);
}



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


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

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

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



การเชื่อมต่อระหว่าง บอร์ด Sensor Shield กับ บอร์ดเซ็นเซอร์ CTRT5000


Arduino Shield ->  
CTRT5000

V -> VCC
G -> GND
D2 -> L
D10 -> C
D4 -> R







เพื่อให้ เซ็นเซอร์ อยู่ใกล้พื้น และ  ตรวจจับเส้นได้ดียิ่งชึ้น จึงใช้ เสารองแผ่นพีซีบีแบบโลหะ ยาว 25 มม.ใสระหว่างตัวรถกับ  3 Channel CTRT5000





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





หลักการทำงานของ เซ็นเซอร์ 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 องศา



จากนั้นทดสอบ หุ่นยนต์เดินตามเส้น 2 WD 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, 120);  // ปรับความเร็วหุ่นยนต์
  analogWrite(enableB, 120); // ปรับความเร็วหุ่นยนต์

  // 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(100);
      forward(1);
      break;
    case GO_LEFT:
      Serial.println("Turn left");
      turnLeft(100);
      forward(1);
      break;
    case STOP:
      Serial.println("No line detected - STOP!");
      coast(300);
      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(300);

      break;
    case GO_POWERRIGHT:
      Serial.println("Power-turn 90 degrees right");
      coast(10);
      turnRight(300);

      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);
}


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


.

โปรเจค หุ่นยนต์เดินตามเส้น 3 เซ็นเซอร์ KY-033


   
     10. 1 Channel KY-033 Track Sensor จำนวน 3 ชิ้น

     11. เสารองแผ่นพีซีบีโลหะแบบเหลี่ยม 25 mm.


เริ่มต้นด้วยการ ประกอบ 2WD Smart Car Robot Chassis Kits และประกอบ รางถ่านแบบ 18650 และ เชื่อมต่อ Motor Drive Module L298N  เข้ากับ Arduino Sensor Shield V 5.0 วงจรตามรูป


หมายเหตุ : ถ้ามี Jumper อยู่ที่ขา ENA และ ENB ของ บอร์ด L298N  ให้ถอดออก



การเชื่อมต่อระหว่าง บอร์ด Arduino Sensor Shield V5.0 กับ บอร์ด L298N  



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


ประกอบ 2WD Smart Car Robot Chassis Kits และประกอบ รางถ่านแบบ 18650 และ เชื่อมต่อ 5V และ GND ของ Motor Drive Module L298N  เข้ากับ V และ G ของ Arduino Sensor Shield 






เชื่อมต่อการควบคุมมอเตอร์ ของ Motor Drive Module L298N  เข้ากับ  Arduino Sensor Shield






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

เปิดโปรแกรม 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(200);
    //Go backward
    backward(200);
    //Turn left
    turnLeft(400);
    coast(200);
    //Turn right
    turnRight(400);
    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);
}




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




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

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


จากนั้นประกอบ 1 Channel KY-033 Track Sensor เข้ากับ บอร์ด Arduino Sensor Shield V 5.0


การเชื่อมต่อ ระหว่าง เซ็นเซอร์ KY-033 ทั้ง3 ตัว กับ Arduino Sensor Shield



ใช้ เสารองแผ่นพีซีบีแบบโลหะ ยาว 25 มม. จำนวน 3 ชิ้น ยึด เซ็นเซอร์ KY-033







KY-033 -> Arduino


เซ็นเซอร์ด้านซ้าย
S-> A3

เซ็นเซอร์ตรงกลาง
S -> A4

เซ็นเซอร์ด้านขวา
S -> A5

เซ็นเซอร์ทั้ง 3ตัว
V -> V

G -> GND




ทดสอบการทำงานของ เซ็นเซอร์ ทั้ง 3 ตัว โดย Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3



/*
read analog data from A3-A5 and send to PC via Serial port
*/

int sensor_L , sensor_C , sensor_R ; //optical sensor values
String tmp ;
int ADC_stabilize = 5 ;

void setup ( ) {
  // initialize the serial communication:
  Serial . begin ( 9600 ) ;
}

void loop ( ) {
  //take a snapshot
  sensor_L = digitalRead ( A3 ) ;
  delay ( ADC_stabilize ) ; //stabilize
  sensor_L = digitalRead ( A3 ) ;
  delay ( ADC_stabilize ) ;

  sensor_C = digitalRead ( A4 ) ;
  delay ( ADC_stabilize ) ;
  sensor_C = digitalRead ( A4 ) ;
  delay ( ADC_stabilize ) ;

  sensor_R = digitalRead ( A5 ) ;
  delay ( ADC_stabilize ) ;
  sensor_R = digitalRead ( A5 ) ;
  delay ( ADC_stabilize ) ;

  tmp = "L=" + String ( sensor_L ) + " C=" + String ( sensor_C ) + " R=" + String ( sensor_R ) ;

  // send the value of analog inputs:
  Serial . println ( tmp ) ;

  // wait a bit for next reading
  delay ( 1000 ) ; //1000=1 sec


}



เมื่อเปิดดูที่ Serial Monitor




เมื่ออยู่ในพื้นที่สีขาว ผลลัพธ์ที่ได้ L=0 , C=0 , R=0 แสดงว่าอ่านค่าการสะท้อนกลับได้



และ เมื่ออยู่ในพื้นที่เทปสีดำ ผลลัพธ์ที่ได้ ผลลัพธ์ที่ได้ L=1 , C=1 , R=1 แสดงว่าอ่านค่าการสะท้อนกลับไม่ได้


 แสดงว่า เซ็นเซอร์ KY-033 ของเรา ทั้ง 3 ตัว พร้อมใช้งานแล้วครับ


ภาพรวมการต่อ หุ่นยนต์เดินตามเส้น 3 เซ็นเซอร์ KY-033






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

 L - C - R
 0 - 1 - 0 ให้หุ่นยนต์เดินหน้า
 0 - 0 - 1 ให้หุ่นยนต์เลี้ยวซ้าย

 0 - 1 - 1 ให้หุ่นยนต์เลี้ยวซ้าย 90 องศา

 1 - 0 - 0 ให้หุ่นยนต์เลี้ยวขวา 

 1 - 1 - 0 ให้หุ่นยนต์เลี้ยวขวา 90 องศา

 อื่นๆ ให้หุ่นยนต์เดินหน้า



จากนั้นทดสอบ หุ่นยนต์เดินตามเส้น 3 เซ็นเซอร์ KY-033ได้โดย  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;

int sensor[3] = {0, 0, 0};

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

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

void loop()
{

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

  sensor[0] = digitalRead(A3);
  sensor[1] = digitalRead(A4);
  sensor[2] = digitalRead(A5);


  if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) )
    forward(1);
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) )
    turnRight(10);
  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) )
    turnRight(400);
  else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0)  )
    turnLeft(10);
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) )
    turnLeft(400);

  else
    forward(1);
}

//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);
}




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

โปรเจค หุ่นยนต์ติดตามวัตถุ Ultrasonic




หุ่นยนต์ติดตามวัตถุ Ultrasonic 
อุปกรณ์ที่ต้องใช้ก็คือ

     1. 4WD Smart Robot Car Chassis Kits

     2. Arduino UNO R3 - Made in italy

     3. Arduino Sensor Shield V5.0

     4. Motor Drive Module L298N

     5. สาย Jumper Female to Male ยาว 20cm.

     6. สาย Jumper Female to Female ยาว 20cm.
   
     7. สาย Jumper Female to Male ยาว 10cm

     8. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน

     9. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน

     10. เสารองแผ่นพีซีบีโลหะแบบเหลี่ยม 6 mm 4 ชิ้น

     11. สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว10มม. 6 ตัว
   
     12. สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว40มม. 4 ตัว

     13. เพาเวอร์สวิตซ์สำหรับเปิดปิด (ON / OFF Rocker Switch)
 
     14. เซอร์โวมอเตอร์ MG996R พร้อมกับอุปกรณ์เสริม
   
     15. Ultrasonic Sensor HC-SR04
   
     16. Mounting Bracket for HC-SR04 แบบสั้น

     17. Tilt U Bracket

     18. Jack 5.5 X 2.1mm สำหรับ เพาเวอร์ซัพพลาย ของ Arduino

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


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


ประกอบเข้ากับ โครง ชิ้นที่ 1 ของ 4WD smart car chassis ดังรูป



ประกอบ น็อต 6 จุด สำหรับ วาง โครง ชิ้นที่ 2


ประกอบ บอร์ด Motor Drive Module L298N ลงที่ โครง ชิ้นที่ 1


เชื่อมสายไฟ ของ มอเตอร์ ด้านบน สายสีดำ ขั้ว -  ของมอเตอร์ ตัวที่1 เข้า กับมอเตอร์ ตัวที่2 และ สายสีแดง ขั้ว+  ของมอเตอร์ ตัวที่1 เข้า กับมอเตอร์ ตัวที่2 และ เช่นกันทำที่ มอเตอร์ คู่ด้านล่างด้วย เพื่อให้เหมือนมีมอเตอร์ จาก 4 ตัว เหลือ 2 ตัว

จากนั้น ประกอบสายไฟ ของมอเตอร์ ทั้ง 2 ตัว เข้ากับ บอร์ด L298N ดังรูป



ประกอบ โครงชิ้นที่ 2 เข้าที่ ด้านบนของ โครงชิ้นที่ 1 และ ประกอบ บอร์ด Arduino UNO R3 ลงที่โครงชิ้นที่ 2 ซึ่งอยู่ชั้นบน


ประกอบ บอร์ด Arduino Sensor Shield V5.0 ลงที่ บอร์ด Arduino UNO R3



ประกอบ รางถ่านแบบ 18650 และ เชื่อมต่อ วงจรตามรูป



หมายเหตุ : ถ้ามี Jumper อยู่ที่ขา ENA และ ENB ของ บอร์ด L298N  ให้ถอดออก




การเชื่อมต่อระหว่าง บอร์ด Arduino Sensor Shield V5.0 กับ บอร์ด L298N  


*** 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(200);
    //Go backward
    backward(200);
    //Turn left
    turnLeft(400);
    coast(200);
    //Turn right
    turnRight(400);
    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 ครั้ง และ เดินหน้าอีกหนึ่งครั้งแล้วจึงหยุด ถ้าไม่ถูกต้องให้แก้ไข เช่นการต่อขั้วของมอเตอร์ผิด เป็นต้น

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


ประกอบ Jack 5.5 X 2.1mm เข้ากับ  เพาเวอร์ซัพพลาย ของ Arduino

โดย ขั้วบวก สายสีแดง เข้ากับ ไฟ+ สายสีแดง จาก รางถ่านแบบ 18650 หลัง ผ่านเพาเวอร์สวิตซ์สำหรับเปิดปิด (ON / OFF Rocker Switch) และ ขั้วลบ สีดำ เข้ากับ กราวด์ (G)



จากนั้นประกอบ เซอร์โวมอเตอร์ MG996R เข้ากับ บอร์ด Arduino Sensor Shield V5.0

โดยให้ สายสีส้ม -> D10 , สายสีแดง -> V , สายสีแดง , สายสีน้ำตาล -> G



ใช้สกรูหัวกลม+น็อตตัวเมีย ขนาด 3มม ยาว 40มม. 4 ตัว ยึด เซอร์โวมอเตอร์ MG996R เข้ากับ ด้านหน้าของหุ่นยนต์



ปรับองศา ของ เซอร์โวมอเตอร์ MG996R = 90 องศา โดย Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3


#include <Servo.h>

Servo myservo;  //ประกาศตัวแปร 
myservo เรียกใช้งานคำสั่งจาก Library
 
void setup()
 
{
 
  myservo.attach(10); //ให้ Digital Pin 10 เป็นขาของ Servo
 
}

void loop()
{


  myservo.write(90);  //หมุน 90 องศา
  delay(1000);


}



แล้วจึงยึดอุปกรณ์เสริม ของเซอร์โวมอเตอร์ MG996R



ประกอบ Ultrasonic Sensor HC-SR04 เข้ากับ Mounting Bracket for HC-SR04 แบบสั้น


ใช้กาว ยึด Mounting Bracket for HC-SR04 แบบสั้น เข้า กับ Tilt U Bracket




จากนั้นประกอบ Ultrasonic Sensor HC-SR04 เข้ากับ บอร์ด Arduino Sensor Shield V5.0

โดยให้ TRIGGER -> A0 , ECHO -> A1 , VCC -> V  , GND  -> G



.ใช้สาย Jumper Female to Male ยาว 10cm สาย ต่อจาก Jumper Female to Female ยาว 20cm. แล้วจึงต่อเข้า Ultrasonic Sensor HC-SR04



ยึด Tilt U Bracket เข้ากับ อุปกรณ์เสริม ของเซอร์โวมอเตอร์ MG996R





การเพิ่มไลบรารี่ NewPing

ไลบรารี่ NewPing คือ ไลบรารี่ ฟังก์ชัน ที่มีผู้พัฒนาเตรียมพร้อมไว้ให้เราแล้ว โดยให้ไปดาวน์โหลด ไลบรารี่ NewPing ได้ที่

  https://bitbucket.org/teckel12/arduino-new-ping/downloads

จากนั้นให้ทำการเพิ่ม ไลบรารี่ NewPing ให้กับ Arduino (IDE) ของเรา ตามรูป



เลือกไฟล์ ไลบรารี่ NewPing  .ZIP ที่เรา ดาวน์โหลดมาอยู่ในคอมพิวเตอร์ของเรา จากนั้นคลิก Open โปรแกรม จะเพิ่ม ไลบรารี่ NewPing เข้าสู่ Arduino (IDE) ของเรา



เมื่อเข้าดูที่ Include Library จะพบ ไลบรารี่ NewPing เพิ่มเข้ามาด้านล่างสุด
(สามารถใช้วิธีการเดียวกันนี้ ในการ การเพิ่ม ไลบรารี่ ฟังก์ชัน อื่นๆ ได้เช่นกัน )



ภาพรวมการต่อ หุ่นยนต์ติดตามวัตถุ Ultrasonic




จากนั้นทดสอบ หุ่นยนต์ติดตามวัตถุ Ultrasonic ได้โดย  Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3


#include <Servo.h>

#include <NewPing.h>

// 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;

#define TRIGGER_PIN  A0 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     A1  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 25 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
Servo myservo;  // create servo object to control a servo
// a maximum of eight servo objects can be created

int pos = 23;
int distance = 0;
int stepsize = 5;

int detect = 0;


void setup() {
  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.

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

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

  myservo.attach(10);  // attaches the servo on pin 10 to the servo object
  myservo.write(pos);

}
void loop() {

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

  delay(30);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int uS = sonar.ping();
  // Send ping, get ping time in microseconds (uS).
  distance = uS / US_ROUNDTRIP_CM;

  if (distance <= 6) {
    // nothing detected
    detect = 0;
    // move left

   turnLeft(100);
    forward(1);


    pos = pos + stepsize;
    myservo.write(pos);
    if (pos > 200) {
      pos = 200;
    }
  }
  else {
    // something is detected
    detect = 1;
    // move right

    turnRight(100);
    forward(1);

    pos = pos - stepsize;
    myservo.write(pos);
    if (pos < 20) {
      pos = 20;
    }




  }

  Serial.print(detect);
  Serial.print(", ");
  Serial.print(pos);
  Serial.print(", ");
  Serial.println(distance);
}

//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);
}



วีดีโอผลลัพธ์การทำงานของ โปรเจค หุ่นยนต์ติดตามวัตถุ Ultrasonic