三个超声波传感器避障小车

5400

const int ENa =5;
const int ENb =6;
const int in1 =4;
const int in2 =7;

const int TrigPin1 = 2;//接板子的2脚
const int EchoPin1 = 3;//接板子的3脚
const int TrigPin2 = 8;//接板子的2脚
const int EchoPin2 = 9;
const int TrigPin3 = 10;//接板子的2脚
const int EchoPin3 = 11;

float distance1;
float distanceL;
float distanceR;//定义精度浮点型数据  后面超声波模块算的的距离存放点
//const int led =13;


void motosp(int sp1,int sp2)//电机速度控制函数。括号内分别为左右电机速度值，
{
  if(sp1>0)                 //范围-255～+255，正值为正转，负值为反转。
  {
   digitalWrite(in1, HIGH);   
  }
  else
  {
   digitalWrite(in1, LOW);
  }
  if(sp2>0)
  {
   digitalWrite(in2, HIGH);
  }
  else
  {
   digitalWrite(in2, LOW);
  }
  analogWrite(ENa,abs (sp1));
  analogWrite(ENb,abs (sp2));
}

void setup ()
{
    for(int i=4;i<7;i++)
    pinMode(i,OUTPUT);//管脚初始化 4到7脚 为输出模式
     
    Serial.begin(9600);//设置波特率
    pinMode(TrigPin1, OUTPUT);
    pinMode(EchoPin1, INPUT);
    pinMode(TrigPin2, OUTPUT);
    pinMode(EchoPin2, INPUT);
    pinMode(TrigPin3, OUTPUT);
    pinMode(EchoPin3, INPUT);
    Serial.println("Ultrasonic sensor:");//在和串口连接好的时候 在超声波工作之前
                                         //将Ultrasonic sensor:显示到串口工具上
   
}
void loop()
{     
                    motosp(200,200);//先前进一点
       /**************启动超声波模块 并计算出距离（cm）************/                 
                    digitalWrite(TrigPin1, LOW);//发一个10ms的高脉冲去触发TrigPin  
                    delayMicroseconds(2);
                    digitalWrite(TrigPin1, HIGH);
                    delayMicroseconds(10);
                    digitalWrite(TrigPin1, LOW);
                  
                    distance1 = pulseIn(EchoPin1, HIGH) / 58.0; //算成厘米
                    distance1 = (int(distance1* 100.0)) / 100.0; //保留两位小数
                    Serial.print(distance1); //将算的距离显示到串口工具上
                    Serial.print("cm");     //输出cm显示出去
                    Serial.println();
                    
                     digitalWrite(TrigPin2, LOW);//发一个10ms的高脉冲去触发TrigPin  
                    delayMicroseconds(2);
                    digitalWrite(TrigPin2, HIGH);
                    delayMicroseconds(10);
                    digitalWrite(TrigPin2, LOW);
                  
                    distanceL = pulseIn(EchoPin2, HIGH) / 58.0; //算成厘米
                    distanceL = (int(distanceL* 100.0)) / 100.0; //保留两位小数
                    Serial.print(distanceL); //将算的距离显示到串口工具上
                    Serial.print("cm");     //输出cm显示出去
                    Serial.println();
                    
                     digitalWrite(TrigPin3, LOW);//发一个10ms的高脉冲去触发TrigPin  
                    delayMicroseconds(2);
                    digitalWrite(TrigPin3, HIGH);
                    delayMicroseconds(10);
                    digitalWrite(TrigPin3, LOW);
                  
                    distanceR = pulseIn(EchoPin3, HIGH) / 58.0; //算成厘米
                    distanceR = (int(distanceR* 100.0)) / 100.0; //保留两位小数
                    Serial.print(distanceR); //将算的距离显示到串口工具上
                    Serial.print("cm");     //输出cm显示出去
                    Serial.println();
            /***************************************************************/
      
          if ((distance1<20)&&(distanceL<20)&&(distanceR>20)){
            motosp(-200,200);
              delay(500);
              motosp(200,200);
          }
          else if ((distance1<20)&&(distanceL>20)&&(distanceR<20)){
            motosp(200,-200);
              delay(500);
              motosp(200,200);
          }
          else if ((distance1<20)&&(distanceL<20)&&(distanceR<20)){
            motosp(-200,-200);
              delay(500);
              motosp(200,200);
          }
         
         else if ((distance1>20.1)&&(distance1<150))//速度在20 到150 之间，将会减慢速度
             motosp(125,125);
            else if ((distance1>150.1)&&(distance1<200))// 速度加大点
                motosp(200,200);
               else if (distance1>200)//速度为最大
                     motosp(255,255);
       }
其中有传感器无法测距，出现一轮正转，一轮反转的情况，求大神看看代码