CHƯƠNG 2. THIẾT KẾ HÌNH DẠNG VÀ GIA CÔNG ROBOT
2.6. Xây dựng chương trình bằng Arduino IDE
2.6.1. Lưu đồ thuật toán
2.6.2. Chương tr2nh điều khiển
#include <Servo.h>
#include <Wire.h>
// Khai báo chân kết nối với mô-đun L298 const int IN1 = 2;
const int IN2 = 3;
const int IN3 = 4;
const int IN4 = 7;
// Khai báo chân điều khiển tốc độ const int speedLeftPin = 6; 6//
const int speedRightPin = 5; //
// Khai báo 3 chân kết nối với cảm biến hồng ngoại const int sensor1Pin = A0;
const int sensor2Pin = A1;
const int sensor3Pin = A2;
const int CB_tra_hang = A4;
// Khai báo chân kết nối với cảm biến siêu âm HC-SR04 const int trigPin = 9; // Chân Trigger
const int echoPin = 10; // Chân Echo // Khai báo chân kết nối với servo
const int servoLeftPin = 11; // Chân kết nối với servo trái const int servoTayPin = 12; // Chân kết nối với servo phải( gắp) // Khai báo đối tượng Servo
Servo servoLeft;
Servo servoTay;
int goc;
// Khai báo biến lưu giữ giá trị tốc độ xe
int speedLeftValue = 150; 6// 80-255 Giá trị tốc độ khởi đầu (có thể điều chỉnh)
int speedRightValue = 150; // Giá trị tốc độ khởi đầu (có thể điều chỉnh)
void setup() { 6 Serial begin. (9600);
6 // Cài đặt chân làm OUTPUT cho mô-đun L298 và chân PWM cho tốc độ 6 pinMode(IN1, OUTPUT ;)
6 pinMode(IN2, OUTPUT ;) 6 pinMode(IN3, OUTPUT ;) 6 pinMode(IN4, OUTPUT ;)
6 pinMode(sensor1Pin, INPUT ;) 6 pinMode(sensor2Pin, INPUT ;) 6 pinMode(sensor3Pin, INPUT ;) 6 pinMode(CB_tra_hang, INPUT ; 6) 6 pinMode(trigPin, OUTPUT ;) 6 pinMode(echoPin, INPUT ;) 6 // Cài đặt chân làm OUTPUT cho servo
6 servoLeft attach. (servoLeftPin ; 6// dễ xung đột với l298 ở pwm: ) 3.5.6.11 thì ok
6 servoTay attach. (servoTayPin ; // ) 6 servoLeft write. (120); 6 6 6 servoTay write. (120); }
void loop() {
6 // Đọc giá trị từ cảm biến hồng ngoại 6 int sensor1Value = analogRead(sensor1Pin); 6 int sensor2Value = analogRead(sensor2Pin); 6 int sensor3Value = analogRead(sensor3Pin); 6 int tra_hang = analogRead(CB_tra_hang); 6 // Đo khoảng cách từ cảm biến siêu âm đến vật cản 6 float duration, distance;
6 digitalWrite(trigPin, LOW ;) 6 delayMicroseconds( )2; 6 digitalWrite(trigPin, HIGH ;) 6 delayMicroseconds(10); 6 digitalWrite(trigPin, LOW ;) 6 duration = pulseIn(echoPin, HIGH ;)
6 distance = duration / * ( 2) 0.0343; // Tính khoảng cách theo thời gian và vận tốc âm thanh
6 // In khoảng cách ra Serial Monitor (sử dụng để kiểm tra) 6 Serial print. ("Distance: ");
6 Serial println. (distance);
6 // Xác định hướng di chuyển dựa trên giá trị cảm biến
6 boolean goForward = sensor2Value > ( 500); 6// Điều chỉnh ngưỡng cảm biến theo yêu cầu
6 boolean turnLeft = sensor1Value > ( 500); 6 boolean turnRight = sensor3Value > ( 500); 6 boolean ha_hang = tra_hang > ( 500);
6 distance < if( 20) {
6 6 // Nếu có vật cản gần, dừng xe 6 6 analogWrite(speedLeftPin, ;0) 6 6 analogWrite(speedRightPin, ;0) 6 6 digitalWrite(IN1, LOW ;) 6 6 digitalWrite(IN2, LOW ;)
6 6 digitalWrite(IN3, LOW ;) 6 6 digitalWrite(IN4, LOW ;) 6 6
6 6 // Mở cánh tay gắp đồ (điều chỉnh góc mở cánh tay theo yêu cầu) 6 //1-Duyệt qua góc từ 60 độ về 0 độ với bước 1 độ
6 for goc = ( 120; goc >= 10; goc-- ) { 6 6// Đặt góc của servo 6 6 servoLeft write. (goc); 6 6 6 6 6 6
6 6 delay(20); // Chờ 20 milliseconds (0.02 giây) 6 6}
6 delay(1000); // Chờ 1 giây ở góc 0 độ // 2-
6 for goc = ( 120; goc >= 65; goc-- ) { 6 6// Đặt góc của servo 6 6 6 6 6 6 servoTay write. (goc); 6 6
6 6 delay(20); // Chờ 20 milliseconds (0.02 giây) 6 6}
6 delay(1000); // Chờ 1 giây ở góc 0 độ // 3- Duyệt qua góc từ 0 đến 60 độ với bước 1 độ 6 for goc = ; goc <= ( 30 120; goc++ ) { // Đặt góc của servo 6 6 6 6 servoLeft write. (goc); 6 6 6 6
6 6 delay(20);// Chờ 20 milliseconds (0.02 giây) 6 }
6 6 goc = servoLeft read. ();
6 6 Serial print. ("Góc hiện tại: "); Serial println. (goc); 6 delay(1000); // Chờ 1 giây ở góc 60 độ
6 }
6 6 distance > if( 20) {
6 6 6// Xác định hướng di chuyển dựa trên giá trị cảm biến 6
6 6 if(goForward){
6 6 // Điều khiển xe tiến lên phía trước với tốc độ đã cài đặt 6 6 6 6 6 6 analogWrite(speedLeftPin, speedLeftValue ;)
6 6 analogWrite(speedRightPin, speedRightValue ;) 6 6 digitalWrite(IN1, HIGH ;)
6 6 digitalWrite(IN2, LOW ;) 6 6 digitalWrite(IN3, LOW ;) 6 6 digitalWrite(IN4, HIGH ;) 6 6 Serial print. ("xe chay thẳng="); 6 6 int toc_do = analogRead(speedLeftPin); 6 6 Serial print. (toc_do);
6 }else{ 6 6 // Dừng xe
6 6 analogWrite(speedLeftPin, ;0) 6 6 analogWrite(speedRightPin, ;0) 6 6 digitalWrite(IN1, LOW ;)
6 6 6int toc_do = analogRead(speedLeftPin); 6 6 Serial print. (toc_do);
6 6 // Nếu cảm biến bên trái hoạt động, rẽ trái 6 6 if(turnLeft){
6 6 6 // Thực hiện các hành động rẽ trái, TRÁI >PHẢI => RẼ PHẢI 6 6 6 // (Bạn có thể cần điều chỉnh động cơ theo hướng tương ứng) 6 6 analogWrite(speedLeftPin, speedLeftValue); //TRÁI >PHẢI => RẼ PHẢI 6 6 analogWrite(speedRightPin, speedRightValue-40);
6 6 digitalWrite(IN1, HIGH ;) 6 6 digitalWrite(IN2, LOW ;) 6 6 digitalWrite(IN3, LOW ;) 6 6 digitalWrite(IN4, HIGH ;) 6 6 6Serial print. ("trai=");
6 6 6int toc_do = analogRead(speedLeftPin); 6 6 Serial print. (toc_do);
6 6 }
6 6 // Nếu cảm biến bên phải hoạt động, rẽ phải 6 6 if(turnRight){
6 6 6 // Thực hiện các hành động rẽ phải
6 6 6 // (Bạn có thể cần điều chỉnh động cơ theo hướng tương ứng) 6 6 analogWrite(speedLeftPin, speedLeftValue-40);
6 6analogWrite(speedRightPin, speedRightValue ;) 6 6 digitalWrite(IN1, HIGH ;)
6 6 digitalWrite(IN2, LOW ;) 6 6 digitalWrite(IN3, LOW ;) 6 6 digitalWrite(IN4, HIGH ;) 6 6 Serial print. ("phai=");
6 6 6int toc_do = analogRead(speedLeftPin); 6 6 Serial print. (toc_do);
6 6 }
6 6 // Nếu cảm biến trả hàng hoạt động 6 6 if(ha_hang){
6 6 6 // Dừng xe
6 6 analogWrite(speedLeftPin, ;0) 6 6 analogWrite(speedRightPin, ;0) 6 6 digitalWrite(IN1, LOW ;) 6 6 digitalWrite(IN2, LOW ;) 6 6 digitalWrite(IN3, LOW ;) 6 6 digitalWrite(IN4, LOW ;) 6 6 Serial print. ("stop="); 6 6 //// Quay phải
6 6 analogWrite(speedLeftPin, 150); 6 6 analogWrite(speedRightPin, 150); 6 6 digitalWrite(IN1, LOW ;) 6 6 digitalWrite(IN2, HIGH ;) 6 6 digitalWrite(IN3, LOW ;)
6 6 digitalWrite(IN4, LOW ; 6 6) 6 6 delay(1000);
6 6 // Dừng xe
6 6 analogWrite(speedLeftPin, ;0) 6 6 analogWrite(speedRightPin, ;0) 6 6 digitalWrite(IN1, LOW ;) 6 6 digitalWrite(IN2, LOW ;) 6 6 digitalWrite(IN3, LOW ;) 6 6 digitalWrite(IN4, LOW ;)
6 6 //1-Duyệt qua góc từ 120 độ về 10 độ với bước 1 độ 6 for goc = ( 120; goc >= 10; goc-- ) { 6 6// Đặt góc của servo 6 6 servoLeft write. (goc); 6 6 6 6 6 6
6 6 delay(20); // Chờ 20 milliseconds (0.02 giây) 6 6}
6 delay(1000); // Chờ 1 giây ở góc 0 độ //2- Duyệt qua góc từ 0 đến 60 độ với bước 1 độ
6 for goc = ; goc <= ( 65 120; goc++ ) { // Đặt góc của servo 6 6 6 6 servoTay write. (goc);
6 6 delay(20);// Chờ 20 milliseconds (0.02 giây) 6 6 6}
6 delay(1000); // Chờ 1 giây ở góc 60 độ 6 //3- Duyệt qua góc từ 0 đến 60 độ với bước 1 độ 6 for goc = ; goc <= ( 30 120; goc++ ) { // Đặt góc của servo 6 6 6 6 servoLeft write. (goc); 6 6 6 6
6 6 delay(20);// Chờ 20 milliseconds (0.02 giây) 6 6 6}
6 delay(1000); // Chờ 1 giây ở góc 60 độ 6 //// Quay trái
6 6 analogWrite(speedLeftPin, 150); 6 6 analogWrite(speedRightPin, 150); 6 6 digitalWrite(IN1, LOW ;) 6 6 digitalWrite(IN2, LOW ;) 6 6 digitalWrite(IN3, HIGH ;) 6 6 digitalWrite(IN4, LOW ; 6 6) 6 6 delay(1000);
6 6 } 6 } 6 } }