arduino小车跟随纸板程序

arduino智能小车，前方安装了两个超声波测距模块左右分布。通过这两个模块跟随前方纸板移动和转弯。

//小车跟随纸板的时候与纸板保持的距离（单位cm，如下：10cm。可以自己调节，更改下方数字即可）
#define dis_paper 10

//两个红外测距模块之间安装距离(单位mm，如下：50mm。可以自己调节，更改下方数字即可）
#define dis_ultr 50

//定义电机速度，范围（0~255），如果速度过快，可以降低该数值
#define speed_motor 200

//左边电机连线
#define motor_L1 12
#define motor_L2 13

#define motorL_s 3
//右边电机连线 方向：7，8 速度：5
#define motor_R1 7
#define motor_R2 8

#define motorR_s 5

//左边红外测距模块连线（信号S）
#define dis_L A0
//右边红外测距模块连线（信号S）
#define dis_R A1 //定义12号IO口连接Trig

//该函数可以测量超声波前方障碍物距离，并返回一个float类型的距离数据
//参数：dir.1：左边。2：右边。
float ceju(int dir){
  float cm=0,cm_sum=0;
  for(int i=0;i<10;i++)
  {
    if(dir==1)
    cm=analogRead(dis_L);
    else if(dir==2)
    cm=analogRead(dis_R);
    else;
    
    cm=((67870.0 / (cm - 3.0)) - 40.0)/10.0;//保留两位小数 
    cm_sum+=cm;
  }
  cm=cm_sum/10;

  return cm;
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600); //打开串口

  //初始化两个电机控制引脚
  //关闭电机,
  //控制引脚状态：1010为前进。0101为后退。1001为右转。0110为左转
  digitalWrite(motor_L1,LOW);
  digitalWrite(motor_L2,LOW);
  analogWrite(motorL_s,0);
  
  digitalWrite(motor_R1,LOW);
  digitalWrite(motor_R2,LOW);
  analogWrite(motorR_s,0);
  
}

void loop() {
  // put your main code here, to run repeatedly:
  float angle=0;
  float dis_l,dis_r;//定义两个变量存储左右两边超声波测量出来的障碍物距离。
  dis_l=ceju(1);
  dis_r=ceju(2);
  //如果左右两边超声波测出的距离都在0~180cm之间，则视为有效数据。
  //否则，视为无效数据，不予应答。
  if(dis_l>0&&dis_l<180&&dis_r>0&&dis_l<180)
  {
    Serial.print("L:");
    Serial.print(dis_l);
    Serial.print("R:");
    Serial.print(dis_r);
    Serial.print(";;;");
    if(dis_l>dis_r)
    {
      angle=atan((dis_l-dis_r)*10/dis_ultr);
      Serial.print("Angle:");
      Serial.print(angle);
      //如果角度大于5度，则响应
      while(angle>5)
      {
        digitalWrite(motor_L1,HIGH);
        digitalWrite(motor_L2,LOW);
        analogWrite(motorL_s,speed_motor);
        
        
      
        digitalWrite(motor_R1,LOW);
        digitalWrite(motor_R2,HIGH);
        analogWrite(motorR_s,speed_motor);

        while(!(dis_l>0&&dis_l<180&&dis_r>0&&dis_l<180))
        {
          dis_l=ceju(1);
          dis_r=ceju(2);
        }
        angle=atan((dis_l-dis_r)*10/dis_ultr);
        
      }
      Serial.print("Angle:");
      Serial.print(angle);
        digitalWrite(motor_L1,LOW);
        digitalWrite(motor_L2,LOW);
        analogWrite(motorL_s,0);
      
        digitalWrite(motor_R1,LOW);
        digitalWrite(motor_R2,LOW);
        analogWrite(motorR_s,0);
      
    }
    else
    {
      angle=atan((dis_r-dis_l)*10/dis_ultr);
      Serial.print("Angle:");
      Serial.print(angle);
      while(angle>5)
      {
        digitalWrite(motor_L1,LOW);
        digitalWrite(motor_L2,HIGH);
        analogWrite(motorL_s,speed_motor);
        
        
      
        digitalWrite(motor_R1,HIGH);
        digitalWrite(motor_R2,LOW);
        analogWrite(motorR_s,speed_motor);

        while(!(dis_l>0&&dis_l<180&&dis_r>0&&dis_l<180))
        {
          dis_l=ceju(1);
          dis_r=ceju(2);
        }
        angle=atan((dis_r-dis_l)*10/dis_ultr);
      }
      Serial.print("Angle:");
      Serial.print(angle);
        digitalWrite(motor_L1,LOW);
        digitalWrite(motor_L2,LOW);
        analogWrite(motorL_s,0);
      
        digitalWrite(motor_R1,LOW);
        digitalWrite(motor_R2,LOW);
        analogWrite(motorR_s,0);
    }

    while(!(dis_l>0&&dis_l<180&&dis_r>0&&dis_l<180))
        {
          dis_l=ceju(1);
          dis_r=ceju(2);
        }
        
    if(((dis_l+dis_r)/2)>(dis_paper+2))
    {
      Serial.print("dis_qianjin:");
      Serial.print((dis_l+dis_r)/2);
        digitalWrite(motor_L1,HIGH);
        digitalWrite(motor_L2,LOW);
        analogWrite(motorL_s,speed_motor);
      
        digitalWrite(motor_R1,HIGH);
        digitalWrite(motor_R2,LOW);
        analogWrite(motorR_s,speed_motor);
    }
    else if(((dis_l+dis_r)/2)<dis_paper)
    {
      Serial.print("dis_houtui:");
      Serial.print((dis_l+dis_r)/2);
        digitalWrite(motor_L1,LOW);
        digitalWrite(motor_L2,HIGH);
        analogWrite(motorL_s,speed_motor);
      
        digitalWrite(motor_R1,LOW);
        digitalWrite(motor_R2,HIGH);
        analogWrite(motorR_s,speed_motor);
    }
    else
    {
      Serial.print("dis:");
      Serial.print((dis_l+dis_r)/2);
        digitalWrite(motor_L1,LOW);
        digitalWrite(motor_L2,LOW);
        analogWrite(motorL_s,0);
      
        digitalWrite(motor_R1,LOW);
        digitalWrite(motor_R2,LOW);
        analogWrite(motorR_s,0);
    }
  }
  Serial.println();
  delay(200);

}