找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 1830|回复: 1
收起左侧

循迹蔽障定点停车Arduino程序

[复制链接]
ID:490217 发表于 2019-3-13 20:46 | 显示全部楼层 |阅读模式
#include <Servo.h>  //servo library
Servo myservo;      // create servo object to control servo

int Echo = A4;  
int Trig = A5;

#define LT_R !digitalRead(10)
#define LT_M !digitalRead(4)
#define LT_L !digitalRead(2)
#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
#define carSpeed 150
int rightDistance = 0, leftDistance = 0, middleDistance = 0;

void forward(){
  analogWrite(ENA, 150);
  analogWrite(ENB, 150);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("Forward");
}

void back() {
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Back");
}

void left() {
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  //analogWrite(ENA, 300);
  //analogWrite(ENB, 300);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("Left");
}

void right() {
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  //analogWrite(ENA, 300);
// analogWrite(ENB, 300);
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  Serial.println("Right");
}

void stop() {
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  Serial.println("Stop!");
}
void sright(){
  analogWrite(ENA, 250);
  analogWrite(ENB, 250);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("go right!");
}
void sleft(){
  analogWrite(ENA, 250);
  analogWrite(ENB, 250);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("go right!");
}

//Ultrasonic distance measurement Sub function
int Distance_test() {
  digitalWrite(Trig, LOW);   
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);   
  float Fdistance = pulseIn(Echo, HIGH);  
  Fdistance= Fdistance / 58;      
  return (int)Fdistance;
}  



void longtracking()
{
            for(int i=0;i<125;i++)
  {
    forward();
    delay(5);
    if(LT_L)
    {
      sleft();
      while(LT_L);
    }
    else if(LT_R)
    {
       sright();
      while(LT_R);
    }
    if(LT_L&<_R&<_M)
    {
      forward();
      delay(40);
      break;
    }

  }
}
void turnleft()
{

}

void longtracking2()
{
            for(int i=0;i<130;i++)
  {
    forward();
    delay(5);
    if(LT_L)
    {
      sleft();
      while(LT_L);
    }
    else if(LT_R)
    {
       sright();
      while(LT_R);
    }
    if(LT_L&<_R&<_M)
    {
      forward();
      delay(40);
      break;
    }
  }
}

void shorttracking()
{
  for(int i=0;i<83;i++)
  {
    forward();
    delay(5);
    if(LT_L)
    {
      sleft();
      while(LT_L);
    }
    else if(LT_R)
    {
       sright();
      while(LT_R);
    }
    if(LT_R&<_L&<_M)
    {
      forward();
      delay(40);
      break;
    }
  }
}


void setup() {
  myservo.attach(3);  // attach servo on pin 3 to servo object
  Serial.begin(9600);     
  pinMode(Echo, INPUT);   
  pinMode(Trig, OUTPUT);  
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  stop();
}


int x=2;
int y=0;
int dx=1;
int dy=3;
int fuck=0;


void loop()
{
  if(fuck==0)
  {
        forward();
        while(!LT_L||!LT_R||!LT_M)
        {
          if(LT_R&<_L&<_M)
          {
            break;
          }
        }
        delay(60);
        stop();
        fuck++;
  }


  if(x==1&&y!=3)                      //中间点
  {
    delay(100);
    middleDistance = Distance_test();


    if(middleDistance<25)
    {
          stop();
          delay(300);



          right();
          delay(100);
          while(!LT_R);
          stop();
          delay(300);




          if(y==0)
          {
shorttracking();
             stop();
          delay(300);


          left();
          delay(100);
          while(!LT_L);
          stop();
          delay(300);




          x++;
    delay(100);
    middleDistance = Distance_test();

          if(middleDistance<25)
          {

              left();
                        delay(100);
          while(!LT_L);
          stop();
          delay(300);

              stop();
              delay(200);
shorttracking();
              stop();


              delay(200);
shorttracking();
              stop();
              delay(200);


          right();
          delay(100);
          while(!LT_R);
          stop();
          delay(300);


              x--;
              x--;


          }
        /*  else
          {



            stop();
            delay(200);
            longtracking();
            stop();
            delay(200);


          }*/
          }
          else
          {
            shorttracking();            //此处短途迅即
             stop();
             delay(300);


             left();
             delay(100);
          while(!LT_L);
          stop();
          delay(300);




             x++;
    delay(100);
    middleDistance = Distance_test();
             if(middleDistance<25)
          {


              left();
              delay(100);
              while(!LT_L);
              stop();
              delay(200);
              shorttracking();
              stop();
              delay(200);
              shorttracking();
              stop();
              delay(200);
              x--;
              x--;

                        right();
          delay(100);
          while(!LT_R);
          stop();
          delay(300);


          }

          }
    }   
    else
    {
      stop();
      delay(200);
      longtracking2();
      stop();
      delay(300);
      y++;
    }
  }



  else if(x==0&&y!=3)
  {
    delay(100);
    middleDistance = Distance_test();


    if(middleDistance<25)
    {
          stop();
          delay(300);



          right();
          delay(100);
          while(!LT_R);
          stop();
          delay(300);



          if(y==0)
          {
            forward();
            delay(520);
             stop();
          delay(300);



          left();
          delay(100);
          while(!LT_L);
          stop();
          delay(300);



          x++;
          }
          else
          {
            shorttracking();            //此处短途迅即
             stop();
             delay(300);



          left();
          delay(100);
          while(!LT_L);
          stop();
          delay(300);


             stop();
             delay(300);
             x++;
          }
    }   
    else
    {
      stop();
      delay(200);
      longtracking();                  //zhongjianxunji
      stop();
      delay(300);
      y++;
    }  
  }




  else if(x==2&&y!=3)
  {


                delay(300);
            middleDistance = Distance_test();
            if(middleDistance<25)
            {
              stop();
              delay(300);


              if(y==0)
              {
              left();
              delay(100);
              while(!LT_L);
              stop();
              delay(300);



              forward();
              delay(520);


              right();
              delay(100);
              while(!LT_R);
              stop();
              delay(300);


              x--;              
              }
              else
              {
                              left();
              delay(100);
              while(!LT_L);
              stop();
              delay(300);


              shorttracking();
              right();
              delay(100);
              while(!LT_R);
              stop();
              delay(300);
              x--;
              }


            }
            else
            {



      stop();
      delay(200);
      longtracking();                 
      stop();
      delay(300);
      y++;



            }
  }


        if(y==3)
      {
        int d;
        d=dx-x;
        if(d==0)
        {
          stop();
          delay(300);
          forward();
          delay(400);
                    stop();
          delay(1000000);
        }
        else if(d<0)
        {
           stop();
          delay(300);
          left();
          delay(350);
          forward();
          delay(600);
           stop();
            delay(300);
           if(d==-2)
           {

          forward();
          delay(600);
           stop();
          delay(300);
           }

          right();
          delay(350);
          forward();
          delay(400);
          stop();
          delay(100000);
        }
        else if(d>0)
        {
           stop();
          delay(300);
          right();
          delay(353);
          forward();
          delay(600);
           stop();
          delay(300);

          if(d==2)
          {
                     forward();
          delay(600);
          stop();
          delay(300);
          }

          left();
          delay(350);
          stop();
          delay(300);
          forward();
          delay(400);
          stop();
          delay(10000000);
        }
      }
}






回复

使用道具 举报

ID:1 发表于 2019-3-13 21:13 | 显示全部楼层
楼主能分享下原理图吗?
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表