巡线 避障

为什么巡线小车加上超声波以后巡不了线了
 

#include <i2cmaster.h>//颜色传感器的控制库
int Left_motor_go=8;     //左电机前进(IN1)
int Left_motor_back=9;     //左电机后退(IN2)
int Right_motor_go=10;    // 右电机前进(IN3)
int Right_motor_back=11;    // 右电机后退(IN4)
int PWML=5;
int PWMR=6;
const int SensorRight = 3;     //右循迹红外传感器(P3.2 OUT1)
const int SensorLeft = 4;       //左循迹红外传感器(P3.3 OUT2)
int SL;    //左循迹红外传感器状态
int SR;    //右循迹红外传感器状态
int trig=A0;//超声波的输出
int echo=A1;//超声波的输入
float v=331.5*0.6*20;//距离比值的一个算法
int A=30;//语音播报的接口，下同。
int B=31;
//颜色传感器的定义
#define uint16_t unsigned int
unsigned char Re_buf;
unsigned char sign=0;
byte color=0,rgb_data[3]={0};
int val=0;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  //初始化电机驱动IO为输出方式
  pinMode(Left_motor_go,OUTPUT); // PIN 8 (PWM)
  pinMode(Left_motor_back,OUTPUT); // PIN 9 (PWM)
  pinMode(Right_motor_go,OUTPUT);// PIN 10 (PWM)
  pinMode(Right_motor_back,OUTPUT);// PIN 11 (PWM)
  pinMode(SensorRight, INPUT); //定义右循迹红外传感器为输入
  pinMode(SensorLeft, INPUT); //定义左循迹红外传感器为输入
  pinMode(trig,OUTPUT);//超声波输出
  pinMode(echo,INPUT);//超声波输入
  pinMode(A,OUTPUT);//语音播报模块的输出，下同。
  pinMode(B,OUTPUT);
 
  //颜色传感器
   i2c_init();
   delay(1);
 
}
void Run()
{
  digitalWrite(Right_motor_go,HIGH);  // 右电机前进
  digitalWrite(Right_motor_back,LOW);
  analogWrite(PWMR,90);
  digitalWrite(Left_motor_go,LOW);  // 左电机前进
  digitalWrite(Left_motor_back,HIGH);
  analogWrite(PWML,90);
}
void brake()
{
  digitalWrite(Right_motor_go,LOW);
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);
  digitalWrite(Left_motor_back,LOW);
}
void  left()
{
  digitalWrite(Right_motor_go,HIGH);  // 右电机前进
  digitalWrite(Right_motor_back,LOW);
  analogWrite(PWMR,100);
  digitalWrite(Left_motor_go,HIGH);   //左轮后退
  digitalWrite(Left_motor_back,LOW);
  analogWrite(PWML,90);
}
void  right()
{
  digitalWrite(Right_motor_go,LOW);  // 右电机前进
  digitalWrite(Right_motor_back,HIGH);
  analogWrite(PWMR,90);
  digitalWrite(Left_motor_go,LOW);   //左轮后退
  digitalWrite(Left_motor_back,HIGH);
  analogWrite(PWML,100);
}
 void RUNING()
 {
    digitalWrite(Right_motor_go,HIGH);  // 右电机前进
  digitalWrite(Right_motor_back,LOW);
  analogWrite(PWMR,200);
  digitalWrite(Left_motor_go,LOW);  // 左电机前进
  digitalWrite(Left_motor_back,HIGH);
  analogWrite(PWML,200);
 }
//超声波占空比的拉高拉低函数
float distanceM()
{
  digitalWrite(trig,LOW);
  delayMicroseconds(10);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  float tus=pulseIn(echo,HIGH);
  float t=tus/1000.0/1000.0/2;
  float d=t*v;
  return d*100;
}
//颜色传感器的检测函数
void iic_read(unsigned char add,unsigned char *data,unsigned char len)
{
  i2c_start_wait(0xb4);
   i2c_write(add);
   i2c_start_wait(0xb5);
   while(len-1)
   {
    *data++=i2c_readAck();
    len--;
    }
    *data=i2c_readNak();
    i2c_stop();
}
void loop() {
  // put your main code here, to run repeatedly:
 while(1)
  {
   
        //超声波
     float d=distanceM();//检测超声波的是
   Serial.println(d,DEC);//打印超声波的值
//颜色传感器
 unsigned char data[9]={0};
 if(!sign)
 {
   iic_read(0x00,data,8);
    iic_read(0x0c,data,3);
    rgb_data[0]=data[0];
    rgb_data[1]=data[1];
    rgb_data[2]=data[2];
//   Serial.print("r:");
//    Serial.print( rgb_data[0]);
//    Serial.print(",g:");
//    Serial.print( rgb_data[1]);
//    Serial.print(",b:");
//    Serial.println( rgb_data[2]);  
 }
 
  //有信号为LOW  没有信号为HIGH   检测到黑线  输出高  检测到白色区域输出低
  SR = digitalRead(SensorRight);//有信号表明在白色区域，车子底板上L1亮；没信号表明压在黑线上，车子底板上L1灭
  SL = digitalRead(SensorLeft);//有信号表明在白色区域，车子底板上L2亮；没信号表明压在黑线上，车子底板上L2灭
//Serial.println(SR);
//Serial.println(SL);
 
 //Serial.println(val);
 
 if(d<400&&d>1)
 {
 brake();
 digitalWrite(A,HIGH);
 digitalWrite(B,LOW);
 }
 else if(val>20)
 {
  RUNING();
      digitalWrite(A,LOW);
      digitalWrite(B,HIGH);
  delay(100);
   val=0;
  }
 else if( rgb_data[0]>120&& rgb_data[1]>120&& rgb_data[2]>120)
 {
 brake();
   digitalWrite(A,LOW);
 digitalWrite(B,HIGH);
 val++;
 }
 
 else if (SL == LOW&&SR==LOW)
  {
    Run();   //调用前进函数
    digitalWrite(A,LOW);
    digitalWrite(B,LOW);
  }
  else if (SL== HIGH & SR == LOW)// 左循迹红外传感器,检测到信号，车子向右偏离轨道，向左转
 {
    left();
 digitalWrite(A,LOW);
 digitalWrite(B,LOW);
 }
  else if (SR == HIGH & SL == LOW) // 右循迹红外传感器,检测到信号，车子向左偏离轨道，向右转 
   {
    right();
        digitalWrite(A,LOW);
        digitalWrite(B,LOW);
   }
  else // 都是黑色, 停止
  {
  brake();
     digitalWrite(A,LOW);
     digitalWrite(B,LOW);
  }
  }
}