Tải bản đầy đủ - 0 (trang)
CHƯƠNG TRÌNH CHO ROBOT TỰ ĐỘNG DÒ MÊ CUNG

CHƯƠNG TRÌNH CHO ROBOT TỰ ĐỘNG DÒ MÊ CUNG

Tải bản đầy đủ - 0trang

Mobile Robot



2



Cơ điện tử K56 - UTC



pinMode(in1, OUTPUT);

pinMode(in2, OUTPUT);

pinMode(in3, OUTPUT);

pinMode(in4, OUTPUT);

pinMode(enA, OUTPUT);

pinMode(enB, OUTPUT);

}

//////////////////////////////////////////////////////////////////////////////////////

void loop()

{

int b1= digitalRead (cb1);

int b2= digitalRead (cb2);

int b3= digitalRead (cb3);

int b4= digitalRead (cb4);

int b5= digitalRead (cb5);

int b6= digitalRead (cb6);

int b7= digitalRead (cb7);

if (b7==LOW)

{

digitalWrite (in1, LOW);

digitalWrite (in2, LOW);

digitalWrite (in3, LOW);

digitalWrite (in4, LOW);

analogWrite (enA, 0);

analogWrite (enB, 0);

}

if (b1==LOW && b2==LOW && b3==HIGH && b4==HIGH &&

b5==LOW && b6==LOW) //// Đi thẳng

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



3



Cơ điện tử K56 - UTC



{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enA, 100);

analogWrite (enB, 100);

}

if (b1==LOW && b2==HIGH && b3==HIGH && b4==LOW &&

b5==LOW && b6==LOW)//// lệch phải 1

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 40);

analogWrite (enA, 100);

}

if (b1==HIGH && b2==HIGH && b3==LOW && b4==LOW &&

b5==LOW && b6==LOW) ////lệch phải 2

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 0);

analogWrite (enA, 100);

}

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



4



Cơ điện tử K56 - UTC



if (b1==LOW && b2==HIGH && b3==HIGH && b4==LOW &&

b5==LOW && b6==LOW) //// lệch phải 3

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 30);

analogWrite (enA, 100);

}

if (b1==HIGH && b2==HIGH && b3==HIGH && b4==LOW &&

b5==LOW && b6==LOW) //// quay trái 1

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 0);

analogWrite (enA, 100);

}

if (b1==HIGH && b2==HIGH && b3==HIGH && b4==HIGH &&

b5==LOW && b6==LOW)//// quay trái 2

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 0);

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



5



Cơ điện tử K56 - UTC



analogWrite (enA, 100);

}

if (b1==LOW && b2==LOW && b3==LOW && b4==HIGH &&

b5==HIGH && b6==LOW)//// lệch trái 1

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite(in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 100);

analogWrite (enA, 40);

}

if (b1==LOW && b2==LOW && b3==LOW && b4==LOW &&

b5==HIGH && b6==HIGH)//// lechtrai2

{

digitalWrite (in1, HIGH);

digitalWrite (in2 ,LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 100);

analogWrite (enA, 0);

}

if (b1==LOW && b2==LOW && b3==LOW && b4==HIGH &&

b5==LOW && b6==LOW)//// lech trai 3

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



6



Cơ điện tử K56 - UTC



digitalWrite (in4, LOW);

analogWrite (enB, 100);

analogWrite (enA, 30);

}

if (b1==LOW && b2==LOW && b3==HIGH && b4==HIGH &&

b5==HIGH && b6==HIGH)//// quay phai1

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 100);

analogWrite (enA, 0);

}

if (b1==LOW && b2==LOW && b3==LOW && b4==HIGH &&

b5==HIGH && b6==HIGH)//// quay phai 2

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 100);

analogWrite (enA, 0);

}

if (b1==LOW && b2==LOW && b3==LOW && b4==LOW &&

b5==LOW && b6==LOW)//// ngõ cụt quay đầu

{

digitalWrite (in1, LOW);

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



7



Cơ điện tử K56 - UTC



digitalWrite (in2, HIGH);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 70);

analogWrite (enA, 70);

}

if (b1==HIGH && b2==HIGH && b3==HIGH && b4==HIGH &&

b5==HIGH && b6==HIGH)//// ngã tư, ngã 3

{

digitalWrite (in1, HIGH);

digitalWrite (in2, LOW);

digitalWrite (in3, HIGH);

digitalWrite (in4, LOW);

analogWrite (enB, 0);

analogWrite (enA, 100);

}

}



SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



8



Cơ điện tử K56 - UTC



2. CHƯƠNG TRÌNH ĐIỀU KHIỂN ROBOT QUA BLUETOOTH

#include //khai báo thư viện đê cho tất cả các

chân của board arduino có thể giao tiếp serial

#define cb1 13

#define cb2 12

# define cb3 11

#define cb4 10

#define cb5 8

#define cb6 2

#define cb7 1

int b1, b2, b3, b4, b5, b6, b7;

int bluetoothTx = A0; // chân Tx của chân hc06 nối vs chân A0 của

arduino.

int bluetoothRx =A1; //chân Rx của chân hc06 nối vs chân A1 của

arduino.

SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);

int blue=0; // khai báo biến đọc tín hiệu gửi từ smartphone và lưu tín

hiệu đến

//khai báo chân tín hiệu bánh trái

#define ENA 9

#define IN1 7

#define IN2 6

//khai báo chân tín hiệu bánh phải

#define IN3 5

#define IN4 4

#define ENB 3

void setup() {

bluetooth.begin(9600); //tốc độ giao tiếp của arduino với hc06 là 9600

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



9



Cơ điện tử K56 - UTC



Serial.begin(9600); //tốc độ truyền của arduino với máy tính

pinMode(cb1, INPUT);

pinMode(cb2,INPUT);

pinMode(cb3, INPUT);

pinMode(cb4,INPUT);

pinMode(cb5, INPUT);

pinMode(cb6, INPUT);

pinMode(cb7, INPUT);

pinMode(IN1, OUTPUT);//thiết đặt chân IN1 là đầu ra

pinMode(IN2, OUTPUT);//thiết đặt chân IN2 là đầu ra

pinMode(IN3, OUTPUT);//thiết đặt chân IN3 là đầu ra

pinMode(IN4, OUTPUT);//thiết đặt chân IN4 là đầu ra

pinMode(ENA, OUTPUT);//thiết đặt chân ENA là đầu ra

pinMode(ENB, OUTPUT);//thiết đặt chân ENB là đầu ra

}

void loop() {

b1 = digitalRead(cb1);

b2 = digitalRead(cb2);

b3 = digitalRead(cb3);

b4 = digitalRead(cb4);

b5 = digitalRead(cb5);

b6 = digitalRead(cb6);

b7 = digitalRead(cb7);

if(bluetooth.available()>0) // xuất tín hiệu khi nhận được tín hiệu

{

blue = bluetooth.read();// đọc các giá trị nhận được từ smartphone

}

if(blue== '7')

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

CHƯƠNG TRÌNH CHO ROBOT TỰ ĐỘNG DÒ MÊ CUNG

Tải bản đầy đủ ngay(0 tr)

×