Tải bản đầy đủ - 0 (trang)
CHƯƠNG TRÌNH ĐIỀU KHIỂN ROBOT QUA BLUETOOTH

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

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

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



Mobile Robot

UTC



10



Cơ điện tử K56 -



{

bamtrai();

}

else if(blue== '8')

{

bamphai();

}

else if((blue == '5') || (blue == '6') )

{

dung_lai();

}

else if ((blue == '2') || (blue == '6'))

{

re_trai();

}

else if ((blue == '3')|| (blue == '6'))

{

re_phai();

}

else if ((blue == '4') || (blue == '6'))

{

lui();

}

else if ((blue == '1')|| (blue == '6') )

{

di_thang();

}

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot



11



Cơ điện tử K56 - UTC



}

void dung_lai()

{

digitalWrite(IN1, LOW);

digitalWrite(IN2, LOW);

digitalWrite(IN3, LOW);

digitalWrite(IN4, LOW);

analogWrite(ENA, 0);

analogWrite(ENB, 0);

}

void di_thang()

{

digitalWrite(IN1, HIGH);//xuất tín hiệu ra chân IN1 ở mức cao

digitalWrite(IN2, LOW);////xuất tín hiệu ra chân IN2 ở mức thấp

digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

analogWrite(ENA, 80);//phát xung PWM trên ENA

analogWrite(ENB, 80);//phát xung PWM trên ENB

}

void lui()

{

digitalWrite(IN1, LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3, LOW);

digitalWrite(IN4, HIGH);

analogWrite(ENA, 80);

analogWrite(ENB, 80);

}

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



12



Cơ điện tử K56 -



void re_trai()

{

digitalWrite(IN1, HIGH);

digitalWrite(IN2, LOW);

digitalWrite(IN3, LOW);

digitalWrite(IN4, HIGH);

analogWrite(ENA, 80);

analogWrite(ENB, 80);

}

void re_phai()

{

digitalWrite(IN1, LOW);

digitalWrite(IN2, HIGH);

digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

analogWrite(ENA, 80);

analogWrite(ENB, 80);

}

void bamtrai()

{

if(b7==LOW)

{

digitalWrite(IN1, LOW);

digitalWrite(IN2, LOW);

digitalWrite(IN3, LOW);

digitalWrite(IN4, LOW);

analogWrite(ENA, 0);

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



13



Cơ điện tử K56 -



analogWrite(ENB, 0);

}

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

b5==LOW && b6==LOW)//// di thang

{

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)//// lech phai 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)//// lech phai 2

{

digitalWrite(IN1, HIGH);

digitalWrite(IN2, LOW);

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



14



Cơ điện tử K56 -



digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

analogWrite(ENB, 0);

analogWrite(ENA, 100);

}

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

b5==LOW && b6==LOW)//// lech phai 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 trai 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 trai 2

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



15



Cơ điện tử K56 -



{

digitalWrite(IN1, HIGH);

digitalWrite(IN2, LOW);

digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

analogWrite(ENB, 0);

analogWrite(ENA, 100);

}

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

b5==HIGH && b6==LOW)//// lech trai 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)//// lech trai 2

{

digitalWrite(IN1, HIGH);

digitalWrite(IN2, LOW);

digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

analogWrite(ENB, 100);

analogWrite(ENA, 0);

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



16



Cơ điện tử K56 -



}

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

digitalWrite(IN4, LOW);

analogWrite(ENB, 100);

analogWrite(ENA, 30);

}

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

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

{

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

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



17



Cơ điện tử K56 -



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

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

}

}

void bamphai()

{

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



18



Cơ điện tử K56 -



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

{

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

SVTH: Phạm Quang Huy

Dương

Nguyễn Mạnh Linh



GVHD: CN. Trịnh Tuấn



Mobile Robot

UTC



19



Cơ điện tử K56 -



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

}

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

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 ĐIỀU KHIỂN ROBOT QUA BLUETOOTH

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

×