วันเสาร์ที่ 21 มกราคม พ.ศ. 2560

Code โปรเจค หุ่นยนต์เดินตามเส้น 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  


ประกอบ 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

เซ็นเซอร์ด้านซ้าย , กลาง และ กลาง
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

ที่

ไม่มีความคิดเห็น:

แสดงความคิดเห็น

สมัครสมาชิก: ส่งความคิดเห็น (Atom)

Code โปรเจค Arduino เปิดปิดไฟ ด้วย Wireless Joystick

PS2 Joystick Playstation Adapter for Arduino อะแดปเตอร์แปลงหัว PS2 เป็นขาต่อแบบ DIP สำหรับ Arduino คอนเนคเตอร์สำหรับแปลงจาก PS2 เป็นขา DI...