arduino

#include <Servo.h>
#define trig 3 //chân trig của HC-SR04
#define echo 4//chân echo của HC-SR04

Servo srf05; // create servo object to control a servo
#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 SERVO_PIN 10

Servo gServo;

void setup()
{
    gServo.attach(SERVO_PIN);
}

void loop()
{
    gServo.write(0);
    delay(1000);

    gServo.write(90);
    delay(1000);

    gServo.write(180);
    delay(1000);



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(trig,OUTPUT);//chân trig sẽ phát tín hiệu
pinMode(echo,INPUT);//chân echo sẽ nhận tín hiệu
Serial.begin(9600);
srf05.attach(5); // attaches the servo on pin 9 to the servo object
}

void loop()
{
objectAvoiderChooseWay ( inA1 , inA2, inB1, inB2, 50, 700);

digitalWrite(6, HIGH);
digitalWrite(7, LOW);
digitalWrite(8, HIGH);
digitalWrite(9,LOW);
}

int objectDistance_cm (byte angle)
{
srf05.write(angle);
delay(500);
unsigned long duration; //biến đo thời gian
int distance; //biến lưu khoảng cách /* phát xung từ chân trig */

digitalWrite(trig,0); //tắt chân trig
delayMicroseconds(2);
digitalWrite(trig,1); // phát xung từ chân trig
delayMicroseconds(5);// xung có độ dài 5 microSeconds
digitalWrite(trig,0);//tắt chân trig /* tính toán thời gian */

duration = pulseIn(echo,HIGH);//đo độ rộng xung HIGH ở chân echo.
distance = int(duration/2/29.412);//tính khoảng cách đến vật.
return distance;
}

void motorControlNoSpeed (byte in1,byte in2, byte 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:
}
}

void robotMover (byte inR1, byte inR2, byte inL1, byte inL2, byte action)
{

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, 1);
motorControlNoSpeed(inL1, inL2, 2);
break;
case 4:// quay phải
motorControlNoSpeed(inR1, inR2, 2);
motorControlNoSpeed(inL1, inL2, 1);
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;
case 7:// rẽ lùi trái
motorControlNoSpeed(inR1, inR2, 2);
motorControlNoSpeed(inL1, inL2, 0);
break;
case 8:// rẽ lùi phải
motorControlNoSpeed(inR1, inR2, 0);
motorControlNoSpeed(inL1, inL2, 2);
break;
default:
action = 0;

}
}

void objectAvoiderChooseWay (byte inR1, byte inR2, byte inL1, byte inL2, byte allow_distance, int turn_back_time)
{

robotMover(inR1,inR2,inL1,inL2,1);

int front_distance = objectDistance_cm (90);
int left_distance;
int right_distance;
int max_distance;
if (front_distance > allow_distance)
{
robotMover(inR1,inR2,inL1,inL2,1);

delay(10);
}
if (front_distance <= allow_distance)
{
robotMover(inR1,inR2,inL1,inL2,2);

delay(300);
robotMover(inR1,inR2,inL1,inL2,0);
left_distance = objectDistance_cm (180); //đo khoảng cách bên trái

right_distance = objectDistance_cm (0); //đo khoảng cách bên phải

max_distance = max(left_distance,right_distance);// so sánh giá trị lớn nhất với khoảng cách bên phải (gán bằng cái lớn nhất)
if (max_distance==left_distance)
{
robotMover(inR1,inR2,inL1,inL2,3);

delay (turn_back_time/2); // Nếu bên trái là khoảng cách lớn nhất thì rẽ trái
}
else
{
if (max_distance==right_distance)
{
robotMover(inR1,inR2,inL1,inL2,4);// Nếu bên phải có khoảng cách lớn nhất thì rẽ phải

delay (turn_back_time/2);
}
}
}
}