arduino跟随小车

 

 

int PrintOk=1;//打印消息

// 红外状态信号
int pin_red_right = 8;
int pin_red_center = 9;
int pin_red_left = 10;

// 电机驱动引脚
int pin_Motor_RightA = 4;
int pin_Motor_RightB = 5;
int pin_Motor_LeftA = 6;
int pin_Motor_LeftB = 7;

int GoTime=10;//前进基本单位 ms
int GoTimeLR=10;
//停止
void Move_Stop(int timedo=1){
  if(PrintOk)Serial.println("停止");
  digitalWrite(pin_Motor_RightA,0);
  digitalWrite(pin_Motor_RightB,0);
  digitalWrite(pin_Motor_LeftA,0);
  digitalWrite(pin_Motor_LeftB,0);
  delay(timedo);
  
}
//前进
void Move_Qian(int timedo=1){
  if(PrintOk)Serial.println("前进");
  digitalWrite(pin_Motor_RightA,0);
  digitalWrite(pin_Motor_RightB,1);
  digitalWrite(pin_Motor_LeftA,0);
  digitalWrite(pin_Motor_LeftB,1);
  delay(timedo);
}
//后退
void Move_Back(int timedo=1){
  if(PrintOk)Serial.println("后退");
  digitalWrite(pin_Motor_RightA,1);
  digitalWrite(pin_Motor_RightB,0);
  digitalWrite(pin_Motor_LeftA,1);
  digitalWrite(pin_Motor_LeftB,0);
  delay(timedo);
}
//左转
void Move_Left(int timedo=1){
  if(PrintOk)Serial.println("左转");
  digitalWrite(pin_Motor_RightA,0);
  digitalWrite(pin_Motor_RightB,1);
  digitalWrite(pin_Motor_LeftA,1);
  digitalWrite(pin_Motor_LeftB,0);
  delay(timedo);
}
//右转
void Move_Right(int timedo=1){
  if(PrintOk)Serial.println("右转");
  digitalWrite(pin_Motor_RightA,1);
  digitalWrite(pin_Motor_RightB,0);
  digitalWrite(pin_Motor_LeftA,0);
  digitalWrite(pin_Motor_LeftB,1);
  delay(timedo);
}





void setup() {
  
  Serial.begin(9600);
  
  pinMode(pin_red_right, INPUT);
  pinMode(pin_red_center, INPUT);  
  pinMode(pin_red_left, INPUT);

  pinMode(pin_Motor_RightA, OUTPUT);
  pinMode(pin_Motor_RightB, OUTPUT);
  pinMode(pin_Motor_LeftA, OUTPUT);
  pinMode(pin_Motor_LeftB, OUTPUT);
  
  Move_Stop(1000);
  //Move_Back(1000);
  //Move_Qian(1000);
  //Move_Left(1000);
  //Move_Right(1000);

}

void loop() {

  //读取信号 无障碍物1 有0
  int State_Left   = digitalRead(pin_red_left);
  int State_Center = digitalRead(pin_red_center);
  int State_Right  = digitalRead(pin_red_right);
  String StateMsg=String("")+State_Left+" "+State_Center+" "+State_Right;
  Serial.println(StateMsg);

  if(State_Left==1 && State_Center==1 && State_Right==1)
  {
     Serial.println("没有任何物体，前进");
     Move_Qian(GoTime);
  }
  else if(State_Left==1 && State_Center==0 && State_Right==1)
  {
     Serial.println("正前有物体，停止");
     Move_Stop(GoTime);
  }

  else if(State_Left==0  && State_Right==1)
  {
     Serial.println("左前有物体，左转");
     Move_Left(GoTimeLR);
  }
   else if(State_Left==1  && State_Right==0)
  {
     Serial.println("右前有物体，右转");
     Move_Right(GoTimeLR);
  }
  else{ 
     Move_Stop(GoTime);
     Serial.println("其他情况，停止");
    }
  
  //delay(1000);  

}