Trang chủ —› Chia sẻ kiến thức —› Chế tạo robot dò đường (có kèm code mẫu)

Chế tạo robot dò đường (có kèm code mẫu)

4 Tháng Hai,2018 Đăng bởi: admin

 

Chào các bạn, bài viết này mình sẽ lưu trữ code mẫu dành cho robot của mình hoạt động trong chế độ dò đường có giảm tốc cho đỡ văng lung tung.

Code ở dưới nha
Giảm 40% khóa chế tạo robot dò đường: Link
Các bạn có thể tham khảo combo full ở đây

Arduino Uno R3https://goo.gl/kyiDgn
Module L298goo.gl/62ZhqQ
1 Module cảm biến hồng ngoại dò đường V2 :goo.gl/BfLaEy
Cáp 7 màu đực cáihttps://goo.gl/RWSmBp
Cáp 7 màu đực đựchttps://goo.gl/KRxyjQ
Pin Ultrafiregoo.gl/BB1xMn
Đế pin Unltrafire (đôi) https://goo.gl/asLztE
1 jack nguồn cho aruduino gắn sẵn dâyhttps://goo.gl/Pp6Kzt
1 chốt nối dây điện https://goo.gl/1u5HHj
Combo xe 3 bánh (hàn sẵn động cơ): https://goo.gl/rRsfbg
mini breadboardhttps://goo.gl/krhVi4
3 Led đơn
1 Sạc pin đơn
https://goo.gl/n2Gg9V
2 dây thít https://goo.gl/6s6RKu

Học lập trình ở đâu

Code mẫu robot dò đường
/* Code by: ANH ROBOT
* Mua linh kiện: linhkienrobotics.com
* Học chế tạo robot: hoclamrobot.com
*/
#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
const byte enA=3;
const byte enB=5;
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(enA,OUTPUT);
pinMode(enB,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
analogWrite(enA,100);
analogWrite(enB,100);
}

void loop() {
darkLineFollower (inA1, inA2, inB1, inB2, linesens1, linesens2, linesens3, linesens4);
//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);// quay trá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
*/

}


Notice: WP_Query được gọi với một tham số đã bị loại bỏ kể từ phiên bản 3.1.0! Hãy sử dụng ignore_sticky_posts thay cho caller_get_posts (sẽ sớm bị loại bỏ) in /home/lapt0260/public_html/wp-includes/functions.php on line 4045

Trả lời

Email của bạn sẽ không được hiển thị công khai. Các trường bắt buộc được đánh dấu *