robot.ino (Arduino R3)

#include <Servo.h>
Servo myservo;
int pos=0;
const int trig=10;
const int echo=9;
int tien1=7;
int tien2=5;
int lui1=6;
int lui2=4;
void dithang();
void disangtrai(int goc);
void disangphai(int goc);
void diluitrai(int goc);
void diluiphai(int goc);
void dilui();
void diquaydau();
void dunglai();
int dokhoangcach();
void setup()
{
    myservo.attach(3);
    pinMode(trig,OUTPUT);
    pinMode(echo,INPUT);
    pinMode(tien1,OUTPUT);
    pinMode(tien2,OUTPUT);
    pinMode(lui1,OUTPUT);
    pinMode(lui2,OUTPUT);
    digitalWrite(tien1,LOW);
    digitalWrite(tien2,LOW);
    digitalWrite(lui1,LOW);
    digitalWrite(lui2,LOW);
    myservo.write(90);
    delay(1000);
    int kcmaxt=0,kcmidt=0;
    while(1)
    {
        int khoangcach,gd,i,gdmax=0,gdmid=0,gdmin=0,kcmax=0,kc810[21],kc90=500;
        for(gd=90;gd>=15;gd--)
        {
            myservo.write(gd);
            delay(10);
        }
        for(gd=15;gd<=165;gd++)
        {
            myservo.write(gd);
            delay(10);
            khoangcach=dokhoangcach();
            if(khoangcach<10)
                gdmin=gd;
            else
            {
                if(khoangcach<30)
                    gdmid=gd;
                else
                {
                    if(khoangcach>kcmax)
                    {
                        kcmax=khoangcach;
                        gdmax=gd;
                    }
                }
            }
            if(gd>=80&&gd<=100)
            {
                kc810[gd-80]=dokhoangcach();
                if(kc810[gd-80]==0)
                    kc810[gd-80]=500;
                else
                {
                    if(kc810[gd-80]<kc90)
                        kc90=kc810[gd-80];
                }
            }
        }
        for(gd=165;gd>=90;gd--)
        {
            myservo.write(gd);
            delay(10);
        }
        while(kc90>50&&gdmid<15&&gdmin<15)
        {
            if(kc90==kcmidt)
                dilui();
            else
            {
                dithang();
                kc90=dokhoangcach();
            }
        }
        dunglai();
        if(gdmin==0)
        {
            if(gdmid==0)
            {
                if(gdmax<80)
                    disangphai(gdmax);
                else
                    disangtrai(gdmax);
            }
            else
            {
                if(gdmid<80)
                {
                    if(gdmax<80)
                        diquaydau();
                    else
                        disangtrai(gdmax);
                }
                else
                {
                    if(gdmid>100)
                    {
                        if(gdmax<80)
                            disangphai(gdmax);
                        else
                            diquaydau();
                    }
                    else
                    {
                        if(gdmax<80)
                            disangphai(gdmax);
                        else
                        {
                            if(gdmax>100)
                                disangtrai(gdmax);
                            else
                                diquaydau();
                        }
                    }
                }
            }
        }
        else
        {
            if(gdmin<80)
                diluiphai(gdmin);
            else
            {
                if(gdmin>100)
                    diluitrai(gdmin);
                else
                    dilui();
            }
        }
        i=kcmax/10;
        if(i==kcmaxt)
            dilui();
        else
            kcmaxt=i;
    }
}
void loop()
{
}
void dithang()
{
    digitalWrite(tien1,HIGH);
    digitalWrite(tien2,HIGH);
    analogWrite(tien2,230);
}
void disangtrai(int goc)
{
    int temp=abs(goc-90)*3;
    digitalWrite(tien2,HIGH);
    delay(temp);
    digitalWrite(tien2,LOW);
}
void disangphai(int goc)
{
    int temp=abs(goc-90)*3;
    digitalWrite(tien1,HIGH);
    delay(temp);
    digitalWrite(tien1,LOW);
}
void diluitrai(int goc)
{
    int temp=abs(goc-90)*5;
    digitalWrite(lui1,HIGH);
    delay(temp);
    digitalWrite(lui1,LOW);
}
void diluiphai(int goc)
{
    int temp=abs(goc-90)*5;
    digitalWrite(lui2,HIGH);
    delay(temp);
    digitalWrite(lui2,LOW);
}
void dilui()
{
    digitalWrite(lui1,HIGH);
    digitalWrite(lui2,HIGH);
    delay(500);
    digitalWrite(lui1,LOW);
    digitalWrite(lui2,LOW);
}
void diquaydau()
{
    digitalWrite(tien1,HIGH);
    digitalWrite(lui2,HIGH);
    delay(300);
    digitalWrite(tien1,LOW);
    digitalWrite(lui2,LOW);
}
void dunglai()
{
    digitalWrite(tien1,LOW);
    digitalWrite(tien2,LOW);
    digitalWrite(lui1,LOW);
    digitalWrite(lui2,LOW);
}
int dokhoangcach()
{
    long tg;
    int kc;
    digitalWrite(trig,LOW);
    delayMicroseconds(2);
    digitalWrite(trig,HIGH);
    delayMicroseconds(10);
    digitalWrite(trig,LOW);
    tg=pulseIn(echo,HIGH,3600000);
    kc=int(tg/2/29.412);
    return kc;
}